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, })