Skip to content

Commit b61857a

Browse files
micah-huthmicah-huth
committed
Merge branch 'Sam-dev-3' of https://github.com/petercorke/robotics-toolbox-python into Sam-dev-3
2 parents ff34211 + a9a17e3 commit b61857a

File tree

6 files changed

+136
-320
lines changed

6 files changed

+136
-320
lines changed

‎graphics/__init__.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
from graphics.graphics_canvas import GraphicsCanvas2D, GraphicsCanvas3D, draw_reference_frame_axes
33
# from graphics.graphics_grid import GraphicsGrid # Deprecated due to GraphicsCanvas
44
from graphics.graphics_robot import GraphicalRobot, RotationalJoint, PrismaticJoint, StaticJoint, Gripper
5-
from graphics.graphics_object2d import Object2D
5+
from graphics.graphics_object2d import Marker2D, STL2D
66
from graphics.graphics_stl import import_object_from_numpy_stl
77
# from graphics.graphics_text import # None needed as of yet
88
# from graphics.model_puma560 import import_puma_560 # Deprecated, file moved

‎graphics/graphics_canvas.py

Lines changed: 0 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1123,16 +1123,6 @@ def __get_colour_from_string(self, colour_string):
11231123
# Return the RGB list (black if not in dictionary)
11241124
return self.__colour_dictionary.get(colour_string, color.black.value)
11251125

1126-
# MAY NOT BE REQUIRED
1127-
# def add_object(self, obj):
1128-
# """
1129-
# Add an object into the scene.
1130-
#
1131-
# :param obj: The object to be added
1132-
# :type obj: class:`graphics.object2d`
1133-
# """
1134-
# pass
1135-
11361126

11371127
def convert_grid_to_z_up(scene):
11381128
"""

‎graphics/graphics_grid.py

Lines changed: 2 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
from vpython import vector, compound, mag, box
22
from numpy import sign, ceil
33
from graphics.graphics_text import update_grid_numbers
4-
from graphics.graphics_object2d import Object2D
4+
from graphics.graphics_object2d import Marker2D
55
from spatialmath import SE2
66

77

@@ -310,33 +310,6 @@ def set_relative(self, is_relative):
310310
"""
311311
self.__relative_cam = is_relative
312312
self.update_grid()
313-
314-
# def clear_scene(self):
315-
# """
316-
# Clear the canvas of all objects (keeping the grid)
317-
#
318-
# Due to how VPython operates, there is no 'deletion' of objects directly.
319-
# To 'delete' objects, first they must be rendered invisible.
320-
#
321-
# Then: if a new object with the same variable name is used, the previous memory will be freed.
322-
# Or: del variable_name will free its memory.
323-
# If the object wasn't invisible, it would remain visible in the scene.
324-
#
325-
# Since the scene doesnt track variable names, the best way to clear the scene is to render all objects invisible,
326-
# and have the user assume they are all deleted. However, all objects can be redisplayed by setting the visibility
327-
# """
328-
# # Save current grid visibility
329-
# grid_visibility = self.grid_object[self.__planes_idx][self.__xy_plane_idx].visible
330-
#
331-
# # Set all objects invisible
332-
# for scene_object in self.__scene.objects:
333-
# scene_object.visible = False
334-
#
335-
# # Set grid visibility to previous
336-
# self.set_visibility(grid_visibility)
337-
#
338-
# # Wait for scene to update
339-
# self.__scene.waitfor("draw_complete")
340313

341314

342315
def create_line(pos1, pos2, scene, colour=None, thickness=0.01):
@@ -496,7 +469,7 @@ def create_marker(scene, x, y, shape, colour=None):
496469
obj_se2 = SE2(x=x, y=y, theta=0)
497470

498471
# Create the object and return it
499-
return Object2D(obj_se2, scene, shape, colour)
472+
return Marker2D(obj_se2, scene, shape, colour)
500473

501474

502475
def vectors_approx_equal(v1, v2):

‎graphics/graphics_object2d.py

Lines changed: 133 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,6 @@
11
from vpython import shapes, radians, extrusion, vector
2+
from graphics.graphics_stl import import_object_from_numpy_stl
3+
24

