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

Commit a1190d0

Browse files
micah-huthmicah-huth
authored
Merge pull request petercorke#43 from petercorke/micah-dev
Importing STL objects and variable indexing in GraphicsGrid for readability.
2 parents b628397 + d1b765c commit a1190d0

File tree

3 files changed

+95
-24
lines changed

3 files changed

+95
-24
lines changed

‎graphics/graphics_grid.py

Lines changed: 26 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -9,13 +9,23 @@ def __init__(self):
99
# Save the current camera settings
1010
self.camera_pos = scene.camera.pos
1111
self.camera_axes = scene.camera.axis
12+
13+
# Private parameters for indexing in grid_object
14+
self.__xy_plane_idx = 0
15+
self.__xz_plane_idx = 1
16+
self.__yz_plane_idx = 2
17+
self.__planes_idx = 0
18+
self.__labels_idx = 1
19+
1220
# Initialise a grid object
1321
# grid_object[0] will always be the 3 plane graphics. [XY, XZ, YZ] (alphabetical in order and connection)
1422
# grid_object[1] will always be the labels. There is always a certain number of indices.
1523
# Order is [x-plane numbers, "X", y-plane numbers, "Y", z-plane numbers, "Z"]
1624
self.grid_object = [[], []]
1725
self.__init_grid()
1826

27+
28+
1929
def __init_grid(self):
2030
"""
2131
Initialise the grid along the x, y, z axes.
@@ -25,10 +35,10 @@ def __init_grid(self):
2535
relative_cam = True # Whether the grid follows the camera rotation and movement
2636

2737
the_grid = self.__create_grid_objects(relative_cam, num_squares)
28-
self.grid_object[0] = the_grid
38+
self.grid_object[self.__planes_idx] = the_grid
2939

3040
# Update the labels instead of recreating them
31-
update_grid_numbers(self.grid_object[1], relative_cam, num_squares)
41+
update_grid_numbers(self.grid_object[self.__labels_idx], relative_cam, num_squares)
3242

3343
def __create_grid_objects(self, bool_camera_relative, num_squares):
3444
"""
@@ -124,7 +134,10 @@ def __create_grid_objects(self, bool_camera_relative, num_squares):
124134
yz_plane = compound(yz_lines)
125135

126136
# Combine all into one list
127-
grid = [xy_plane, xz_plane, yz_plane]
137+
grid = [None, None, None]
138+
grid[self.__xy_plane_idx] = xy_plane
139+
grid[self.__xz_plane_idx] = xz_plane
140+
grid[self.__yz_plane_idx] = yz_plane
128141

129142
return grid
130143

@@ -174,21 +187,21 @@ def __move_grid_objects(self, bool_camera_relative, num_squares):
174187

175188
# XY Plane
176189
if camera_axes.z < 0:
177-
self.grid_object[0][0].pos = vector(x_middle, y_middle, min_z_coord)
190+
self.grid_object[self.__planes_idx][self.__xy_plane_idx].pos = vector(x_middle, y_middle, min_z_coord)
178191
else:
179-
self.grid_object[0][0].pos = vector(x_middle, y_middle, max_z_coord)
192+
self.grid_object[self.__planes_idx][self.__xy_plane_idx].pos = vector(x_middle, y_middle, max_z_coord)
180193

181194
# XZ Plane
182195
if camera_axes.y < 0:
183-
self.grid_object[0][1].pos = vector(x_middle, min_y_coord, z_middle)
196+
self.grid_object[self.__planes_idx][self.__xz_plane_idx].pos = vector(x_middle, min_y_coord, z_middle)
184197
else:
185-
self.grid_object[0][1].pos = vector(x_middle, max_y_coord, z_middle)
198+
self.grid_object[self.__planes_idx][self.__xz_plane_idx].pos = vector(x_middle, max_y_coord, z_middle)
186199

187200
# YZ Plane
188201
if camera_axes.x < 0:
189-
self.grid_object[0][2].pos = vector(min_x_coord, y_middle, z_middle)
202+
self.grid_object[self.__planes_idx][self.__yz_plane_idx].pos = vector(min_x_coord, y_middle, z_middle)
190203
else:
191-
self.grid_object[0][2].pos = vector(max_x_coord, y_middle, z_middle)
204+
self.grid_object[self.__planes_idx][self.__yz_plane_idx].pos = vector(max_x_coord, y_middle, z_middle)
192205

