def AddMeshcatTriad(meshcat, path, length=.25, radius=0.01, opacity=1., X_PT=RigidTransform()): meshcat.SetTransform(path, X_PT) # x-axis X_TG = RigidTransform(RotationMatrix.MakeYRotation(np.pi / 2), [length / 2., 0, 0]) meshcat.SetTransform(path + "/x-axis", X_TG) meshcat.SetObject(path + "/x-axis", Cylinder(radius, length), Rgba(1, 0, 0, opacity)) # y-axis X_TG = RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2), [0, length / 2., 0]) meshcat.SetTransform(path + "/y-axis", X_TG) meshcat.SetObject(path + "/y-axis", Cylinder(radius, length), Rgba(0, 1, 0, opacity)) # z-axis X_TG = RigidTransform([0, 0, length / 2.]) meshcat.SetTransform(path + "/z-axis", X_TG) meshcat.SetObject(path + "/z-axis", Cylinder(radius, length), Rgba(0, 0, 1, opacity))
def test_contact_visualizer(self, T): meshcat = Meshcat() params = ContactVisualizerParams() params.publish_period = 0.123 params.default_color = Rgba(0.5, 0.5, 0.5) params.prefix = "py_visualizer" params.delete_on_initialization_event = False params.force_threshold = 0.2 params.newtons_per_meter = 5 params.radius = 0.1 self.assertNotIn("object at 0x", repr(params)) params2 = ContactVisualizerParams(publish_period=0.4) self.assertEqual(params2.publish_period, 0.4) vis = ContactVisualizer_[T](meshcat=meshcat, params=params) vis.Delete() vis.contact_results_input_port() builder = DiagramBuilder_[T]() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.01) plant.Finalize() ContactVisualizer_[T].AddToBuilder(builder=builder, plant=plant, meshcat=meshcat, params=params) ContactVisualizer_[T].AddToBuilder( builder=builder, contact_results_port=plant.get_contact_results_output_port(), meshcat=meshcat, params=params)
def plot_surface(meshcat, path, X, Y, Z, rgba=Rgba(.87, .6, .6, 1.0), wireframe=False, wireframe_line_width=1.0): (rows, cols) = Z.shape assert (np.array_equal(X.shape, Y.shape)) assert (np.array_equal(X.shape, Z.shape)) vertices = np.empty((rows * cols, 3), dtype=np.float32) vertices[:, 0] = X.reshape((-1)) vertices[:, 1] = Y.reshape((-1)) vertices[:, 2] = Z.reshape((-1)) # Vectorized faces code from https://stackoverflow.com/questions/44934631/making-grid-triangular-mesh-quickly-with-numpy # noqa faces = np.empty((rows - 1, cols - 1, 2, 3), dtype=np.uint32) r = np.arange(rows * cols).reshape(rows, cols) faces[:, :, 0, 0] = r[:-1, :-1] faces[:, :, 1, 0] = r[:-1, 1:] faces[:, :, 0, 1] = r[:-1, 1:] faces[:, :, 1, 1] = r[1:, 1:] faces[:, :, :, 2] = r[1:, :-1, None] faces.shape = (-1, 3) # TODO(Russ): support per vertex / Colormap colors. meshcat.SetTriangleMesh(path, vertices.T, faces.T, rgba, wireframe, wireframe_line_width)
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 _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)
# q - quaternion message return RigidTransform(Quaternion(wxyz=[q.w, q.x, q.y, q.z]), [p.x, p.y, p.z]) def from_ros_pose(pose): """Converts ROS pose to Drake transform.""" return _read_pose_msg(p=pose.position, q=pose.orientation) def from_ros_transform(tr): """Converts ROS transform to Drake transform.""" return _read_pose_msg(p=tr.translation, q=tr.rotation) DEFAULT_RGBA = Rgba(0.9, 0.9, 0.9, 1.0) def to_ros_markers(shape, stamp, frame_name, X_FG, color_rgba): assert isinstance(shape, Shape), shape assert isinstance(frame_name, str), frame_name assert isinstance(X_FG, RigidTransform), X_FG marker = Marker() marker.header.stamp = stamp marker.header.frame_id = frame_name marker.pose = to_ros_pose(X_FG) marker.action = Marker.ADD marker.lifetime = Duration(0.) marker.frame_locked = True def use_color():
def plot_mathematical_program(meshcat, path, prog, X, Y, result=None, point_size=0.05): assert prog.num_vars() == 2 assert X.size == Y.size N = X.size values = np.vstack((X.reshape(-1), Y.reshape(-1))) costs = prog.GetAllCosts() # Vectorized multiply for the quadratic form. # Z = (D*np.matmul(Q,D)).sum(0).reshape(nx, ny) if costs: Z = prog.EvalBindingVectorized(costs[0], values) for b in costs[1:]: Z = Z + prog.EvalBindingVectorized(b, values) cv = f"{path}/constraints" for binding in prog.GetAllConstraints(): if isinstance(binding.evaluator(), BoundingBoxConstraint): c = binding.evaluator() var_indices = [ int(prog.decision_variable_index()[v.get_id()]) for v in binding.variables() ] satisfied = np.array( c.CheckSatisfiedVectorized(values[var_indices, :], 0.001)).reshape(1, -1) if costs: Z[~satisfied] = np.nan v = f"{cv}/{type(c).__name__}" Zc = np.zeros(Z.shape) Zc[satisfied] = np.nan plot_surface(meshcat, v, X, Y, Zc.reshape(X.shape), rgba=Rgba(1.0, .2, .2, 1.0), wireframe=True) else: Zc = prog.EvalBindingVectorized(binding, values) evaluator = binding.evaluator() low = evaluator.lower_bound() up = evaluator.upper_bound() cvb = f"{cv}/{type(evaluator).__name__}" for index in range(Zc.shape[0]): # TODO(russt): Plot infeasible points in a different color. infeasible = np.logical_or(Zc[index, :] < low[index], Zc[index, :] > up[index]) plot_surface(meshcat, f"{cvb}/{index}", X, Y, Zc[index, :].reshape(X.shape), rgba=Rgba(1.0, .3, 1.0, 1.0), wireframe=True) if costs: plot_surface(meshcat, f"{path}/objective", X, Y, Z.reshape(X.shape), rgba=Rgba(.3, 1.0, .3, 1.0), wireframe=True) if result: v = f"{path}/solution" meshcat.SetObject(v, Sphere(point_size), Rgba(.3, 1.0, .3, 1.0)) x_solution = result.get_x_val() meshcat.SetTransform( v, RigidTransform( [x_solution[0], x_solution[1], result.get_optimal_cost()]))
def _build_body_patches(self, use_random_colors, substitute_collocated_mesh_files, inspector): """ Generates body patches. self._patch_Blist stores a list of patches for each body (starting at body id 1). A body patch is a list of all 3D vertices of a piece of visual geometry. """ self._patch_Blist = {} self._patch_Blist_colors = {} for frame_id in inspector.GetAllFrameIds(): count = inspector.NumGeometriesForFrameWithRole( frame_id, Role.kIllustration) if count == 0: continue frame_name = self.frame_name(frame_id, inspector) this_body_patches = [] this_body_colors = [] for g_id in inspector.GetGeometries(frame_id, Role.kIllustration): X_BG = inspector.GetPoseInFrame(g_id) shape = inspector.GetShape(g_id) if isinstance(shape, Box): # Draw a bounding box. patch_G = np.vstack( (shape.width() / 2. * np.array([-1, -1, 1, 1, -1, -1, 1, 1]), shape.depth() / 2. * np.array([-1, 1, -1, 1, -1, 1, -1, 1]), shape.height() / 2. * np.array([-1, -1, -1, -1, 1, 1, 1, 1]))) elif isinstance(shape, Sphere): # Sphere is the only shape that allows a zero-measure. A # zero-radius sphere is a point, and we skip it. if shape.radius() == 0: continue lati, longi = np.meshgrid(np.arange(0., 2. * math.pi, 0.5), np.arange(0., 2. * math.pi, 0.5)) lati = lati.ravel() longi = longi.ravel() patch_G = np.vstack([ np.sin(lati) * np.cos(longi), np.sin(lati) * np.sin(longi), np.cos(lati) ]) patch_G *= shape.radius() elif isinstance(shape, Cylinder): radius = shape.radius() length = shape.length() # In the lcm geometry, cylinders are along +z # https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/systems/plants/RigidBodyCylinder.m # Two circles: one at bottom, one at top. sample_pts = np.arange(0., 2. * math.pi, 0.25) patch_G = np.hstack([ np.array([[ radius * math.cos(pt), radius * math.sin(pt), -length / 2. ], [ radius * math.cos(pt), radius * math.sin(pt), length / 2. ]]).T for pt in sample_pts ]) elif isinstance(shape, (Mesh, Convex)): filename = shape.filename() base, ext = os.path.splitext(filename) if (ext.lower() != ".obj" and substitute_collocated_mesh_files): # Check for a co-located .obj file (case insensitive). for f in glob.glob(base + '.*'): if f[-4:].lower() == '.obj': filename = f break if filename[-4:].lower() != '.obj': raise RuntimeError( f"The given file {filename} is not " f"supported and no alternate {base}" ".obj could be found.") if not os.path.exists(filename): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), filename) # Get mesh scaling. scale = shape.scale() mesh = ReadObjToTriangleSurfaceMesh(filename, scale) patch_G = np.vstack(mesh.vertices()) # Only store the vertices of the (3D) convex hull of the # mesh, as any interior vertices will still be interior # vertices after projection, and will therefore be removed # in _update_body_fill_verts(). vpoly = optimization.VPolytope(patch_G.T) patch_G = vpoly.GetMinimalRepresentation().vertices() elif isinstance(shape, HalfSpace): # For a half space, we'll simply create a large box with # the top face at z = 0, the bottom face at z = -1 and the # far corners at +/- 50 in the x- and y- directions. x = 50 y = 50 z = -1 patch_G = np.vstack( (x * np.array([-1, -1, 1, 1, -1, -1, 1, 1]), y * np.array([-1, 1, -1, 1, -1, 1, -1, 1]), z * np.array([-1, -1, -1, -1, 0, 0, 0, 0]))) # TODO(SeanCurtis-TRI): Provide support for capsule and # ellipsoid. else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( type(shape))) continue # Compute pose in body. patch_B = X_BG @ patch_G # Close path if not closed. if (patch_B[:, -1] != patch_B[:, 0]).any(): patch_B = np.hstack((patch_B, patch_B[:, 0][np.newaxis].T)) this_body_patches.append(patch_B) if not use_random_colors: # If we need to use random colors, we apply them after the # fact. See below. props = inspector.GetIllustrationProperties(g_id) assert props is not None rgba = props.GetPropertyOrDefault("phong", "diffuse", Rgba(0.9, 0.9, 0.9, 1.0)) color = np.array((rgba.r(), rgba.g(), rgba.b(), rgba.a())) this_body_colors.append(color) self._patch_Blist[frame_name] = this_body_patches self._patch_Blist_colors[frame_name] = this_body_colors # Spawn a random color generator. Each body will be given a unique # color when using this random generator, with each visual element of # the body colored the same. if use_random_colors: color = iter( plt_cm.rainbow(np.linspace(0, 1, len(self._patch_Blist_colors)))) for name in self._patch_Blist_colors.keys(): this_color = next(color) patch_count = len(self._patch_Blist[name]) self._patch_Blist_colors[name] = [this_color] * patch_count