35
class Object2D:
46
"""
@@ -15,21 +17,114 @@ class Object2D:
1517
:type colour: `list`
1618
:raises ValueError: The shape must be in the list of possible shapes
1719
"""
18-
19-
# TODO
20-
# Have options to give stl file, shape, or arrow
21-
# Have option to set size, colour, etc
22-
# Have funcs that update pose, texture/colours, visibility
23-
2420
def __init__(self, se2, scene, shape, colour):
2521
# Save inputs
26-
self.__se2 = se2
27-
self.__scene = scene
28-
self.__shape = shape
22+
self.se2 = se2
23+
self.scene = scene
24+
self.shape = shape
25+
self.size = 0
2926
if colour[0] > 1.0 or colour[1] > 1.0 or colour[2] > 1.0 or \
3027
colour[0] < 0.0 or colour[1] < 0.0 or colour[2] < 0.0:
3128
raise ValueError("RGB values must be normalised between 0 and 1")
3229
self.__colour = vector(colour[0], colour[1], colour[2])
30+
self.graphic = None
31+
32+
def __create_object(self):
33+
"""
34+
To be overridden by child classes
35+
"""
36+
pass
37+
38+
def update_pose(self, new_se2):
39+
"""
40+
Update the pose of the object
41+
42+
:param new_se2: The SE2 representation of the pose
43+
:type new_se2: class:`spatialmath.se2`
44+
"""
45+
self.se2 = new_se2
46+
x = self.se2.t[0]
47+
y = self.se2.t[1]
48+
t = self.se2.theta
49+
self.graphic.pos = vector(x, y, 0)
50+
self.graphic.axis = vector(0, 1, 0).rotate(t)
51+
52+
def update_colour(self, colour):
53+
"""
54+
Update the colour of the object
55+
56+
:param colour: The RGB colour
57+
:type colour: `list`
58+
"""
59+
if colour[0] > 1.0 or colour[1] > 1.0 or colour[2] > 1.0 or \
60+
colour[0] < 0.0 or colour[1] < 0.0 or colour[2] < 0.0:
61+
raise ValueError("RGB values must be normalised between 0 and 1")
62+
self.graphic.color = vector(colour[0], colour[1], colour[2])
63+
64+
def update_visibility(self, is_visible):
65+
"""
66+
Update the visibility of the object
67+
68+
:param is_visible: Whether to draw or not
69+
:type is_visible: `bool`
70+
"""
71+
self.graphic.visible = is_visible
72+
73+
def update_size(self, multiply):
74+
"""
75+
Update the size of the object by a multiple of the original size.
76+
77+
:param multiply: A number to multiply the original size by
78+
:type multiply: `float`, `int`
79+
"""
80+
self.graphic.size = self.size * multiply
81+
82+
83+
class STL2D(Object2D):
84+
"""
85+
This object is for 2D objects that contain an STL.
86+
87+
:param se2: The SE2 object representing position and orientation
88+
:type se2: class:`spatialmath.se2`
89+
:param scene: The scene in which to add the link
90+
:type scene: class:`vpython.canvas`
91+
:param stl_path: The file path to the STL to apply
92+
:type stl_path: `str`
93+
:param colour: The colour of the shape
94+
:type colour: `list`
95+
"""
96+
def __init__(self, se2, scene, stl_path, colour):
97+
super().__init__(se2, scene, stl_path, colour)
98+
self.graphic = self.__create_object()
99+
self.size = self.graphic.size
100+
self.graphic.color = self.__colour
101+
102+
def __create_object(self):
103+
"""
104+
Return a compound of the loaded STL
105+
106+
:return: A compound object of the triangles in the STL
107+
:rtype: class:`vpython.compound`
108+
"""
109+
return import_object_from_numpy_stl(self.shape, self.scene)
110+
111+
112+
class Marker2D(Object2D):
113+
"""
114+
This class will place a marker in the given location based on the given marker inputs
115+
116+
:param se2: The SE2 object representing position and orientation
117+
:type se2: class:`spatialmath.se2`
118+
:param scene: The scene in which to add the link
119+
:type scene: class:`vpython.canvas`
120+
:param shape: The shape of the object
121+
:type shape: `str`
122+
:param colour: The colour of the shape
123+
:type colour: `list`
124+
:raises ValueError: The shape must be in the list of possible shapes
125+
"""
126+
def __init__(self, se2, scene, shape, colour):
127+
super().__init__(se2, scene, shape, colour)
33128

34129
self.__marker_size = 0.2
35130

@@ -54,57 +149,57 @@ def __init__(self, se2, scene, shape, colour):
54149
raise ValueError("The shape must be in the list of possible shapes")
55150

56151
# Create the object
57-
self.__graphic = self.__create_object()
152+
self.graphic = self.__create_object()
58153

