def convert_visuals(urdf, texture_path, vis):
link_name_to_index = {}
link_name_to_index[urdf.base_links[0].link_name] = -1
for link_index in range(len(urdf.links)):
l = urdf.links[link_index]
link_name_to_index[l.link_name] = link_index
b2vis = {}
uid = -1
if texture_path:
material = g.MeshLambertMaterial(
map=g.ImageTexture(
wrap=[0, 0],
repeat=[1, 1],
image=g.PngImage.from_file(texture_path)))
else:
material = g.MeshLambertMaterial(color=0xffffff, reflectivity=0.8)
#first the base link
link_index = -1
b2v, uid = convert_link_visuals(urdf.base_links[0], link_index, material, vis,
uid, b2vis)
#then convert each child link
for joint in urdf.joints:
link_index = link_name_to_index[joint.child_name]
link = urdf.links[link_index]
b2v, uid = convert_link_visuals(link, link_index, material, vis, uid, b2vis)
return b2vis
def runTest(self):
self.vis.delete()
v = self.vis["shapes"]
v.set_transform(tf.translation_matrix([1., 0, 0]))
v["cube"].set_object(g.Box([0.1, 0.2, 0.3]))
v["cube"].set_transform(tf.translation_matrix([0.05, 0.1, 0.15]))
v["cylinder"].set_object(g.Cylinder(0.2, 0.1), g.MeshLambertMaterial(color=0x22dd22))
v["cylinder"].set_transform(tf.translation_matrix([0, 0.5, 0.1]).dot(tf.rotation_matrix(-np.pi / 2, [1, 0, 0])))
v["sphere"].set_object(g.Mesh(g.Sphere(0.15), g.MeshLambertMaterial(color=0xff11dd)))
v["sphere"].set_transform(tf.translation_matrix([0, 1, 0.15]))
v["ellipsoid"].set_object(g.Ellipsoid([0.3, 0.1, 0.1]))
v["ellipsoid"].set_transform(tf.translation_matrix([0, 1.5, 0.1]))
v = self.vis["meshes/valkyrie/head"]
v.set_object(g.Mesh(
g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "data/head_multisense.obj")),
g.MeshLambertMaterial(
map=g.ImageTexture(
image=g.PngImage.from_file(os.path.join(meshcat.viewer_assets_path(), "data/HeadTextureMultisense.png"))
)
)
))
v.set_transform(tf.translation_matrix([0, 0.5, 0.5]))
v = self.vis["points"]
v.set_transform(tf.translation_matrix([-1, 0, 0]))
verts = np.random.rand(3, 100000)
colors = verts
v["random"].set_object(g.PointCloud(verts, colors))
v["random"].set_transform(tf.translation_matrix([-0.5, -0.5, 0]))
def convert_visuals_pb(vis, urdfLinks, urdfJoints, p0, texture_path):
b2vis = {}
uid = -1
print("num_links=", len(urdfLinks))
for link in urdfLinks:
print("num_visuals=", len(link.urdf_visual_shapes))
for v in link.urdf_visual_shapes:
print("v.geom_type=", v.geom_type)
if v.geom_type == p0.GEOM_MESH:
print("mesh filename=", v.geom_meshfilename)
print("geom_meshscale=", v.geom_meshscale)
vis_name = link.link_name + str(uid)
b2v = VisualLinkInfo()
b2v.vis_name = vis_name
b2v.uid = uid
b2v.origin_rpy = v.origin_rpy
b2v.origin_xyz = v.origin_xyz
b2v.inertia_xyz = link.urdf_inertial.origin_xyz
b2v.inertia_rpy = link.urdf_inertial.origin_rpy
vis[vis_name].set_object(
g.ObjMeshGeometry.from_file(v.geom_meshfilename),
g.MeshLambertMaterial(map=g.ImageTexture(
image=g.PngImage.from_file(texture_path))))
v.uid = uid
b2vis[v.uid] = b2v
uid += 1
print("num_joints=", len(urdfJoints))
return b2vis
def test_geometry_material_texture(self):
viewer = MeshcatViewer()
box = Shape.create_box([0.2] * 3, Material())
texture = g.ImageTexture(
image=g.PngImage.from_file(Path(os.path.realpath(__file__)).parent.joinpath('data/texture.png'))
)
box.set_user_data(dict(visual_mesh_texture=texture))
mat = viewer._get_shape_material(box)
self.assertEqual(mat.map, texture)
self.assertAlmostEqual(mat.opacity, 1.)
def runTest(self):
self.vis.delete()
v = self.vis["shapes"]
v.set_transform(tf.translation_matrix([1., 0, 0]))
v["box"].set_object(g.Box([1.0, 0.2, 0.3]))
v["box"].delete()
v["box"].set_object(g.Box([0.1, 0.2, 0.3]))
v["box"].set_transform(tf.translation_matrix([0.05, 0.1, 0.15]))
v["cylinder"].set_object(g.Cylinder(0.2, 0.1), g.MeshLambertMaterial(color=0x22dd22))
v["cylinder"].set_transform(tf.translation_matrix([0, 0.5, 0.1]).dot(tf.rotation_matrix(-np.pi / 2, [1, 0, 0])))
v["sphere"].set_object(g.Mesh(g.Sphere(0.15), g.MeshLambertMaterial(color=0xff11dd)))
v["sphere"].set_transform(tf.translation_matrix([0, 1, 0.15]))
v["ellipsoid"].set_object(g.Ellipsoid([0.3, 0.1, 0.1]))
v["ellipsoid"].set_transform(tf.translation_matrix([0, 1.5, 0.1]))
v["transparent_ellipsoid"].set_object(g.Mesh(
g.Ellipsoid([0.3, 0.1, 0.1]),
g.MeshLambertMaterial(color=0xffffff,
opacity=0.5)))
v["transparent_ellipsoid"].set_transform(tf.translation_matrix([0, 2.0, 0.1]))
v = self.vis["meshes/valkyrie/head"]
v.set_object(g.Mesh(
g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "data/head_multisense.obj")),
g.MeshLambertMaterial(
map=g.ImageTexture(
image=g.PngImage.from_file(os.path.join(meshcat.viewer_assets_path(), "data/HeadTextureMultisense.png"))
)
)
))
v.set_transform(tf.translation_matrix([0, 0.5, 0.5]))
v = self.vis["meshes/convex"]
v["obj"].set_object(g.Mesh(g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.obj"))))
v["stl_ascii"].set_object(g.Mesh(g.StlMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.stl_ascii"))))
v["stl_ascii"].set_transform(tf.translation_matrix([0, -0.5, 0]))
v["stl_binary"].set_object(g.Mesh(g.StlMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.stl_binary"))))
v["stl_binary"].set_transform(tf.translation_matrix([0, -1, 0]))
v["dae"].set_object(g.Mesh(g.DaeMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.dae"))))
v["dae"].set_transform(tf.translation_matrix([0, -1.5, 0]))
v = self.vis["points"]
v.set_transform(tf.translation_matrix([0, 2, 0]))
verts = np.random.rand(3, 1000000)
colors = verts
v["random"].set_object(g.PointCloud(verts, colors))
v["random"].set_transform(tf.translation_matrix([-0.5, -0.5, 0]))
v = self.vis["lines"]
v.set_transform(tf.translation_matrix(([-2, -3, 0])))
vertices = np.random.random((3, 10)).astype(np.float32)
v["line_segments"].set_object(g.LineSegments(g.PointsGeometry(vertices)))
v["line"].set_object(g.Line(g.PointsGeometry(vertices)))
v["line"].set_transform(tf.translation_matrix([0, 1, 0]))
v["line_loop"].set_object(g.LineLoop(g.PointsGeometry(vertices)))
v["line_loop"].set_transform(tf.translation_matrix([0, 2, 0]))
v["line_loop_with_material"].set_object(g.LineLoop(g.PointsGeometry(vertices), g.LineBasicMaterial(color=0xff0000)))
v["line_loop_with_material"].set_transform(tf.translation_matrix([0, 3, 0]))
colors = vertices # Color each line by treating its xyz coordinates as RGB colors
v["line_with_vertex_colors"].set_object(g.Line(g.PointsGeometry(vertices, colors), g.LineBasicMaterial(vertexColors=True)))
v["line_with_vertex_colors"].set_transform(tf.translation_matrix([0, 4, 0]))
v["triad"].set_object(g.LineSegments(
g.PointsGeometry(position=np.array([
[0, 0, 0], [1, 0, 0],
[0, 0, 0], [0, 1, 0],
[0, 0, 0], [0, 0, 1]]).astype(np.float32).T,
color=np.array([
[1, 0, 0], [1, 0.6, 0],
[0, 1, 0], [0.6, 1, 0],
[0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T
),
g.LineBasicMaterial(vertexColors=True)))
v["triad"].set_transform(tf.translation_matrix(([0, 5, 0])))
v["triad_function"].set_object(g.triad(0.5))
v["triad_function"].set_transform(tf.translation_matrix([0, 6, 0]))
def load(self, context=None):
"""
Loads ``meshcat`` visualization elements.
Precondition:
Either the context is a valid Context for this system with the
geometry_query port connected or the ``scene_graph`` passed in the
constructor must be a valid SceneGraph.
"""
if self._delete_prefix_on_load:
self.vis[self.prefix].delete()
if context and self.get_geometry_query_input_port().HasValue(context):
inspector = self.get_geometry_query_input_port().Eval(
context).inspector()
elif self._scene_graph:
inspector = self._scene_graph.model_inspector()
else:
raise RuntimeError(
"You must provide a valid Context for this system with the "
"geometry_query port connected or the ``scene_graph`` passed "
"in the constructor must be a valid SceneGraph.")
vis = self.vis[self.prefix]
# Make a fixed-seed generator for random colors for bodies.
color_generator = np.random.RandomState(seed=42)
for frame_id in inspector.GetAllFrameIds():
count = inspector.NumGeometriesForFrameWithRole(
frame_id, self._role)
if count == 0:
continue
if frame_id == inspector.world_frame_id():
name = "world"
else:
# Note: MBP declares frames with SceneGraph using `::`, we
# replace those with `/` here to expose the full tree to
# meshcat.
name = (inspector.GetOwningSourceName(frame_id) + "/" +
inspector.GetName(frame_id).replace("::", "/"))
frame_vis = vis[name]
for g_id in inspector.GetGeometries(frame_id, self._role):
color = 0xe5e5e5 # default color
alpha = 1.0
hydro_mesh = None
if self._role == Role.kIllustration:
props = inspector.GetIllustrationProperties(g_id)
if props and props.HasProperty("phong", "diffuse"):
rgba = props.GetProperty("phong", "diffuse")
# Convert Rgba from [0-1] to hex 0xRRGGBB.
color = int(255 * rgba.r()) * 256**2
color += int(255 * rgba.g()) * 256
color += int(255 * rgba.b())
alpha = rgba.a()
elif self._role == Role.kProximity:
# Pick a random color to make collision geometry
# visually distinguishable.
color = color_generator.randint(2**(24))
if self._prefer_hydro:
hydro_mesh = inspector. \
maybe_get_hydroelastic_mesh(g_id)
material = g.MeshLambertMaterial(color=color,
transparent=alpha != 1.,
opacity=alpha)
shape = inspector.GetShape(g_id)
X_FG = inspector.GetPoseInFrame(g_id).GetAsMatrix4()
if hydro_mesh is not None:
# We've got a hydroelastic mesh to load.
surface_mesh = hydro_mesh
if isinstance(hydro_mesh, VolumeMesh):
surface_mesh = ConvertVolumeToSurfaceMesh(hydro_mesh)
v_count = len(surface_mesh.triangles()) * 3
vertices = np.empty((v_count, 3), dtype=float)
normals = np.empty((v_count, 3), dtype=float)
mesh_verts = surface_mesh.vertices()
v = 0
for face in surface_mesh.triangles():
p_MA = mesh_verts[int(face.vertex(0))]
p_MB = mesh_verts[int(face.vertex(1))]
p_MC = mesh_verts[int(face.vertex(2))]
vertices[v, :] = tuple(p_MA)
vertices[v + 1, :] = tuple(p_MB)
vertices[v + 2, :] = tuple(p_MC)
p_AB_M = p_MB - p_MA
p_AC_M = p_MC - p_MA
n_M = np.cross(p_AB_M, p_AC_M)
nhat_M = n_M / np.sqrt(n_M.dot(n_M))
normals[v, :] = nhat_M
normals[v + 1, :] = nhat_M
normals[v + 2, :] = nhat_M
v += 3
geom = HydroTriSurface(vertices, normals)
elif isinstance(shape, Box):
geom = g.Box(
[shape.width(),
shape.depth(),
shape.height()])
elif isinstance(shape, Sphere):
geom = g.Sphere(shape.radius())
elif isinstance(shape, Cylinder):
geom = g.Cylinder(shape.length(), shape.radius())
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
R_GC = RotationMatrix.MakeXRotation(np.pi / 2.0).matrix()
X_FG[0:3, 0:3] = X_FG[0:3, 0:3].dot(R_GC)
elif isinstance(shape, (Mesh, Convex)):
geom = g.ObjMeshGeometry.from_file(shape.filename()[0:-3] +
"obj")
# Attempt to find a texture for the object by looking for
# an identically-named *.png next to the model.
# TODO(gizatt): Support .MTLs and prefer them over png,
# since they're both more expressive and more standard.
# TODO(gizatt): In the long term, this kind of material
# information should be gleaned from the SceneGraph
# constituents themselves, so that we visualize what the
# simulation is *actually* reasoning about rather than what
# files happen to be present.
candidate_texture_path = shape.filename()[0:-3] + "png"
if os.path.exists(candidate_texture_path):
material = g.MeshLambertMaterial(map=g.ImageTexture(
image=g.PngImage.from_file(
candidate_texture_path)))
# Make the uuid's deterministic for mesh geometry, to
# support caching at the zmqserver. This means that
# multiple (identical) geometries may have the same UUID,
# but testing suggests that meshcat + three.js are ok with
# it.
geom.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
geom.contents + "mesh"))
material.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
geom.contents + "material"))
X_FG = X_FG.dot(tf.scale_matrix(shape.scale()))
else:
warnings.warn(f"Unsupported shape {shape} ignored")
continue
geometry_vis = frame_vis[str(g_id.get_value())]
geometry_vis.set_object(geom, material)
geometry_vis.set_transform(X_FG)
if frame_id in self.frames_to_draw:
AddTriad(self.vis,
name=name,
prefix=self.prefix + "/" + name,
length=self.axis_length,
radius=self.axis_radius,
opacity=self.frames_opacity)
self.frames_to_draw.remove(frame_id)
if frame_id != inspector.world_frame_id():
self._dynamic_frames.append({
"id": frame_id,
"name": name,
})
# Loop through the input frames_to_draw list and warn the user if the
# frame_id does not exist in the scene graph.
for frame_id in self.frames_to_draw:
warnings.warn(f"Non-existent frame {frame_id} ignored")
continue
actor.set_mass(1.)
scene.add_actor(actor)
actor = RigidDynamic()
sphere = Shape.create_sphere(0.1, Material(restitution=1.))
sphere.set_user_data(dict(color='tab:blue'))
actor.attach_shape(sphere)
actor.set_global_pose([-0.5, 0.5, 1.0])
actor.set_mass(1.)
scene.add_actor(actor)
"Create a sphere with a texture."
actor = RigidDynamic()
sphere = Shape.create_sphere(0.1, Material(restitution=1.))
sphere.set_user_data(
dict(visual_mesh_texture=g.ImageTexture(image=g.PngImage.from_file(
Path(os.path.realpath(__file__)).parent.joinpath('texture.png')))))
actor.attach_shape(sphere)
actor.set_global_pose([-0.8, 0.5, 1.0])
actor.set_mass(1.)
scene.add_actor(actor)
actor = RigidDynamic()
obj: trimesh.Scene = trimesh.load('spade.obj',
split_object=True,
group_material=False)
for g in obj.geometry.values():
actor.attach_shape(
Shape.create_convex_mesh_from_points(g.vertices,
Material(restitution=1.),
scale=1e-3))
# Add custom coloring to the shapes
def load(self, context=None):
"""
Loads ``meshcat`` visualization elements.
Precondition:
Either the context is a valid Context for this system with the
geometry_query port connected or the ``scene_graph`` passed in the
constructor must be a valid SceneGraph.
"""
if self._delete_prefix_on_load:
self.vis[self.prefix].delete()
if context and self.get_geometry_query_input_port().HasValue(context):
inspector = self.get_geometry_query_input_port().Eval(
context).inspector()
elif self._scene_graph:
inspector = self._scene_graph.model_inspector()
else:
raise RuntimeError(
"You must provide a valid Context for this system with the "
"geometry_query port connected or the ``scene_graph`` passed "
"in the constructor must be a valid SceneGraph.")
vis = self.vis[self.prefix]
for frame_id in inspector.all_frame_ids():
count = inspector.NumGeometriesForFrameWithRole(
frame_id, Role.kIllustration)
if count == 0:
continue
if frame_id == inspector.world_frame_id():
name = "world"
else:
# Note: MBP declares frames with SceneGraph using `::`, we
# replace those with `/` here to expose the full tree to
# meshcat.
name = (inspector.GetOwningSourceName(frame_id) + "/" +
inspector.GetName(frame_id).replace("::", "/"))
frame_vis = vis[name]
for g_id in inspector.GetGeometries(frame_id, Role.kIllustration):
color = 0xe5e5e5 # default color
alpha = 1.0
props = inspector.GetIllustrationProperties(g_id)
if props and props.HasProperty("phong", "diffuse"):
rgba = props.GetProperty("phong", "diffuse")
# Convert Rgba from [0-1] to hex 0xRRGGBB.
color = int(255 * rgba.r()) * 256**2
color += int(255 * rgba.g()) * 256
color += int(255 * rgba.b())
alpha = rgba.a()
material = g.MeshLambertMaterial(color=color,
transparent=alpha != 1.,
opacity=alpha)
shape = inspector.GetShape(g_id)
X_FG = inspector.GetPoseInFrame(g_id).GetAsMatrix4()
if isinstance(shape, Box):
geom = g.Box(
[shape.width(),
shape.depth(),
shape.height()])
elif isinstance(shape, Sphere):
geom = g.Sphere(shape.radius())
elif isinstance(shape, Cylinder):
geom = g.Cylinder(shape.length(), shape.radius())
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
R_GC = RotationMatrix.MakeXRotation(np.pi / 2.0).matrix()
X_FG[0:3, 0:3] = X_FG[0:3, 0:3].dot(R_GC)
elif isinstance(shape, Mesh):
geom = g.ObjMeshGeometry.from_file(shape.filename()[0:-3] +
"obj")
# Attempt to find a texture for the object by looking for
# an identically-named *.png next to the model.
# TODO(gizatt): Support .MTLs and prefer them over png,
# since they're both more expressive and more standard.
# TODO(gizatt): In the long term, this kind of material
# information should be gleaned from the SceneGraph
# constituents themselves, so that we visualize what the
# simulation is *actually* reasoning about rather than what
# files happen to be present.
candidate_texture_path = shape.filename()[0:-3] + "png"
if os.path.exists(candidate_texture_path):
material = g.MeshLambertMaterial(map=g.ImageTexture(
image=g.PngImage.from_file(
candidate_texture_path)))
# Make the uuid's deterministic for mesh geometry, to
# support caching at the zmqserver. This means that
# multiple (identical) geometries may have the same UUID,
# but testing suggests that meshcat + three.js are ok with
# it.
geom.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
geom.contents + "mesh"))
material.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
geom.contents + "material"))
X_FG = X_FG.dot(tf.scale_matrix(shape.scale()))
else:
warnings.warn(f"Unsupported shape {shape} ignored")
continue
geometry_vis = frame_vis[str(g_id.get_value())]
geometry_vis.set_object(geom, material)
geometry_vis.set_transform(X_FG)
if frame_id != inspector.world_frame_id():
self._dynamic_frames.append({
"id": frame_id,
"name": name,
})