193206
def update_grid(self):
194207
"""
@@ -212,7 +225,7 @@ def update_grid(self):
212225
num_squares = 10 # Length of the grid in each direction (in units)
213226
relative_cam = True # Whether the grid follows the camera rotation and movement
214227
self.__move_grid_objects(relative_cam, num_squares)
215-
update_grid_numbers(self.grid_object[1], relative_cam, num_squares)
228+
update_grid_numbers(self.grid_object[self.__labels_idx], relative_cam, num_squares)
216229

217230
# Else save current grid
218231
else:
@@ -226,9 +239,9 @@ def set_visibility(self, is_visible):
226239
:param is_visible: Boolean of whether to display the grid
227240
:type is_visible: bool
228241
"""
229-
for plane in self.grid_object[0]:
242+
for plane in self.grid_object[self.__planes_idx]:
230243
plane.visible = is_visible
231-
for number in self.grid_object[1]:
244+
for number in self.grid_object[self.__labels_idx]:
232245
number.visible = is_visible
233246

234247
def clear_scene(self):
@@ -246,7 +259,7 @@ def clear_scene(self):
246259
and have the user assume they are all deleted. However, all objects can be redisplayed by setting the visibility
247260
"""
248261
# Save current grid visibility
249-
grid_visibility = self.grid_object[0][0].visible
262+
grid_visibility = self.grid_object[self.__planes_idx][self.__xy_plane_idx].visible
250263

251264
# Set all objects invisible
252265
for scene_object in scene.objects:

‎graphics/graphics_stl.py

Lines changed: 62 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,8 @@
11
from vpython import *
22
from graphics.common_functions import *
3+
from stl import mesh
34

45

5-
# TODO maybe change input to include extension? Or have entire path as input?
66
def import_object_from_stl(filename):
77
"""
88
Import an stl object and convert it into a usable vpython object.
@@ -14,10 +14,11 @@ def import_object_from_stl(filename):
1414
:type filename: str
1515
:return: Compound object of a collection of triangles formed from an stl file.
1616
:rtype: class:`vpython.compound`
17-
"""
18-
# TODO: put error handling in case binary stl file used instead of ascii
1917
20-
# TODO: put error handling in case of bad file
18+
.. deprecated::
19+
A new function using numpy import is available. It accepts both ASCII and BINARY formats.
20+
"""
21+
raise DeprecationWarning("This function is outdated. Use import_object_from_numpy_stl")
2122

2223
# Open the file
2324
filepath = './graphics/models/' + filename + '.stl'
@@ -63,6 +64,63 @@ def import_object_from_stl(filename):
6364
return compound(triangles)
6465

6566