59154
def __create_object(self):
60155
"""
61-
Create the physical graphical object
156+
Create the physical graphical object
62157
63-
:returns: The graphical entity
64-
:rtype: class:`vpython.baseobj`
65-
"""
66-
if self.__shape == '':
158+
:returns: The graphical entity
159+
:rtype: class:`vpython.baseobj`
160+
"""
161+
if self.shape == '':
67162
# 2D coords of the circle boundary
68-
shape_path = shapes.circle(radius=self.__marker_size/2)
69-
elif self.__shape == '+':
163+
shape_path = shapes.circle(radius=self.__marker_size / 2)
164+
elif self.shape == '+':
70165
# 2D coords of the cross boundary
71-
shape_path = shapes.cross(width=self.__marker_size, thickness=self.__marker_size/5)
72-
elif self.__shape == 'o':
166+
shape_path = shapes.cross(width=self.__marker_size, thickness=self.__marker_size / 5)
167+
elif self.shape == 'o':
73168
# 2D coords of the circle boundary
74-
shape_path = shapes.circle(radius=self.__marker_size/2)
75-
elif self.__shape == '*':
169+
shape_path = shapes.circle(radius=self.__marker_size / 2)
170+
elif self.shape == '*':
76171
# 2D coords of the star boundary
77-
shape_path = shapes.star(radius=self.__marker_size/2, n=6)
78-
elif self.__shape == '.':
172+
shape_path = shapes.star(radius=self.__marker_size / 2, n=6)
173+
elif self.shape == '.':
79174
# 2D coords of the square boundary
80175
shape_path = shapes.rectangle(width=self.__marker_size, height=self.__marker_size)
81-
elif self.__shape == 'x':
176+
elif self.shape == 'x':
82177
# 2D coords of the cross boundary
83-
shape_path = shapes.cross(width=self.__marker_size, thickness=self.__marker_size/5, rotate=radians(45))
84-
elif self.__shape == 's':
178+
shape_path = shapes.cross(width=self.__marker_size, thickness=self.__marker_size / 5, rotate=radians(45))
179+
elif self.shape == 's':
85180
# 2D coords of the square boundary
86181
shape_path = shapes.rectangle(width=self.__marker_size, height=self.__marker_size,
87182
thickness=self.__marker_size)
88-
elif self.__shape == 'd':
183+
elif self.shape == 'd':
89184
# 2D coords of the diamond boundary
90185
shape_path = shapes.rectangle(width=self.__marker_size, height=self.__marker_size,
91186
thickness=self.__marker_size, rotate=radians(45))
92-
elif self.__shape == '^':
187+
elif self.shape == '^':
93188
# 2D coords of the triangle boundary
94189
shape_path = shapes.triangle(length=self.__marker_size)
95-
elif self.__shape == 'v':
190+
elif self.shape == 'v':
96191
# 2D coords of the triangle boundary
97192
shape_path = shapes.triangle(length=self.__marker_size, rotate=radians(180))
98-
elif self.__shape == '<':
193+
elif self.shape == '<':
99194
# 2D coords of the triangle boundary
100195
shape_path = shapes.triangle(length=self.__marker_size, rotate=radians(90))
101-
elif self.__shape == '>':
196+
elif self.shape == '>':
102197
# 2D coords of the triangle boundary
103198
shape_path = shapes.triangle(length=self.__marker_size, rotate=radians(-90))
104-
elif self.__shape == 'p':
199+
elif self.shape == 'p':
105200
# 2D coords of the pentagon boundary
106201
shape_path = shapes.pentagon(length=self.__marker_size)
107-
elif self.__shape == 'h':
202+
elif self.shape == 'h':
108203
# 2D coords of the hexagon boundary
109204
shape_path = shapes.hexagon(length=self.__marker_size)
110205
# CURRENTLY UNUSED
@@ -118,13 +213,14 @@ def __create_object(self):
118213
raise ValueError("Invalid shape given")
119214

120215
# Create the shape
121-
x = self.__se2.t[0]
122-
y = self.__se2.t[1]
123-
obj = extrusion(scene=self.__scene,
216+
x = self.se2.t[0]
217+
y = self.se2.t[1]
218+
obj = extrusion(scene=self.scene,
124219
path=[vector(x, y, 0.001), vector(x, y, -0.001)],
125220
shape=shape_path,
126221
color=self.__colour,
127222
shininess=0)
128-
if self.__shape == '':
223+
self.size = obj.size
224+
if self.shape == '':
129225
obj.visible = False
130226
return obj

0 commit comments

Comments
 (0)