def DoPublish(self, context, event):
# TODO(SeanCurtis-TRI) We want to be able to use this visualizer to
# draw without having it part of a Simulator. That means we'd like
# vis.Publish(context) to work. Currently, pydrake offers no mechanism
# to declare a forced event. However, by overriding DoPublish and
# putting the forced event callback code in the override, we can
# simulate it.
# We need to bind a mechanism for declaring forced events so we don't
# have to resort to overriding the dispatcher.
LeafSystem.DoPublish(self, context, event)
contact_results = self.EvalAbstractInput(context, 0).get_value()
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
contacts = []
for i_contact in range(contact_results.num_point_pair_contacts()):
contact_info = contact_results.point_pair_contact_info(i_contact)
# Do not display small forces.
force_norm = np.linalg.norm(contact_info.contact_force())
if force_norm < self._force_threshold:
continue
point_pair = contact_info.point_pair()
key = (point_pair.id_A.get_value(), point_pair.id_B.get_value())
cvis = vis[str(key)]
contacts.append(key)
arrow_height = self._radius * 2.0
if key not in self._published_contacts:
# New key, so create the geometry. Note: the height of the
# cylinder is 2 and gets scaled to twice the contact force
# length, because I am drawing both (equal and opposite)
# forces. Note also that meshcat (following three.js) puts
# the height of the cylinder along the y axis.
cvis["cylinder"].set_object(
meshcat.geometry.Cylinder(height=2.0, radius=self._radius),
meshcat.geometry.MeshLambertMaterial(color=0x33cc33))
cvis["head"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=0,
radiusBottom=self._radius * 2.0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
cvis["tail"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=self._radius * 2.0,
radiusBottom=0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
height = force_norm / self._contact_force_scale
cvis["cylinder"].set_transform(
tf.scale_matrix(height, direction=[0, 1, 0]))
cvis["head"].set_transform(
tf.translation_matrix([0, height + arrow_height / 2.0, 0.0]))
cvis["tail"].set_transform(
tf.translation_matrix([0, -height - arrow_height / 2.0, 0.0]))
# The contact frame's origin is located at contact_point and is
# oriented such that Cy is aligned with the contact force.
p_WC = contact_info.contact_point() # documented as in world.
if force_norm < 1e-6:
# We cannot rely on self._force_threshold to determine if the
# force can be normalized; that threshold can be zero.
R_WC = RotationMatrix()
else:
fhat_C_W = contact_info.contact_force() / force_norm
R_WC = RotationMatrix.MakeFromOneVector(b_A=fhat_C_W,
axis_index=1)
X_WC = RigidTransform(R=R_WC, p=p_WC)
cvis.set_transform(X_WC.GetAsMatrix4())
# We only delete any contact vectors that did not persist into this
# publish. It is tempting to just delete() the root branch at the
# beginning of this publish, but this leads to visual artifacts
# (flickering) in the browser.
for key in set(self._published_contacts) - set(contacts):
vis[str(key)].delete()
self._published_contacts = contacts
def DoPublish(self, context, event):
LeafSystem.DoPublish(self, context, event)
if (not self._warned_pose_bundle_input_port_connected
and self.get_input_port(0).HasValue(context)):
_warn_deprecated(
"The pose_bundle input port of MeshcatContactVisualizer is"
"deprecated; use the geometry_query inport port instead.",
date="2021-04-01")
self._warned_pose_bundle_input_port_connected = True
contact_results = self.EvalAbstractInput(context, 1).get_value()
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
contacts = []
for i_contact in range(contact_results.num_point_pair_contacts()):
contact_info = contact_results.point_pair_contact_info(i_contact)
# Do not display small forces.
force_norm = np.linalg.norm(contact_info.contact_force())
if force_norm < self._force_threshold:
continue
point_pair = contact_info.point_pair()
key = (point_pair.id_A.get_value(), point_pair.id_B.get_value())
cvis = vis[str(key)]
contacts.append(key)
arrow_height = self._radius * 2.0
if key not in self._published_contacts:
# New key, so create the geometry. Note: the height of the
# cylinder is 2 and gets scaled to twice the contact force
# length, because I am drawing both (equal and opposite)
# forces. Note also that meshcat (following three.js) puts
# the height of the cylinder along the y axis.
cvis["cylinder"].set_object(
meshcat.geometry.Cylinder(height=2.0, radius=self._radius),
meshcat.geometry.MeshLambertMaterial(color=0x33cc33))
cvis["head"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=0,
radiusBottom=self._radius * 2.0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
cvis["tail"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=self._radius * 2.0,
radiusBottom=0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
height = force_norm / self._contact_force_scale
cvis["cylinder"].set_transform(
tf.scale_matrix(height, direction=[0, 1, 0]))
cvis["head"].set_transform(
tf.translation_matrix([0, height + arrow_height / 2.0, 0.0]))
cvis["tail"].set_transform(
tf.translation_matrix([0, -height - arrow_height / 2.0, 0.0]))
# Frame C is located at the contact point, but with the world frame
# orientation.
if force_norm < 1e-6:
X_CGeom = tf.identity_matrix()
else:
# Rotates [0,1,0] to contact_force/force_norm.
angle_axis = np.cross(
np.array([0, 1, 0]),
contact_info.contact_force() / force_norm)
X_CGeom = tf.rotation_matrix(
np.arcsin(np.linalg.norm(angle_axis)), angle_axis)
X_WC = tf.translation_matrix(contact_info.contact_point())
cvis.set_transform(X_WC @ X_CGeom)
# We only delete any contact vectors that did not persist into this
# publish. It is tempting to just delete() the root branch at the
# beginning of this publish, but this leads to visual artifacts
# (flickering) in the browser.
for key in set(self._published_contacts) - set(contacts):
vis[str(key)].delete()
self._published_contacts = contacts
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
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,
})