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([1.0, 0.2, 0.3])) v["cube"].delete() 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 PlotTrajectoryMeshcat(x, t, vis, wpts_list = None): # initialize vis.delete() # plot waypoints if not(wpts_list is None): for i, wpts in enumerate(wpts_list): vis["wpt_%d" % i].set_object(geometry.Sphere(0.03), geometry.MeshLambertMaterial(color=0xffff00)) T_wp = tf.translation_matrix(wpts) vis["wpt_%d" % i].set_transform(T_wp) d_prop = 0.10 # propeller diameter vis["quad"]["CG"].set_object(geometry.Sphere(0.03), geometry.MeshLambertMaterial(color=0x00ffff)) vis["quad"]["body"].set_object(geometry.Box([0.2, 0.1, 0.1]), geometry.MeshLambertMaterial(color=0x404040)) vis["quad"]["prop0"].set_object(geometry.Cylinder(0.01, d_prop), geometry.MeshLambertMaterial(color=0x00ff00)) vis["quad"]["prop1"].set_object(geometry.Cylinder(0.01, d_prop), geometry.MeshLambertMaterial(color=0xff0000)) vis["quad"]["prop2"].set_object(geometry.Cylinder(0.01, d_prop), geometry.MeshLambertMaterial(color=0xffffff)) vis["quad"]["prop3"].set_object(geometry.Cylinder(0.01, d_prop), geometry.MeshLambertMaterial(color=0xffffff)) Rx_prop = CalcRx(np.pi/2) TB = tf.translation_matrix([0,0,-0.05]) T0 = tf.translation_matrix([l, -l, 0]) T1 = tf.translation_matrix([l, l, 0]) T2 = tf.translation_matrix([-l, l, 0]) T3 = tf.translation_matrix([-l, -l, 0]) T0[0:3,0:3] = Rx_prop T1[0:3,0:3] = Rx_prop T2[0:3,0:3] = Rx_prop T3[0:3,0:3] = Rx_prop vis["quad"]["body"].set_transform(TB) vis["quad"]["prop0"].set_transform(T0) vis["quad"]["prop1"].set_transform(T1) vis["quad"]["prop2"].set_transform(T2) vis["quad"]["prop3"].set_transform(T3) # visualize trajectory time.sleep(1.0) N = len(x) if not (t is None): assert N == len(t) for i, xi in enumerate(x): xyz = xi[0:3] rpy = xi[3:6] R_WB = CalcR_WB(rpy) T = tf.translation_matrix(xyz) T[0:3,0:3] = R_WB vis["quad"].set_transform(T) if i < N-1 and not(t is None): dt = t[i+1] - t[i] time.sleep(dt)
def draw_mblock(self): self.mblock = self.vis["mblock"] # create and draw the mblock dim = [3, 1, 1] self.mblock.set_object(g.Box(dim)) temp = tf.translation_matrix([0, 1, 0.5]) self.mblock.set_transform(temp)
def __init__(self): self.vis = meshcat.Visualizer() self.cube = self.vis["cube"] self.pivot = self.cube["pivot"] self.wheel = self.pivot["wheel"] # create and draw the cube self.cube_dim = [1.0, 1.0, 1.0] # x,y,z self.cube.set_object(g.Box(self.cube_dim)) # pivot and wheel self.pivot.set_transform(tf.translation_matrix( [0, 0, 0])) # set location of pole wheel_dim = [1.5, .5, .5] # x,y,z self.wheel.set_object(g.Box(wheel_dim)) self.initialize()
def runTest(self): 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])) animation = meshcat.animation.Animation() with animation.at_frame(v, 0) as frame_vis: frame_vis.set_transform(tf.translation_matrix([0, 0, 0])) with animation.at_frame(v, 30) as frame_vis: frame_vis.set_transform(tf.translation_matrix([2, 0, 0]).dot(tf.rotation_matrix(np.pi/2, [0, 0, 1]))) v.set_animation(animation)
def runTest(self): """ Test that we can set_object with a PerspectiveCamera. """ self.vis.set_object(g.Box([0.5, 0.5, 0.5])) camera = g.PerspectiveCamera(fov=90) self.vis['/Cameras/default/rotated'].set_object(camera) self.vis['/Cameras/default'].set_transform( tf.translation_matrix([1, -1, 0.5])) self.vis['/Cameras/default/rotated/<object>'].set_property( "position", [0, 0, 0])
def meshcat_draw_frustrum(vis, TF, K, near_distance, far_distance, w, h): # TODO(gizatt): This obviously isn't right -- the projected # light doesn't match the drawn view frustrum. # Not dealing with, for now; I think the issue is a combination # of bad intrinsics and bugs related to flipped image coordinates # somewhere along the pipeline. image_bbox_verts = np.array([ [0., w, w, 0.], [0., 0., h, h] ]) TF_inv = np.eye(4) TF_inv[:3, :3] = TF[:3, :3].T TF_inv[:3, 3] = -TF_inv[:3, :3].dot(TF[:3, 3]) TF = TF_inv N = image_bbox_verts.shape[1] Kinv = np.linalg.inv(K) def project_bbox_verts(dist): homog = np.concatenate( [image_bbox_verts*dist, dist*np.ones((1, N))], axis=0 ) pts = np.dot(Kinv, homog) return ((TF[:3, :3].dot(pts)).T + TF[:3, 3]).T near_pts = project_bbox_verts(near_distance) far_pts= project_bbox_verts(far_distance) near_colors = np.zeros((3, N)) near_colors[1, :] = 1. far_colors = np.zeros((3, N)) far_colors[2, :] = 1. vis['frustrum']['near'].set_object(g.LineLoop( g.PointsGeometry(near_pts, color=near_colors), g.MeshBasicMaterial(vertexColors=True, linewidth=0.1))) vis['frustrum']['far'].set_object(g.LineLoop( g.PointsGeometry(far_pts, color=far_colors), g.MeshBasicMaterial(vertexColors=True, linewidth=0.1))) connecting = np.zeros((3, N*2)) connecting[:, ::2] = near_pts connecting[:, 1::2] = far_pts connecting_colors = np.zeros((3, N*2)) connecting_colors[:, ::2] = near_colors connecting_colors[:, 1::2] = far_colors vis['frustrum']['connecting'].set_object(g.LineSegments( g.PointsGeometry(connecting, color=connecting_colors), g.MeshBasicMaterial(vertexColors=True, linewidth=1.) )) # Draw a little box for the projector :) vis['projector'].set_object( g.Box([0.1, 0.1, 0.1]), g.MeshLambertMaterial( color=0xaaffaa))
def _get_shape_geometry(self, shape): visual_mesh = shape.get_user_data().get('visual_mesh', None) if shape.get_user_data() is not None else None if visual_mesh is not None: return g.TriangularMeshGeometry(vertices=visual_mesh.vertices, faces=visual_mesh.faces) elif shape.get_geometry_type() == GeometryType.CONVEXMESH: data = shape.get_shape_data() # N x 9 - i.e. 3 triangles faces = np.arange(0, data.shape[0] * 3, 1, dtype=np.int).reshape(-1, 3) return g.TriangularMeshGeometry(vertices=data.reshape(-1, 3), faces=faces) elif shape.get_geometry_type() == GeometryType.SPHERE: return g.Sphere(radius=shape.get_sphere_radius()) elif shape.get_geometry_type() == GeometryType.BOX: return g.Box((2 * shape.get_box_half_extents()).tolist()) else: raise NotImplementedError("Not supported geometry type.")
def runTest(self): """ Test that we can set_object with an OrthographicCamera. """ self.vis.set_object(g.Box([0.5, 0.5, 0.5])) camera = g.OrthographicCamera( left=-1, right=1, bottom=-1, top=1, near=-1000, far=1000) self.vis['/Cameras/default/rotated'].set_object(camera) self.vis['/Cameras/default'].set_transform( tf.translation_matrix([0, -1, 0])) self.vis['/Cameras/default/rotated/<object>'].set_property( "position", [0, 0, 0]) self.vis['/Grid'].set_property("visible", False)
def runTest(self): 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])) animation = meshcat.animation.Animation() with animation.at_frame(v, 0) as frame_vis: frame_vis.set_transform(tf.translation_matrix([0, 0, 0])) with animation.at_frame(v, 30) as frame_vis: frame_vis.set_transform(tf.translation_matrix([2, 0, 0]).dot(tf.rotation_matrix(np.pi/2, [0, 0, 1]))) with animation.at_frame(v, 0) as frame_vis: frame_vis["/Cameras/default/rotated/<object>"].set_property("zoom", "number", 1) with animation.at_frame(v, 30) as frame_vis: frame_vis["/Cameras/default/rotated/<object>"].set_property("zoom", "number", 0.5) v.set_animation(animation)
def visualize(self, states, dt): import meshcat import meshcat.geometry as g import meshcat.transformations as tf import time # import meshcat.animation as anim # Create a new visualizer vis = meshcat.Visualizer() vis.open() vis["cart"].set_object(g.Box([0.2, 0.5, 0.2])) vis["pole"].set_object(g.Cylinder(self.length_pole, 0.01)) # animation = anim.Animation() # for i, state in enumerate(states): # with animation.at_frame(vis, i*10) as frame: # print(frame) # frame["cart"].set_transform(tf.translation_matrix([0, state[0], 0])) # frame["pole"].set_transform( # tf.translation_matrix([0, state[0] + self.length_pole/2, 0]).dot( # tf.rotation_matrix(np.pi/2 + state[1], [1,0,0], [0,-self.length_pole/2,0]))) # vis.set_animation(animation, play=True, repeat=10) # time.sleep(10) # # anim.convert_frame_to_video() while True: # vis["cart"].set_transform(tf.translation_matrix([0, 0, 0])) # vis["pole"].set_transform( # tf.translation_matrix([0, 0 + self.length_pole/2, 0]).dot( # tf.rotation_matrix(np.pi/2 + 0, [1,0,0], [0,-self.length_pole/2,0]))) # time.sleep(dt) for state in states: vis["cart"].set_transform( tf.translation_matrix([0, state[0], 0])) vis["pole"].set_transform( tf.translation_matrix( [0, state[0] + self.length_pole / 2, 0]).dot( tf.rotation_matrix(np.pi / 2 + state[1], [1, 0, 0], [0, -self.length_pole / 2, 0]))) time.sleep(dt)
def _get_shape_geometry(self, shape): visual_mesh = shape.get_user_data().get( 'visual_mesh', None) if shape.get_user_data() is not None else None if visual_mesh is not None: try: exp_obj = trimesh.exchange.obj.export_obj(visual_mesh) except ValueError: exp_obj = trimesh.exchange.obj.export_obj( visual_mesh, include_texture=False) return g.ObjMeshGeometry.from_stream( trimesh.util.wrap_as_stream(exp_obj)) elif shape.get_geometry_type() == GeometryType.CONVEXMESH: data = shape.get_shape_data() # N x 9 - i.e. 3 triangles faces = np.arange(0, data.shape[0] * 3, 1, dtype=np.int).reshape(-1, 3) return g.TriangularMeshGeometry(vertices=data.reshape(-1, 3), faces=faces) elif shape.get_geometry_type() == GeometryType.SPHERE: return g.Sphere(radius=shape.get_sphere_radius()) elif shape.get_geometry_type() == GeometryType.BOX: return g.Box((2 * shape.get_box_half_extents()).tolist()) else: raise NotImplementedError("Not supported geometry type.")
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] 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, })
def runTest(self): v = self.vis["shapes"] v["cube"].set_object(g.Box([0.1, 0.2, 0.3])) v.set_transform(tf.translation_matrix([1., 0, 0])) v.set_transform(tf.translation_matrix([1., 1., 0]))
def runTest(self): v = self.vis["shapes"] v["cube"].set_object(g.Box([0.1, 0.2, 0.3]))
dt = np.diff(data[:,0]) for i in range(num_agents): v = np.sqrt(data[:,i*4+3]**2 + data[:,i*4+4]**2) a = np.diff(v) / dt ax2.plot(data[0:-1,0],a) plt.show() if args.animate: import meshcat import meshcat.geometry as g import meshcat.transformations as tf # Create a new visualizer vis = meshcat.Visualizer() vis.open() for i in range(num_agents): vis["agent"+str(i)].set_object(g.Sphere(0.2)) for i, o in enumerate(map_data["map"]["obstacles"]): vis["obstacles"+str(i)].set_object(g.Box([1.0, 1.0, 0.2])) print(o) vis["obstacles"+str(i)].set_transform(tf.translation_matrix(np.array([o[0]+0.5, o[1]+0.5, 0]))) while True: for k in np.arange(0,data.shape[0],10): t = data[k,0] for i in range(num_agents): state = data[k,i*4+1:i*4+5] vis["agent" + str(i)].set_transform(tf.translation_matrix([state[0], state[1], 0])) time.sleep(0.01)
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 draw_tree(tree, vis, prefix="", draw_regions=False): # Given a scene tree (nx.DiGraph), draw it in the # specified meshcat visualizer. # Draw the scene geometry flat, to keep TFs easy. name_prefix = prefix + "scene" vis[name_prefix].delete() k = 0 for node in tree.nodes: name = name_prefix + "/%s_%03d" % (node.__class__.__name__, k) if node.geometry is not None: color = node.geometry_color alpha = 1.0 vis[name].set_object( node.geometry, meshcat_geom.MeshLambertMaterial(color=color, opacity=alpha, transparent=(alpha != 1.)) ) tf = node.tf.GetAsMatrix4() geom_tf = node.geometry_tf.GetAsMatrix4() tf = tf.dot(geom_tf) tf[:3, :3] = tf[:3, :3].dot(np.diag(node.geometry_scale)) print(tf) vis[name].set_transform(tf) k += 1 # Draw the tree structure. tree_prefix = prefix + "tree" vis[tree_prefix].delete() k = 0 for node in tree.nodes: name = tree_prefix + "/" + node.__class__.__name__ + "_%03d" % k k += 1 # Draw node as randomly colored sphere color = random.randint(0, 0xFFFFFF) alpha = 0.5 vis[name]["triad"].set_object( meshcat_geom.triad(scale=0.1) ) vis[name]["sphere"].set_object( meshcat_geom.Sphere(0.01), meshcat_geom.MeshToonMaterial(color=color, opacity=alpha, transparent=(alpha != 1.)) ) vis[name].set_transform(node.tf.GetAsMatrix4()) # Draw children verts = [] for child in tree.successors(node): # Draw link to child verts.append(node.tf.translation()), verts.append(child.tf.translation()) if len(verts) > 0: verts = np.vstack(verts).T # Don't want this as a direct child or it'll inherit the transform vis[name + "_child_connections"].set_object( meshcat_geom.Line(meshcat_geom.PointsGeometry(verts), meshcat_geom.LineBasicMaterial(linewidth=50, color=color))) if draw_regions: # Draw the child regions for each child if isinstance(node, (AndNode, OrNode, RepeatingSetNode)): for info_k, child_info in enumerate(node.child_infos): region_name = "child_region_%03d" % info_k lb = child_info.child_xyz_bounds.xyz_min ub = child_info.child_xyz_bounds.xyz_max vis[name][region_name].set_object( meshcat_geom.Box(ub - lb), meshcat_geom.MeshToonMaterial(color=0x111111, opacity=0.1, transparent=True) ) tf = RigidTransform(p=(ub+lb)/2) vis[name][region_name].set_transform(tf.GetAsMatrix4())