Apply origin SE3 to STL model creation
@author Micah Huth
"""
-
from vpython import vec, vertex, color, triangle, compound
from roboticstoolbox.backend.VPython.common_functions import *
from stl import mesh
@@ -14,8 +13,8 @@ def import_object_from_numpy_stl(meshdata, scene):
Import either an ASCII or BINARY file format of an STL file.
The triangles will be combined into a single compound entity.
- :param meshdata: Path of the stl file to import.
- :type meshdata: `str`
+ :param meshdata: Mesh data: path str, scales, origin SE3
+ :type meshdata: `list`
:param scene: The scene in which to draw the object
:type scene: class:`vpython.canvas`
:return: Compound object of a collection of triangles formed from an stl file.
@@ -23,6 +22,11 @@ def import_object_from_numpy_stl(meshdata, scene):
"""
filename = meshdata[0]
scale = meshdata[1]
+ origin = meshdata[2]
+ origin_pos = get_pose_pos(origin)
+ origin_x = get_pose_x_vec(origin)
+ origin_y = get_pose_y_vec(origin)
+ origin_z = get_pose_z_vec(origin)
# Load the mesh using NumPy-STL
the_mesh = mesh.Mesh.from_file(filename)
@@ -67,7 +71,13 @@ def import_object_from_numpy_stl(meshdata, scene):
triangles.append(triangle(canvas=scene, vs=vertices))
# Return a compound of the triangles
- visual_mesh = compound(triangles, origin=vector(0, 0, 0), canvas=scene)
+ visual_mesh = compound(
+ triangles,
+ origin=origin_pos,
+ canvas=scene,
+ axis=origin_x,
+ up=origin_y
+ )
visual_mesh.length *= scale[0]
visual_mesh.height *= scale[1]
visual_mesh.width *= scale[2]
