def make_frame(vis, name, h, radius, o=1.0): """Add a red-green-blue triad to the Meschat visualizer. Args: vis (MeshCat Visualizer): the visualizer name (string): name for this frame (should be unique) h (float): height of frame visualization radius (float): radius of frame visualization o (float): opacity """ vis[name]['x'].set_object( g.Cylinder(height=h, radius=radius), g.MeshLambertMaterial(color=0xff0000, reflectivity=0.8, opacity=o)) rotate_x = mtf.rotation_matrix(np.pi / 2.0, [0, 0, 1]) rotate_x[0, 3] = h / 2 vis[name]['x'].set_transform(rotate_x) vis[name]['y'].set_object( g.Cylinder(height=h, radius=radius), g.MeshLambertMaterial(color=0x00ff00, reflectivity=0.8, opacity=o)) rotate_y = mtf.rotation_matrix(np.pi / 2.0, [0, 1, 0]) rotate_y[1, 3] = h / 2 vis[name]['y'].set_transform(rotate_y) vis[name]['z'].set_object( g.Cylinder(height=h, radius=radius), g.MeshLambertMaterial(color=0x0000ff, reflectivity=0.8, opacity=o)) rotate_z = mtf.rotation_matrix(np.pi / 2.0, [1, 0, 0]) rotate_z[2, 3] = h / 2 vis[name]['z'].set_transform(rotate_z)
def draw_transformation(self, state): state = list(state) origin = state[0:3] origin[0] = state[1] origin[1] = state[0] origin[2] = state[2] + self.cube_dim[2] / 2.0 theta = state[4] wheel_angle = state[6] temp = tf.rotation_matrix(theta, [1, 0, 0]) # assume rotate about y temp[0:3, -1] = tf.translation_from_matrix(tf.translation_matrix(origin)) self.cube.set_transform(temp) self.wheel.set_transform(tf.rotation_matrix( -wheel_angle, [1, 0, 0])) # rotate the pole
def draw_transformation(self, state, dim=2.0): nd = len(state) state = list(state) origin = state[0:3] # so you can edit origin[0] = 0.0 origin[1] = state[0] origin[2] = state[1] + self.cube_dim[2] / 2.0 theta = state[dim] wheel_angle = state[len(state) / 2 - 1] temp = tf.rotation_matrix(theta, [1, 0, 0]) # assume rotate about y temp[0:3, -1] = tf.translation_from_matrix(tf.translation_matrix(origin)) self.cube.set_transform(temp) self.wheel.set_transform(tf.rotation_matrix( wheel_angle, [1, 0, 0])) # rotate the pole
def add_geometry(self, geometry, pathname, transform): vis = self.vis material = g.MeshLambertMaterial(reflectivity=1.0, sides=0) material.transparency = True material.opacity = 0.5 if isinstance(geometry, Sphere): sphere = geometry vis[pathname].set_object( g.Sphere(sphere.radius), material) vis[pathname].set_transform(transform) elif isinstance(geometry, Cylinder): cyl = geometry vis[pathname].set_object( g.Cylinder(cyl.length, cyl.radius), material ) # meshcat cylinder is aligned along y-axis. Align along z then apply the # node's transform as normal. vis[pathname].set_transform( transform.dot( tf.rotation_matrix(np.radians(-90), [1, 0, 0]) ) ) elif isinstance(geometry, Mesh): obj = meshcat.geometry.StlMeshGeometry.from_stream( io.BytesIO(trimesh.exchange.stl.export_stl(geometry.trimesh)) ) vis[pathname].set_object(obj, material) vis[pathname].set_transform(transform) else: raise NotImplementedError("Cannot yet add {} to visualiser".format(type(geometry)))
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 PublishAllGeometry(self): n_bodies = self.rbtree.get_num_bodies()-1 all_meshcat_geometry = {} for body_i in range(n_bodies): body = self.rbtree.get_body(body_i+1) # TODO(gizatt) Replace these body-unique indices # with more readable body.get_model_name() or other # model index information when an appropriate # function gets bound in pydrake. body_name = body.get_name() + ("(%d)" % body_i) if self.draw_collision: draw_elements = [self.rbtree.FindCollisionElement(k) for k in body.get_collision_element_ids()] else: draw_elements = body.get_visual_elements() for element_i, element in enumerate(draw_elements): element_local_tf = element.getLocalTransform() if element.hasGeometry(): geom = element.getGeometry() geom_type = geom.getShape() if geom_type == Shape.SPHERE: meshcat_geom = meshcat.geometry.Sphere(geom.radius) elif geom_type == Shape.BOX: meshcat_geom = meshcat.geometry.Box(geom.size) elif geom_type == Shape.CYLINDER: meshcat_geom = meshcat.geometry.Cylinder( geom.length, geom.radius) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( np.pi/2., [1, 0, 0]) element_local_tf[0:3, 0:3] = \ element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3]) elif geom_type == Shape.MESH: meshcat_geom = \ meshcat.geometry.ObjMeshGeometry.from_file( geom.resolved_filename[0:-3] + "obj") # respect mesh scale element_local_tf[0:3, 0:3] *= geom.scale else: print "UNSUPPORTED GEOMETRY TYPE ",\ geom.getShape(), " IGNORED" continue rgba = [1., 0.7, 0., 1.] if not self.draw_collision: rgba = element.getMaterial() self.vis[self.prefix][body_name][str(element_i)]\ .set_object(meshcat_geom, meshcat.geometry.MeshLambertMaterial( color=Rgba2Hex(rgba))) self.vis[self.prefix][body_name][str(element_i)].\ set_transform(element_local_tf)
def PublishAllGeometry(self): n_bodies = self.rbtree.get_num_bodies()-1 all_meshcat_geometry = {} for body_i in range(n_bodies): body = self.rbtree.get_body(body_i+1) # TODO(gizatt) Replace these body-unique indices # with more readable body.get_model_name() or other # model index information when an appropriate # function gets bound in pydrake. body_name = body.get_name() + ("(%d)" % body_i) if self.draw_collision: draw_elements = [self.rbtree.FindCollisionElement(k) for k in body.get_collision_element_ids()] else: draw_elements = body.get_visual_elements() for element_i, element in enumerate(draw_elements): element_local_tf = element.getLocalTransform() if element.hasGeometry(): geom = element.getGeometry() geom_type = geom.getShape() if geom_type == Shape.SPHERE: meshcat_geom = meshcat.geometry.Sphere(geom.radius) elif geom_type == Shape.BOX: meshcat_geom = meshcat.geometry.Box(geom.size) elif geom_type == Shape.CYLINDER: meshcat_geom = meshcat.geometry.Cylinder( geom.length, geom.radius) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi/2., [1, 0, 0]) element_local_tf[0:3, 0:3] = \ element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3]) elif geom_type == Shape.MESH: meshcat_geom = \ meshcat.geometry.ObjMeshGeometry.from_file( geom.resolved_filename[0:-3] + "obj") # respect mesh scale element_local_tf[0:3, 0:3] *= geom.scale else: print "UNSUPPORTED GEOMETRY TYPE ",\ geom.getShape(), " IGNORED" continue rgba = [1., 0.7, 0., 1.] if not self.draw_collision: rgba = element.getMaterial() self.vis[self.prefix][body_name][str(element_i)]\ .set_object(meshcat_geom, meshcat.geometry.MeshLambertMaterial( color=Rgba2Hex(rgba))) self.vis[self.prefix][body_name][str(element_i)].\ set_transform(element_local_tf)
def AlignSceneToModel(scene_points, model_points, max_distance=0.05, num_iters=10, num_sample_points=250): """ Finds a good (x, y, sin(theta), cos(theta) transform between scene_points and model_points. Args: @param scene_points An Nx3 numpy array of scene points. @param model_points An Nx3 numpy array of model points. @param max_distance float. The maximum distance in meters to consider of nearest neighbors between scene_points and model_points. @param num_iters int. The number of times to try the best fit optimization from different initial guesses. @param num_sample_points: int. The number of points to sample from the scene to limit the time the algorithm takes to run. Returns: @return best_X_MS The best 4x4 homogenous transform between scene_points and model_points. @return best_cost float. The cost function evaluated at best_X_MS. """ scene_sample = scene_points[np.random.choice(scene_points.shape[0], num_sample_points, replace=False), :] homogenous_scene = np.ones((4, scene_sample.shape[0])) homogenous_model = np.ones((4, model_points.shape[0])) homogenous_scene[:3, :] = scene_sample.T homogenous_model[:3, :] = model_points.T centroid_scene = np.mean(homogenous_scene, axis=1) centroid_model = np.mean(homogenous_model, axis=1) best_X_MS = None best_cost = float('inf') for i in range(num_iters): init_theta = np.random.uniform(0, 2*np.pi) init_guess = reduce(np.dot, [tf.translation_matrix((centroid_model[0], centroid_model[1], 0.)), tf.rotation_matrix(init_theta, (0, 0, 1)), tf.translation_matrix((-centroid_scene[0], -centroid_scene[1], 0.))]) X_MS, cost = FindBestFitTransform( homogenous_scene,homogenous_model, init_guess, max_distance) if cost < best_cost: best_cost = cost best_X_MS = X_MS return best_X_MS, best_cost
def load(self): """ Loads `meshcat` visualization elements. @pre The `scene_graph` used to construct this object must be part of a fully constructed diagram (e.g. via `DiagramBuilder.Build()`). """ # Intercept load message via mock LCM. mock_lcm = DrakeMockLcm() DispatchLoadMessage(self._scene_graph, mock_lcm) load_robot_msg = lcmt_viewer_load_robot.decode( mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT")) # Translate elements to `meshcat`. for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] [source_name, frame_name] = link.name.split("::") for j in range(link.num_geom): geom = link.geom[j] element_local_tf = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsMatrix4() if geom.type == geom.BOX: assert geom.num_float_data == 3 meshcat_geom = meshcat.geometry.Box(geom.float_data) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0]) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 meshcat_geom = meshcat.geometry.Cylinder( geom.float_data[1], geom.float_data[0]) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi/2., [1, 0, 0]) element_local_tf[0:3, 0:3] = \ element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3]) elif geom.type == geom.MESH: meshcat_geom = \ meshcat.geometry.ObjMeshGeometry.from_file( geom.string_data[0:-3] + "obj") else: print "UNSUPPORTED GEOMETRY TYPE ", \ geom.type, " IGNORED" continue self.vis[self.prefix][source_name][str(link.robot_num)][ frame_name][str(j)]\ .set_object(meshcat_geom, meshcat.geometry.MeshLambertMaterial( color=Rgba2Hex(geom.color))) self.vis[self.prefix][source_name][str(link.robot_num)][ frame_name][str(j)].set_transform(element_local_tf)
def runTest(self): """ Test that we can render meshes from raw vertices and faces as numpy arrays """ v = self.vis["triangular_mesh"] v.set_transform(tf.rotation_matrix(np.pi / 2, [0., 0, 1])) vertices = np.array([[0, 0, 0], [1, 0, 0], [1, 0, 1], [0, 0, 1]]) faces = np.array([[0, 1, 2], [3, 0, 2]]) v.set_object(g.TriangularMeshGeometry(vertices, faces), g.MeshLambertMaterial(color=0xeedd22, wireframe=True)) v = self.vis["triangular_mesh_w_vertex_coloring"] v.set_transform( tf.translation_matrix([1, 0, 0]).dot( tf.rotation_matrix(np.pi / 2, [0, 0, 1]))) colors = vertices v.set_object(g.TriangularMeshGeometry(vertices, faces, colors), g.MeshLambertMaterial(vertexColors=True, wireframe=True))
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): 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 PublishAllGeometry(self): n_bodies = self.rbtree.get_num_bodies() - 1 all_meshcat_geometry = {} for body_i in range(n_bodies): body = self.rbtree.get_body(body_i + 1) body_name = body.get_name() visual_elements = body.get_visual_elements() this_body_patches = [] for element_i, element in enumerate(visual_elements): element_local_tf = element.getLocalTransform() if element.hasGeometry(): geom = element.getGeometry() geom_type = geom.getShape() if geom_type == Shape.SPHERE: meshcat_geom = meshcat.geometry.Sphere(geom.radius) elif geom_type == Shape.BOX: meshcat_geom = meshcat.geometry.Box(geom.size) elif geom_type == Shape.CYLINDER: meshcat_geom = meshcat.geometry.Cylinder( geom.length, geom.radius) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi / 2., [1, 0, 0]) print extra_rotation element_local_tf[0:3, 0:3] = \ element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3]) elif geom_type == Shape.MESH: meshcat_geom = \ meshcat.geometry.ObjMeshGeometry.from_file( geom.resolved_filename[0:-3] + "obj") # respect mesh scale element_local_tf[0:3, 0:3] *= geom.scale else: print "UNSUPPORTED GEOMETRY TYPE ",\ geom.getShape(), " IGNORED" continue self.vis[body_name][str(element_i)].set_object( meshcat_geom) print body_name, element_i, element_local_tf self.vis[body_name][str(element_i)].set_transform( element_local_tf)
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 draw_bond(v, label, p1, d, radius, color=0xffffff): H = np.linalg.norm(d) R = np.linalg.norm(d[:2]) e = d / H x = np.array([0, 1, 0]) rot = -acos(e @ x) if -1 < e @ x < 1: axis = np.cross(e, x) v[label].set_object( g.Mesh(g.Cylinder(H, radius), g.MeshLambertMaterial(color=color))) v[label].set_transform( tf.translation_matrix(p1).dot( tf.rotation_matrix(rot, axis).dot( tf.translation_matrix([0, H / 2, 0])))) else: v[label].set_object( g.Mesh(g.Cylinder(H, radius), g.MeshLambertMaterial(color=color))) v[label].set_transform(tf.translation_matrix(p1 + d / 2))
def PublishAllGeometry(self): n_bodies = self.rbtree.get_num_bodies() - 1 all_meshcat_geometry = {} for body_i in range(n_bodies): body = self.rbtree.get_body(body_i + 1) # TODO(gizatt) Replace these body-unique indices # with more readable body.get_model_name() or other # model index information when an appropriate # function gets bound in pydrake. body_name = body.get_name() + ("(%d)" % body_i) visual_elements = body.get_visual_elements() this_body_patches = [] for element_i, element in enumerate(visual_elements): element_local_tf = element.getLocalTransform() if element.hasGeometry(): geom = element.getGeometry() geom_type = geom.getShape() if geom_type == Shape.SPHERE: meshcat_geom = meshcat.geometry.Sphere(geom.radius) elif geom_type == Shape.BOX: meshcat_geom = meshcat.geometry.Box(geom.size) elif geom_type == Shape.CYLINDER: meshcat_geom = meshcat.geometry.Cylinder( geom.length, geom.radius) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi / 2., [1, 0, 0]) print extra_rotation element_local_tf[0:3, 0:3] = \ element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3]) elif geom_type == Shape.MESH: meshcat_geom = \ meshcat.geometry.ObjMeshGeometry.from_file( geom.resolved_filename[0:-3] + "obj") # respect mesh scale element_local_tf[0:3, 0:3] *= geom.scale else: print "UNSUPPORTED GEOMETRY TYPE ",\ geom.getShape(), " IGNORED" continue def rgba2hex(rgb): ''' Turn a list of R,G,B elements (any indexable list of >= 3 elements will work), where each element is specified on range [0., 1.], into the equivalent 24-bit value 0xRRGGBB. ''' val = 0. for i in range(3): val += (256**(2 - i)) * (255. * rgb[i]) return val self.vis[self.prefix][body_name][str(element_i)]\ .set_object(meshcat_geom, meshcat.geometry.MeshLambertMaterial( color=rgba2hex(element.getMaterial()))) self.vis[self.prefix][body_name][str(element_i)].\ set_transform(element_local_tf)
def load(self): """ Loads `meshcat` visualization elements. @pre The `scene_graph` used to construct this object must be part of a fully constructed diagram (e.g. via `DiagramBuilder.Build()`). """ self.vis[self.prefix].delete() # Intercept load message via mock LCM. mock_lcm = DrakeMockLcm() DispatchLoadMessage(self._scene_graph, mock_lcm) load_robot_msg = lcmt_viewer_load_robot.decode( mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT")) # Translate elements to `meshcat`. for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] [source_name, frame_name] = link.name.split("::") for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currenly sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue element_local_tf = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsMatrix4() if geom.type == geom.BOX: assert geom.num_float_data == 3 meshcat_geom = meshcat.geometry.Box(geom.float_data) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0]) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 meshcat_geom = meshcat.geometry.Cylinder( geom.float_data[1], geom.float_data[0]) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi/2., [1, 0, 0]) element_local_tf[0:3, 0:3] = \ element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3]) elif geom.type == geom.MESH: meshcat_geom = \ meshcat.geometry.ObjMeshGeometry.from_file( geom.string_data[0:-3] + "obj") else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) continue # Turn a list of R,G,B elements (any indexable list of >= 3 # elements will work), where each element is specified on range # [0., 1.], into the equivalent 24-bit value 0xRRGGBB. def Rgb2Hex(rgb): val = 0 for i in range(3): val += (256**(2 - i)) * int(255 * rgb[i]) return val self.vis[self.prefix][source_name][str(link.robot_num)][ frame_name][str(j)]\ .set_object(meshcat_geom, meshcat.geometry .MeshLambertMaterial( color=Rgb2Hex(geom.color))) self.vis[self.prefix][source_name][str(link.robot_num)][ frame_name][str(j)].set_transform(element_local_tf)
def load(self): """ Loads `meshcat` visualization elements. @pre The `scene_graph` used to construct this object must be part of a fully constructed diagram (e.g. via `DiagramBuilder.Build()`). """ self.vis[self.prefix].delete() # Intercept load message via mock LCM. mock_lcm = DrakeMockLcm() DispatchLoadMessage(self._scene_graph, mock_lcm) load_robot_msg = lcmt_viewer_load_robot.decode( mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT")) # Translate elements to `meshcat`. for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] [source_name, frame_name] = link.name.split("::") for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currenly sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue element_local_tf = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsMatrix4() if geom.type == geom.BOX: assert geom.num_float_data == 3 meshcat_geom = meshcat.geometry.Box(geom.float_data) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0]) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 meshcat_geom = meshcat.geometry.Cylinder( geom.float_data[1], geom.float_data[0]) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi / 2., [1, 0, 0]) element_local_tf[0:3, 0:3] = \ element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3]) elif geom.type == geom.MESH: meshcat_geom = \ meshcat.geometry.ObjMeshGeometry.from_file( geom.string_data[0:-3] + "obj") else: print "UNSUPPORTED GEOMETRY TYPE ", \ geom.type, " IGNORED" continue # Turn a list of R,G,B elements (any indexable list of >= 3 # elements will work), where each element is specified on range # [0., 1.], into the equivalent 24-bit value 0xRRGGBB. def Rgb2Hex(rgb): val = 0 for i in range(3): val += (256**(2 - i)) * int(255 * rgb[i]) return val self.vis[self.prefix][source_name][str(link.robot_num)][ frame_name][str(j)]\ .set_object(meshcat_geom, meshcat.geometry .MeshLambertMaterial( color=Rgb2Hex(geom.color))) self.vis[self.prefix][source_name][str( link.robot_num)][frame_name][str(j)].set_transform( element_local_tf)
def _convert_mesh(geom): # Given a LCM geometry message, forms a meshcat geometry and material # for that geometry, as well as a local tf to the meshcat geometry # that matches the LCM geometry. # For MESH geometry, this function checks if a texture file exists next # to the mesh file, and uses that to provide the material information if # present. For BOX, SPHERE, CYLINDER geometry as well as MESH geometry # not satisfying the above, this function uses the geom.color field to # create a flat color for the object. In the case of other geometry types, # both fields are returned as None. meshcat_geom = None material = None element_local_tf = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsMatrix4() if geom.type == geom.BOX: assert geom.num_float_data == 3 meshcat_geom = meshcat.geometry.Box(geom.float_data) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0]) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 meshcat_geom = meshcat.geometry.Cylinder( geom.float_data[1], geom.float_data[0]) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi/2., [1, 0, 0]) element_local_tf[0:3, 0:3] = ( element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3])) elif geom.type == geom.MESH: meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file( geom.string_data[0:-3] + "obj") # Handle scaling. # TODO(gizatt): See meshcat-python#40 for incorporating scale as a # field rather than a matrix multiplication. scale = geom.float_data[:3] element_local_tf[:3, :3] = element_local_tf[:3, :3].dot(np.diag(scale)) # 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_png = geom.string_data[0:-3] + "png" if os.path.exists(candidate_texture_path_png): material = meshcat.geometry.MeshLambertMaterial( map=meshcat.geometry.ImageTexture( image=meshcat.geometry.PngImage.from_file( candidate_texture_path_png))) else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) return meshcat_geom, material if material is None: def rgb_2_hex(rgb): # Turn a list of R,G,B elements (any indexable list # of >= 3 elements will work), where each element is # specified on range [0., 1.], into the equivalent # 24-bit value 0xRRGGBB. val = 0 for i in range(3): val += (256**(2 - i)) * int(255 * rgb[i]) return val material = meshcat.geometry.MeshLambertMaterial( color=rgb_2_hex(geom.color[:3]), transparent=geom.color[3] != 1., opacity=geom.color[3]) return meshcat_geom, material, element_local_tf
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])) # 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 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 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]))
ray_direction = norm((1.0, 0.2, 0.2)) expected = ( (-0.9082895433880116, 0.41834209132239775, 0.2183420913223977), (0.5, 0.7, 0.5) ) surface_and_top = (ray_origin, ray_direction, expected) tests = (touching, end_caps_only, surface_and_bottom, surface_and_top) # Place cylinder in scene material = g.MeshLambertMaterial(reflectivity=1.0, sides=0) vis['cyl'].set_object( g.Cylinder(length, radius), material) vis['cyl'].set_transform( tf.translation_matrix([0.0, 0.0, 0.0]).dot( tf.rotation_matrix(np.radians(-90), [1, 0, 0])) ) # Visualise test rays for idx, (ray_origin, ray_direction, expected) in enumerate(tests): ray_inf = np.array(ray_origin) + 5.0 * np.array(ray_direction) vertices = np.column_stack((ray_origin, ray_inf)) red_material = g.MeshLambertMaterial( reflectivity=1.0, sides=0, color=0xff0000) vis['line_{}'.format(idx)].set_object(g.Line(g.PointsGeometry(vertices))) points = ray_z_cylinder(length, radius, ray_origin, ray_direction) for ptidx, pt in enumerate(points): vis['point_{}_{}'.format(idx, ptidx)].set_object( g.Sphere(0.05), red_material)
def PublishAllGeometry(self): n_bodies = self.rbtree.get_num_bodies() - 1 all_meshcat_geometry = {} for body_i in range(n_bodies): body = self.rbtree.get_body(body_i + 1) # TODO(gizatt) Replace these body-unique indices # with more readable body.get_model_name() or other # model index information when an appropriate # function gets bound in pydrake. body_name = body.get_name() + ("(%d)" % body_i) self.body_names.append(body_name) if self.old_mrbv is not None and \ body_name in self.old_mrbv.body_names: continue self.vis[self.prefix][body_name].delete() if self.draw_collision: draw_elements = [ self.rbtree.FindCollisionElement(k) for k in body.get_collision_element_ids() ] else: draw_elements = body.get_visual_elements() for element_i, element in enumerate(draw_elements): element_local_tf = element.getLocalTransform() if element.hasGeometry(): geom = element.getGeometry() geom_type = geom.getShape() if geom_type == Shape.SPHERE: meshcat_geom = meshcat.geometry.Sphere(geom.radius) elif geom_type == Shape.BOX: meshcat_geom = meshcat.geometry.Box(geom.size) elif geom_type == Shape.CYLINDER: meshcat_geom = meshcat.geometry.Cylinder( geom.length, geom.radius) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi / 2., [1, 0, 0]) element_local_tf[0:3, 0:3] = \ element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3]) elif geom_type == Shape.MESH: meshcat_geom = \ meshcat.geometry.ObjMeshGeometry.from_file( geom.resolved_filename[0:-3] + "obj") # respect mesh scale element_local_tf[0:3, 0:3] *= geom.scale else: print "UNSUPPORTED GEOMETRY TYPE ",\ geom.getShape(), " IGNORED" continue def rgba2hex(rgb): ''' Turn a list of R,G,B elements (any indexable list of >= 3 elements will work), where each element is specified on range [0., 1.], into the equivalent 24-bit value 0xRRGGBB. ''' val = 0 for i in range(3): val += (256**(2 - i)) * int(255 * rgb[i]) return val rgba = [1., 0.7, 0., 1.] if not self.draw_collision: rgba = element.getMaterial() # Publish this object to the visualizer # if we have been instructed to reset bodies # with this name, or if the vis has no record # of this body. self.vis[self.prefix][body_name][str(element_i)]\ .set_object(meshcat_geom, meshcat.geometry.MeshLambertMaterial( color=rgba2hex(rgba))) self.vis[self.prefix][body_name][str(element_i)].\ set_transform(element_local_tf) # Finally, nuke any bodies that used to exist that don't exist # now. if self.old_mrbv is not None: for body_name in self.old_mrbv.body_names: if body_name not in self.body_names: self.vis[self.prefix][body_name].delete()
def _add_rbt_geometry(self, rbt, rbt_key, draw_collision=False): # type: (RigidBodyTree, str, bool) -> None """ Add the geometry of a RigidBodyTree to the meshcat visualizer. Note that later update to rbt WOULD NOT be reflected :param rbt: :param rbt_key: The key to index the rigid body tree :param draw_collision: :return: """ n_bodies = rbt.get_num_bodies() - 1 # all_meshcat_geometry = {} for body_i in range(n_bodies): # Get the body index body = rbt.get_body(body_i + 1) body_name = rbt_key + body.get_name() + ("(%d)" % body_i) if draw_collision: draw_elements = [rbt.FindCollisionElement(k) for k in body.get_collision_element_ids()] else: draw_elements = body.get_visual_elements() for element_i, element in enumerate(draw_elements): element_local_tf = element.getLocalTransform() if element.hasGeometry(): geom = element.getGeometry() geom_type = geom.getShape() if geom_type == Shape.SPHERE: meshcat_geom = meshcat.geometry.Sphere(geom.radius) elif geom_type == Shape.BOX: meshcat_geom = meshcat.geometry.Box(geom.size) elif geom_type == Shape.CYLINDER: meshcat_geom = meshcat.geometry.Cylinder( geom.length, geom.radius) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi / 2., [1, 0, 0]) element_local_tf[0:3, 0:3] = \ element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3]) elif geom_type == Shape.MESH: meshcat_geom = \ meshcat.geometry.ObjMeshGeometry.from_file( geom.resolved_filename[0:-3] + "obj") # respect mesh scale element_local_tf[0:3, 0:3] *= geom.scale elif geom_type == Shape.MESH_POINTS: # The shape of points is (3, N) points = geom.getPoints() # type: np.ndarray n_points = points.shape[1] color = MeshCatVisualizer._make_meshcat_color(n_points, r=255, g=0, b=0) meshcat_geom = meshcat_geometry.PointCloud(points, color.T, size=0.01) self._meshcat_vis[self._prefix][body_name][str(element_i)] \ .set_object(meshcat_geom) self._meshcat_vis[self._prefix][body_name][str(element_i)]. \ set_transform(element_local_tf) self._rbt_geometry_element_set.add(body_name) continue else: print "UNSUPPORTED GEOMETRY TYPE ", \ geom.getShape(), " IGNORED" continue # The color of the body rgba = [1., 0.7, 0., 1.] if not draw_collision: rgba = element.getMaterial() self._meshcat_vis[self._prefix][body_name][str(element_i)] \ .set_object(meshcat_geom, meshcat.geometry.MeshLambertMaterial( color=MeshCatVisualizer.rgba2hex(rgba))) self._meshcat_vis[self._prefix][body_name][str(element_i)]. \ set_transform(element_local_tf) self._rbt_geometry_element_set.add(body_name)
def _convert_mesh(geom): # Given a LCM geometry message, forms a meshcat geometry and material # for that geometry, as well as a local tf to the meshcat geometry # that matches the LCM geometry. # For MESH geometry, this function checks if a texture file exists next # to the mesh file, and uses that to provide the material information if # present. For BOX, SPHERE, CYLINDER geometry as well as MESH geometry # not satisfying the above, this function uses the geom.color field to # create a flat color for the object. In the case of other geometry types, # both fields are returned as None. meshcat_geom = None material = None element_local_tf = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsMatrix4() if geom.type == geom.BOX: assert geom.num_float_data == 3 meshcat_geom = meshcat.geometry.Box(geom.float_data) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0]) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 meshcat_geom = meshcat.geometry.Cylinder( geom.float_data[1], geom.float_data[0]) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi/2., [1, 0, 0]) element_local_tf[0:3, 0:3] = ( element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3])) elif geom.type == geom.MESH: meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file( geom.string_data[0:-3] + "obj") # Handle scaling. # TODO(gizatt): See meshcat-python#40 for incorporating scale as a # field rather than a matrix multiplication. scale = geom.float_data[:3] element_local_tf[:3, :3] = element_local_tf[:3, :3].dot(np.diag(scale)) # 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_png = geom.string_data[0:-3] + "png" if os.path.exists(candidate_texture_path_png): material = meshcat.geometry.MeshLambertMaterial( map=meshcat.geometry.ImageTexture( image=meshcat.geometry.PngImage.from_file( candidate_texture_path_png))) else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) return meshcat_geom, material if material is None: def rgb_2_hex(rgb): # Turn a list of R,G,B elements (any indexable list # of >= 3 elements will work), where each element is # specified on range [0., 1.], into the equivalent # 24-bit value 0xRRGGBB. val = 0 for i in range(3): val += (256**(2 - i)) * int(255 * rgb[i]) return val material = meshcat.geometry.MeshLambertMaterial( color=rgb_2_hex(geom.color[:3]), transparent=geom.color[3] != 1., opacity=geom.color[3]) return meshcat_geom, material, element_local_tf
light_attenuations = [40.0, 40.0, 40.0, 40.0] else: raise NotImplementedError("Rotating light pattern_name %s" % pattern_name) return light_locations, light_attenuations if __name__ == "__main__": logging.basicConfig(filename='../out/info.log', filemode='w', level=logging.DEBUG) # First connect to meshcat: this'll block if there's no server + display a helpful # reminder for me to start a server. print("Opening meshcat server at default url: make sure meshcat-server is running.") vis = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000") vis.delete() camera_frame_vis = vis["camera_frame"] # Makes point cloud look "upright" in meshcat vis camera_frame_vis.set_transform(tf.rotation_matrix(np.pi/2, [1., 0., 0])) meshcat_draw_frustrum(camera_frame_vis, TF=get_extrinsics(), K=get_projector_intrinsics(), near_distance=0.025, far_distance=1., w=1280, h=768 ) modes = [ "Off", "Depth recoloring", "Normal recoloring",