8000 Merge pull request #39 from petercorke/micah-dev · navrobot/robotics-toolbox-python@b628397 · GitHub
[go: up one dir, main page]
Skip to content

Commit b628397

Browse files
micah-huthmicah-huth
authored
Merge pull request petercorke#39 from petercorke/micah-dev
Micah dev
2 parents 4530961 + 391e1bf commit b628397

File tree

7 files changed

+217
-49
lines changed

7 files changed

+217
-49
lines changed

‎graphics/README.md

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ Instructions on how to use the graphical section of the toolbox below.
1111
* Grid Updating
1212
* On rotation/move finish
1313
* ~~Don't redraw labels, move/update them~~
14-
* Don't redraw the grid, move/update them
14+
* ~~Don't redraw the grid, move/update them~~
1515
* Error handling
1616
* ~~Throw custom error messages~~
1717
* Handle vpython error messages
@@ -39,6 +39,8 @@ The `vector` class is also very crucial to the graphics. It can either represent
3939
For convenience, some functions and variables are provided for easy use. `wrap_to_pi()` takes in an angle, and specification on degrees or radians. It returns the respective angle between -pi and pi.
4040
Three vectors are also supplied for readability to ensure correct axes are used when referencing. `x_axis_vector`, `y_axis_vector`, `z_axis_vector` can be used when supplying the rotation axis, for example.
4141
```python
42+
# Wrap an angle (deg) to the range [-pi pi]. use "rad" instead of "deg" for radian angles.
43+
wrap_to_pi("deg", 450)
4244
# Rotate the joint around its local x-axis by 30 degrees
4345
rot_link.rotate_around_joint_axis(radians(30), x_axis_vector)
4446
```
@@ -70,6 +72,15 @@ canvas_grid.set_visibility(False)
7072
```
7173
Now that the scene is created, a robot must be created to be displayed.
7274

75+
At anytime you can clear the scene of all objects (The grid will remain if visible). Note: This will note delete the objects,
76+
they still exist, and can be rendered visible afterwards. However, overwriting/deleting the variables will free the memory.
77+
If an object is overwritten/deleted while still visible, the objects will remain in the scene.
78+
```python
79+
canvas_grid.clear_scene()
80+
```
81+
82+
83+
7384
## Displaying Robot Joints
7485
If you want to use the example puma560 robot, simply call the creation function that will return a GraphicalRobot object. It will automatically be displayed in the canvas
7586
```python

‎graphics/common_functions.py

Lines changed: 28 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,36 @@
11
from vpython import radians, vector
22

3+
"""
4+
global variables that can be used to easily reference X, Y, and Z axes directions.
5+
"""
36
x_axis_vector = vector(1, 0, 0)
47
y_axis_vector = vector(0, 1, 0)
58
z_axis_vector A93C = vector(0, 0, 1)
69

710

8-
# TODO handle radians (needed more than degrees)
9-
def wrap_to_pi(angle):
10-
angle = angle % radians(360)
11-
if angle > radians(180):
12-
angle -= radians(360)
11+
def wrap_to_pi(angle_type, angle):
12+
"""
13+
Wrap the given angle (deg or rad) to [-pi pi]
14+
15+
:param angle_type: String of whether the angle is deg or rad
16+
:type angle_type: `str`
17+
:param angle: The angle to wrap
18+
:type angle: `float`
19+
:raises ValueError: Throws the error if the given string is not "deg" or "rad"
20+
:return: The wrapped angle
21+
:rtype: `float`
22+
"""
23+
if angle_type == "deg":
24+
angle = angle % 360
25+
if angle > 180:
26+
angle -= 360
27+
28+
elif angle_type == "rad":
29+
angle = angle % radians(360)
30+
if angle > radians(180):
31+
angle -= radians(360)
32+
33+
else:
34+
raise ValueError('angle_type must be "deg" or "rad"')
35+
1336
return angle

‎graphics/graphics_canvas.py

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,6 @@ def init_canvas(height=500, width=1000, title='', caption='', grid=True):
3939
convert_grid_to_z_up()
4040

4141
graphics_grid = GraphicsGrid()
42-
graphics_grid.draw_grid()
4342
if not grid:
4443
graphics_grid.set_visibility(False)
4544