67+
def import_object_from_numpy_stl(filename):
68+
"""
69+
Import either an ASCII or BINARY file format of an STL file.
70+
The triangles will be combined into a single compound entity.
71+
72+
:param filename: Path of the stl file to import.
73+
:type filename: str
74+
:return: Compound object of a collection of triangles formed from an stl file.
75+
:rtype: class:`vpython.compound`
76+
"""
77+
# Load the mesh using NumPy-STL
78+
the_mesh = mesh.Mesh.from_file(filename)
79+
80+
num_faces = len(the_mesh.vectors)
81+
triangles = []
82+
83+
# For every face in the model
84+
for face in range(0, num_faces):
85+
# Get the (3) 3D points
86+
point0 = the_mesh.vectors[face][0]
87+
point1 = the_mesh.vectors[face][1]
88+
point2 = the_mesh.vectors[face][2]
89+
90+
# Get the normal direction for the face
91+
normal0 = the_mesh.normals[face][0]
92+
normal1 = the_mesh.normals[face][1]
93+
normal2 = the_mesh.normals[face][2]
94+
normal = vec(normal0, normal1, normal2)
95+
96+
# Create the VPython 3D points
97+
vertex0 = vertex(
98+
pos=vec(point0[0], point0[1], point0[2]),
99+
normal=normal,
100+
color=color.white
101+
)
102+
vertex1 = vertex(
103+
pos=vec(point1[0], point1[1], point1[2]),
104+
normal=normal,
105+
color=color.white
106+
)
107+
vertex2 = vertex(
108+
pos=vec(point2[0], point2[1], point2[2]),
109+
normal=normal,
110+
color=color.white
111+
)
112+
113+
# Combine them in a list
114+
vertices = [vertex0, vertex1, vertex2]
115+
116+
# Create a triangle using the points, and add it to the list
117+
triangles.append(triangle(vs=vertices))
118+
119+
# Return a compound of the triangles
120+
visual_mesh = compound(triangles)
121+
return visual_mesh
122+
123+
66124
def set_stl_origin(stl_obj, current_obj_origin, required_obj_origin):
67125
"""
68126
Move the object so the required origin is at (0, 0, 0). Then set the origin for the generated stl object.

‎graphics/model_puma560.py

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ def create_link_0():
3131
:rtype: class:`graphics.graphics_robot.RotationalJoint
3232
"""
3333
# Load the STL file into an object
34-
stl_obj = import_object_from_stl(filename='link0')
34+
stl_obj = import_object_from_numpy_stl('./graphics/models/link0.stl')
3535
# Orient the object so that it's origin and toolpoint in known locations
3636
# This way, rotations are relative to the correct 3D position of the object
3737
stl_obj_z_origin = stl_obj.pos.z - stl_obj.width / 2
@@ -60,7 +60,7 @@ def create_link_1():
6060
:rtype: class:`graphics.graphics_robot.StaticJoint
6161
"""
6262
# Load the STL file into an object
63-
stl_obj = import_object_from_stl(filename='link1')
63+
stl_obj = import_object_from_numpy_stl('./graphics/models/link1.stl')
6464
# Orient the object so that it's origin and toolpoint in known locations
6565
# This way, rotations are relative to the correct 3D position of the object
6666
stl_obj.rotate(angle=radians(90), axis=y_axis_vector, origin=vector(0, 0, 0))
@@ -92,7 +92,7 @@ def create_link_2():
9292
:rtype: class:`graphics.graphics_robot.RotationalJoint
9393
"""
9494
# Load the STL file into an object
95-
stl_obj = import_object_from_stl('link2')
95+
stl_obj = import_object_from_numpy_stl('./graphics/models/link2.stl')
9696
# Orient the object so that it's origin and toolpoint in known locations
9797
# This way, rotations are relative to the correct 3D position of the object
9898
stl_obj.rotate(angle=radians(-90), axis=x_axis_vector, origin=vector(0, 0, 0))
@@ -124,7 +124,7 @@ def create_link_3():
124124
:rtype: class:`graphics.graphics_robot.RotationalJoint
125125
"""
126126
# Load the STL file into an object
127-
stl_obj = import_object_from_stl('link3')
127+
stl_obj = import_object_from_numpy_stl('./graphics/models/link3.stl')
128128
# Orient the object so that it's origin and toolpoint in known locations
129129
# This way, rotations are relative to the correct 3D position of the object
130130
stl_obj.rotate(angle=radians(90), axis=y_axis_vector, origin=vector(0, 0, 0))
@@ -156,7 +156,7 @@ def create_link_4():
156156
:rtype: class:`graphics.graphics_robot.RotationalJoint
157157
"""
158158
# Load the STL file into an object
159-
stl_obj = import_object_from_stl('link4')
159+
stl_obj = import_object_from_numpy_stl('./graphics/models/link4.stl')
160160
# Orient the object so that it's origin and toolpoint in known locations
161161
# This way, rotations are relative to the correct 3D position of the object
162162
stl_obj.rotate(angle=radians(-90), axis=z_axis_vector, origin=vector(0, 0, 0))
@@ -187,7 +187,7 @@ def create_link_5():
187187
:rtype: class:`graphics.graphics_robot.RotationalJoint
188188
"""
189189
# Load the STL file into an object
190-
stl_obj = import_object_from_stl('link5')
190+
stl_obj = import_object_from_numpy_stl('./graphics/models/link5.stl')
191191
# Orient the object so that it's origin and toolpoint in known locations
192192
# This way, rotations are relative to the correct 3D position of the object
193193
stl_obj.rotate(angle=radians(90), axis=x_axis_vector, origin=vector(0, 0, 0))
@@ -218,7 +218,7 @@ def create_link_6():
218218
:rtype: class:`graphics.graphics_robot.Gripper
219219
"""
220220
# Load the STL file into an object
221-
stl_obj = import_object_from_stl('link6')
221+
stl_obj = import_object_from_numpy_stl('./graphics/models/link6.stl')
222222
# Orient the object so that it's origin and toolpoint in known locations
223223
# This way, rotations are relative to the correct 3D position of the object
224224
stl_obj.rotate(angle=radians(90), axis=y_axis_vector, origin=vector(0, 0, 0))

0 commit comments

Comments
 (0)
0