def _convert_geom(self, geom):
"""Given an lcmt_viewer_geometry_data, parse it into a tuple of (Shape,
Rgba, RigidTransform)."""
shape = None
if geom.type == lcmt_viewer_geometry_data.BOX:
(width, depth, height) = geom.float_data
shape = Box(width=width, depth=depth, height=height)
elif geom.type == lcmt_viewer_geometry_data.CAPSULE:
(radius, length) = geom.float_data
shape = Capsule(radius=radius, length=length)
elif geom.type == lcmt_viewer_geometry_data.CYLINDER:
(radius, length) = geom.float_data
shape = Cylinder(radius=radius, length=length)
elif geom.type == lcmt_viewer_geometry_data.ELLIPSOID:
(a, b, c) = geom.float_data
shape = Ellipsoid(a=a, b=b, c=c)
elif geom.type == lcmt_viewer_geometry_data.MESH:
(scale_x, scale_y, scale_z) = geom.float_data
filename = geom.string_data
assert scale_x == scale_y and scale_y == scale_z
shape = Mesh(absolute_filename=filename, scale=scale_x)
elif geom.type == lcmt_viewer_geometry_data.SPHERE:
(radius,) = geom.float_data
shape = Sphere(radius=radius)
else:
_logger.warning(f"Unknown geom.type of {geom.type}")
return (None, None, None)
rgba = Rgba(*geom.color)
pose = self._to_pose(geom.position, geom.quaternion)
return (shape, rgba, pose)
def scene_graph_with_mesh(filename):
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
world_body = mbp.world_body()
mesh_shape = Mesh(filename)
mesh_body = mbp.AddRigidBody("mesh", SpatialInertia(
mass=1.0, p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), mesh_body.body_frame(),
RigidTransform())
mbp.RegisterVisualGeometry(
mesh_body, RigidTransform.Identity(), mesh_shape, "mesh_vis",
np.array([0.5, 0.5, 0.5, 1.]))
mbp.Finalize()
return scene_graph
def _convert_geom(self, geom):
"""Given an lcmt_viewer_geometry_data, parse it into a tuple of (Shape,
Rgba, RigidTransform)."""
shape = None
if geom.type == lcmt_viewer_geometry_data.BOX:
(width, depth, height) = geom.float_data
shape = Box(width=width, depth=depth, height=height)
elif geom.type == lcmt_viewer_geometry_data.CAPSULE:
(radius, length) = geom.float_data
shape = Capsule(radius=radius, length=length)
elif geom.type == lcmt_viewer_geometry_data.CYLINDER:
(radius, length) = geom.float_data
shape = Cylinder(radius=radius, length=length)
elif geom.type == lcmt_viewer_geometry_data.ELLIPSOID:
(a, b, c) = geom.float_data
shape = Ellipsoid(a=a, b=b, c=c)
elif geom.type == lcmt_viewer_geometry_data.MESH and geom.string_data:
# A mesh to be loaded from a file.
(scale_x, scale_y, scale_z) = geom.float_data
filename = geom.string_data
assert scale_x == scale_y and scale_y == scale_z
shape = Mesh(absolute_filename=filename, scale=scale_x)
elif geom.type == lcmt_viewer_geometry_data.MESH:
assert not geom.string_data
# A mesh with the data inline, i.e.,
# V | T | v0 | v1 | ... vN | t0 | t1 | ... | tM
# where
# V: The number of vertices.
# T: The number of triangles.
# N: 3V, the number of floating point values for the V vertices.
# M: 3T, the number of vertex indices for the T triangles.
_logger.warning("Meldis cannot yet display hydroelastic collision "
"meshes; that geometry will be ignored.")
elif geom.type == lcmt_viewer_geometry_data.SPHERE:
(radius,) = geom.float_data
shape = Sphere(radius=radius)
else:
_logger.warning(f"Unknown geom.type of {geom.type}")
return (None, None, None)
rgba = Rgba(*geom.color)
pose = self._to_pose(geom.position, geom.quaternion)
return (shape, rgba, pose)