‎graphics/graphics_grid.py

Lines changed: 111 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -10,33 +10,36 @@ def __init__(self):
1010
self.camera_pos = scene.camera.pos
1111
self.camera_axes = scene.camera.axis
1212
# Initialise a grid object
13-
self.grid_object = [None, []]
13+
# grid_object[0] will always be the 3 plane graphics. [XY, XZ, YZ] (alphabetical in order and connection)
14+
# grid_object[1] will always be the labels. There is always a certain number of indices.
15+
# Order is [x-plane numbers, "X", y-plane numbers, "Y", z-plane numbers, "Z"]
16+
self.grid_object = [[], []]
17+
self.__init_grid()
1418

15-
def draw_grid(self):
19+
def __init_grid(self):
1620
"""
17-
Display grids along the x, y, z axes.
18-
21+
Initialise the grid along the x, y, z axes.
1922
"""
20-
23+
# TODO base off user input?
2124
num_squares = 10 # Length of the grid in each direction (in units)
2225
relative_cam = True # Whether the grid follows the camera rotation and movement
2326

24-
the_grid = self.create_grid(relative_cam, num_squares)
27+
the_grid = self.__create_grid_objects(relative_cam, num_squares)
2528
self.grid_object[0] = the_grid
2629

2730
# Update the labels instead of recreating them
28-
create_grid_numbers(self.grid_object[1], relative_cam, num_squares)
31+
update_grid_numbers(self.grid_object[1], relative_cam, num_squares)
2932

30-
def create_grid(self, bool_camera_relative, num_squares):
33+
def __create_grid_objects(self, bool_camera_relative, num_squares):
3134
"""
3235
Draw a grid along each 3D plane, that is closest to the camera.
3336
3437
:param bool_camera_relative: Whether to draw the axes at the camera focus point or at (0, 0, 0).
3538
:type bool_camera_relative: bool
3639
:param num_squares: How many unit squares to draw along the axis.
3740
:type num_squares: int
38-
:return: Vpython compound object of the three drawn axes.
39-
:rtype: class:`vpython.compound`
41+
:return: List of the three drawn axes.
42+
:rtype: list
4043
"""
4144

4245
# Initial conditions
@@ -120,21 +123,77 @@ def create_grid(self, bool_camera_relative, num_squares):
120123
xy_plane = compound(xy_lines)
121124
yz_plane = compound(yz_lines)
122125

123-
# Combine all into one object
124-
grid = compound([xy_plane, xz_plane, yz_plane])
125-
126-
# TODO
127-
# Instead of creating a new grid everytime, reuse old, but rotate. e.g. (+) inc in x-axis pos = (+) rot about z
128-
# Other option is to not do compound, but update pos of each of the planes. (might be easier)
126+
# Combine all into one list
127+
grid = [xy_plane, xz_plane, yz_plane]
129128

130129
return grid
131130

132-
def update_grid(self):
131+
def __move_grid_objects(self, bool_camera_relative, num_squares):
133132
"""
134-
Update the grid axes and numbers if the camera position/rotation has changed.
133+
Reusing the current assets, move the planes to the new origins.
135134
135+
:param bool_camera_relative: Whether to draw the axes at the camera focus point or at (0, 0, 0).
136+
:type bool_camera_relative: bool
137+
:param num_squares: How many unit squares to draw along the axis.
138+
:type num_squares: int
136139
"""
140+
camera_axes = self.camera_axes
141+
# Locate centre of axes
142+
if bool_camera_relative:
143+
x_origin, y_origin, z_origin = round(scene.center.x), round(scene.center.y), round(scene.center.z)
144+
else:
145+
x_origin, y_origin, z_origin = 0, 0, 0
146+
147+
# CAMERA AXES | DISPLAYED GRID | XZ PLANE | XY PLANE | YZ PLANE
148+
# x,y,z | x,y,z | x,z | x,y | y,z
149+
# -------------+-----------------+----------+----------+----------
150+
# -,-,- | +,+,+ | +,+ | +,+ | +,+
151+
# -,-,+ | +,+,- | +,- | +,+ | +,-
152+
# -,+,- | +,-,+ | +,+ | +,- | -,+
153+
# -,+,+ | +,-,- | +,- | +,- | -,-
154+
# +,-,- | -,+,+ | -,+ | -,+ | +,+
155+
# +,-,+ | -,+,- | -,- | -,+ | +,-
156+
# +,+,- | -,-,+ | -,+ | -,- | -,+
157+
# +,+,+ | -,-,- | -,- | -,- | -,-
158+
# min = -num_squares or 0, around the default position
159+
# max = +num_squares or 0, around the default position
160+
# e.g. at the origin, for negative axes: -10 -> 0, positive axes: 0 -> 10
161+
min_x_coord = x_origin + int(-(num_squares / 2) + (sign(camera_axes.x) * -1) * (num_squares / 2))
162+
max_x_coord = x_origin + int((num_squares / 2) + (sign(camera_axes.x) * -1) * (num_squares / 2))
163+
164+
min_y_coord = y_origin + int(-(num_squares / 2) + (sign(camera_axes.y) * -1) * (num_squares / 2))
165+
max_y_coord = y_origin + int((num_squares / 2) + (sign(camera_axes.y) * -1) * (num_squares / 2))
166+
167+
min_z_coord = z_origin + int(-(num_squares / 2) + (sign(camera_axes.z) * -1) * (num_squares / 2))
168+
max_z_coord = z_origin + int((num_squares / 2) + (sign(camera_axes.z) * -1) * (num_squares / 2))
169+
170+
# Compound origins are in the middle of the bounding boxes. Thus new pos will be between max and min.
171+
x_middle = (max_x_coord + min_x_coord) / 2
172+
y_middle = (max_y_coord + min_y_coord) / 2
173+
z_middle = (max_z_coord + min_z_coord) / 2
174+
175+
# XY Plane
176+
if camera_axes.z < 0:
177+
self.grid_object[0][0].pos = vector(x_middle, y_middle, min_z_coord)
178+
else:
179+
self.grid_object[0][0].pos = vector(x_middle, y_middle, max_z_coord)
180+
181+
# XZ Plane
182+
if camera_axes.y < 0:
183+
self.grid_object[0][1].pos = vector(x_middle, min_y_coord, z_middle)
184+
else:
185+
self.grid_object[0][1].pos = vector(x_middle, max_y_coord, z_middle)
186+
187+
# YZ Plane
188+
if camera_axes.x < 0:
189+
self.grid_object[0][2].pos = vector(min_x_coord, y_middle, z_middle)
190+
else:
191+
self.grid_object[0][2].pos = vector(max_x_coord, y_middle, z_middle)
137192

193+
def update_grid(self):
194+
"""
195+
Update the grid axes and numbers if the camera position/rotation has changed.
196+
"""
138197
# Obtain the new camera settings
139198
new_camera_pos = scene.camera.pos
140199
new_camera_axes = scene.camera.axis
@@ -148,12 +207,13 @@ def update_grid(self):
148207
self.camera_pos = new_camera_pos
149208
self.camera_axes = new_camera_axes
150209

151-
# Delete old grid
152-
self.set_visibility(False)
153-
self.grid_object[0] = None
210+
# Update grid
211+
# TODO base off user input?
212+
num_squares = 10 # Length of the grid in each direction (in units)
213+
relative_cam = True # Whether the grid follows the camera rotation and movement
214+
self.__move_grid_objects(relative_cam, num_squares)
215+
update_grid_numbers(self.grid_object[1], relative_cam, num_squares)
154216

155-
# Save new grid
156-
self.draw_grid()
157217
# Else save current grid
158218
else:
159219
# Already current
@@ -166,11 +226,34 @@ def set_visibility(self, is_visible):
166226
:param is_visible: Boolean of whether to display the grid
167227
:type is_visible: bool
168228
"""
169-
# If no grid, turn the objects invisible
170-
self.grid_object[0].visible = is_visible
171-
# TODO this will need to be uncommented once grid_object reuse is done
172-
#for number in self.grid_object[1]:
173-
# number.visible = is_visible
229+
for plane in self.grid_object[0]:
230+
plane.visible = is_visible
231+
for number in self.grid_object[1]:
232+
number.visible = is_visible
233+
234+
def clear_scene(self):
235+
"""
236+
Clear the canvas of all objects (keeping the grid)
237+
238+
Due to how VPython operates, there is no 'deletion' of objects directly.
239+
To 'delete' objects, first they must be rendered invisible.
240+
241+
Then: if a new object with the same variable name is used, the previous memory will be freed.
242+
Or: del variable_name will free its memory.
243+
If the object wasn't invisible, it would remain visible in the scene.
244+
245+
Since the scene doesnt track variable names, the best way to clear the scene is to render all objects invisible,
246+
and have the user assume they are all deleted. However, all objects can be redisplayed by setting the visibility
247+
"""
248+
# Save current grid visibility
249+
grid_visibility = self.grid_object[0][0].visible
250+
251+
# Set all objects invisible
252+
for scene_object in scene.objects:
253+
scene_object.visible = False
254+
255+
# Set grid visibility to previous
256+
self.set_visibility(grid_visibility)
174257

175258

176259
def create_line(pos1, pos2):

‎graphics/graphics_robot.py

Lines changed: 22 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -79,18 +79,19 @@ def rotate_around_joint_axis(self, angle_of_rotation, axis_of_rotation):
7979
:type angle_of_rotation: float (radians)
8080
:param axis_of_rotation: X, Y, or Z axis to apply around the objects specific X, Y, or Z axes
8181
:type axis_of_rotation: class:`vpython.vector`
82+
:raise ValueError: The given axis_of_rotation must be one of the default X, Y, Z vectors (e.g. x_axis_vector)
8283
"""
8384
# Determine the axis of rotation based on the given joint axis direction
8485
# Then add the rotation amount to the axis counter
8586
if axis_of_rotation.equals(x_axis_vector):
8687
rotation_axis = self.__x_vector
87-
self.__x_rotation = wrap_to_pi(self.__x_rotation + angle_of_rotation)
88+
self.__x_rotation = wrap_to_pi("rad", self.__x_rotation + angle_of_rotation)
8889
elif axis_of_rotation.equals(y_axis_vector):
8990
rotation_axis = self.__y_vector
90-
self.__y_rotation = wrap_to_pi(self.__y_rotation + angle_of_rotation)
91+
self.__y_rotation = wrap_to_pi("rad", self.__y_rotation + angle_of_rotation)
9192
elif axis_of_rotation.equals(z_axis_vector):
9293
rotation_axis = self.__z_vector
93-
self.__z_rotation = wrap_to_pi(self.__z_rotation + angle_of_rotation)
94+
self.__z_rotation = wrap_to_pi("rad", self.__z_rotation + angle_of_rotation)
9495
else:
9596
error_str = "Bad input vector given ({0}). Must be either x_axis_vector ({1}), y_axis_vector ({2})," \
9697
"or z_axis_vector ({3}). Use rotate_around_vector for rotation about an arbitrary vector."
@@ -140,11 +141,11 @@ def rotate_around_vector(self, angle_of_rotation, axis_of_rotation):
140141
# axis of rotation will have the smallest (it's less affected by the rotation)
141142
min_angle_diff = min(angle_diff_x, angle_diff_y, angle_diff_z)
142143
if min_angle_diff == angle_diff_x:
143-
self.__x_rotation = wrap_to_pi(self.__x_rotation + angle_of_rotation)
144+
self.__x_rotation = wrap_to_pi("rad", self.__x_rotation + angle_of_rotation)
144145
elif min_angle_diff == angle_diff_y:
145-
self.__y_rotation = wrap_to_pi(self.__y_rotation + angle_of_rotation)
146+
self.__y_rotation = wrap_to_pi("rad", self.__y_rotation + angle_of_rotation)
146147
else:
147-
self.__z_rotation = wrap_to_pi(self.__z_rotation + angle_of_rotation)
148+
self.__z_rotation = wrap_to_pi("rad", self.__z_rotation + angle_of_rotation)
148149

149150
# Calculate the updated toolpoint location
150151
self.__connect_dir.rotate(angle=angle_of_rotation, axis=axis_of_rotation)
@@ -274,6 +275,7 @@ def get_rotation_angle(self, axis):
274275
:type axis: class:`vpython.vector`
275276
:return: Current angle of rotation with respect to world (includes rotation from previous joints)
276277
:rtype: float (radians)
278+
:raise ValueError: The given axis_of_rotation must be one of the default X, Y, Z vectors (e.g. x_axis_vector)
277279
"""
278280
# Ensure copying value not reference
279281
ans = 0.0
@@ -284,7 +286,9 @@ def get_rotation_angle(self, axis):
284286
elif axis.equals(z_axis_vector):
285287
ans += self.__z_rotation
286288
else:
287-
ans += self.__y_rotation
289+
error_str = "Bad input vector given ({0}). Must be either x_axis_vector ({1}), y_axis_vector ({2})," \
290+
"or z_axis_vector ({3})."
291+
raise ValueError(error_str.format(axis), x_axis_vector, y_axis_vector, z_axis_vector)
288292

289293
return ans
290294

@@ -296,6 +300,7 @@ def get_axis_vector(self, axis):
296300
:type axis: class:`vpython.vector`
297301
:return: Current vector representation of the joints X, Y, or Z axis
298302
:rtype: class:`vpython.vector`
303+
:raise ValueError: The given axis_of_rotation must be one of the default X, Y, Z vectors (e.g. x_axis_vector)
299304
"""
300305
# Return new vectors to avoid pass by reference
301306
if axis.equals(x_axis_vector):
@@ -305,7 +310,9 @@ def get_axis_vector(self, axis):
305310
elif axis.equals(z_axis_vector):
306311
return vector(self.__z_vector)
307312
else:
308-
return vector(self.__y_vector)
313+
error_str = "Bad input vector given ({0}). Must be either x_axis_vector ({1}), y_axis_vector ({2})," \
314+
"or z_axis_vector ({3})."
315+
raise ValueError(error_str.format(axis), x_axis_vector, y_axis_vector, z_axis_vector)
309316

310317
def get_joint_type(self):
311318
"""
@@ -353,10 +360,10 @@ def rotate_joint(self, new_angle):
353360
:type new_angle: float (radians)
354361
"""
355362
# Wrap given angle to -pi to pi
356-
new_angle = wrap_to_pi(new_angle)
363+
new_angle = wrap_to_pi("rad", new_angle)
357364
current_angle = self.rotation_angle
358365
# Calculate amount to rotate the link
359-
angle_diff = wrap_to_pi(new_angle - current_angle)
366+
angle_diff = wrap_to_pi("rad", new_angle - current_angle)
360367
# Update the link
361368
self.rotate_around_joint_axis(angle_diff, self.rotation_axis)
362369
self.rotation_angle = new_angle
@@ -459,6 +466,7 @@ class GraphicalRobot:
459466
460467
:param joints: A list of the joints in order from base (0) to gripper (end), or other types.
461468
:type joints: list
469+
:raise ValueError: The given length of joints must not be 0
462470
"""
463471

464472
def __init__(self, joints):
@@ -514,6 +522,8 @@ def set_joint_angle(self, link_num, new_angle):
514522
:type link_num: int
515523
:param new_angle: The required angle to set the arm rotated towards
516524
:type new_angle: float (radians)
525+
:raise IndexError: Link index must be between 0 (inclusive) and number of joints (exclusive)
526+
:raise TypeError: The joint index chosen must be indexing a revolute joint
517527
"""
518528
if (link_num < 0) or (link_num >= self.num_joints):
519529
error_str = "link number given ({0}) is not between range of 0 (inclusive) and {1} (exclusive)"
@@ -538,7 +548,7 @@ def set_joint_angle(self, link_num, new_angle):
538548
self.__position_joints()
539549
else:
540550
error_str = "Given joint {0} is not a revolute joint. It is a {1} joint"
541-
raise ValueError(error_str.format(link_num, self.joints[link_num].get_joint_type()))
551+
raise TypeError(error_str.format(link_num, self.joints[link_num].get_joint_type()))
542552

543553
def set_all_joint_angles(self, new_angles):
544554
"""
@@ -547,6 +557,7 @@ def set_all_joint_angles(self, new_angles):
547557
:param new_angles: List of new angles (radians) to set each joint to.
548558
Must have the same length as number of joints in robot arm, even if the joints aren't revolute
549559
:type new_angles: float list (radians)
560+
:raise IndexError: The length of the given list must equal the number of joints.
550561
"""
551562
# Raise error if lengths don't match
552563
if len(new_angles) != len(self.joints):

0 commit comments

Comments
 (0)
0