def load(self): """ Loads ``meshcat`` visualization elements. Precondition: 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] = self._parse_name(link.name) for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue meshcat_geom, material, element_local_tf = _convert_mesh(geom) if meshcat_geom is not None: cur_vis = (self.vis[self.prefix][source_name][str( link.robot_num)][frame_name][str(j)]) cur_vis.set_object(meshcat_geom, material) cur_vis.set_transform(element_local_tf)
def get_box_from_geom(scene_graph, visual_only=True): # TODO: GeometryInstance, InternalGeometry, & GeometryContext to get the shape of objects # TODO: get_contact_results_output_port # https://github.com/RussTedrake/underactuated/blob/master/src/underactuated/meshcat_visualizer.py # https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_viewer_draw.lcm mock_lcm = DrakeMockLcm() DispatchLoadMessage(scene_graph, mock_lcm) load_robot_msg = lcmt_viewer_load_robot.decode( mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT")) # 'link', 'num_links' #builder.Connect(scene_graph.get_pose_bundle_output_port(), # viz.get_input_port(0)) # TODO: hash bodies instead box_from_geom = {} for body_index in range(load_robot_msg.num_links): # 'geom', 'name', 'num_geom', 'robot_num' link = load_robot_msg.link[body_index] [source_name, frame_name] = link.name.split("::") # source_name = 'Source_1' model_index = link.robot_num visual_index = 0 for geom in sorted(link.geom, key=lambda g: g.position[::-1]): # sort by z, y, x # 'color', 'float_data', 'num_float_data', 'position', 'quaternion', 'string_data', 'type' # string_data is empty... if visual_only and (geom.color[3] == 0): continue # TODO: sort by lowest point on the bounding box? # TODO: maybe just return the set of bodies in order and let the user decide what to with them visual_index += 1 # TODO: affected by transparent visual if geom.type == geom.BOX: assert geom.num_float_data == 3 [width, length, height] = geom.float_data extent = np.array([width, length, height]) / 2. elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 [radius] = geom.float_data extent = np.array([radius, radius, radius]) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 [radius, height] = geom.float_data extent = np.array([radius, radius, height/2.]) #meshcat_geom = meshcat.geometry.Cylinder( # geom.float_data[1], geom.float_data[0]) # In Drake, cylinders are along +z #elif geom.type == geom.MESH: # meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file( # geom.string_data[0:-3] + "obj") else: #print("Robot {}, link {}, geometry {}: UNSUPPORTED GEOMETRY TYPE {} WAS IGNORED".format( # model_index, frame_name, visual_index-1, geom.type)) continue link_from_box = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsIsometry3() #.GetAsMatrix4() box_from_geom[model_index, frame_name, visual_index-1] = \ (BoundingBox(np.zeros(3), extent), link_from_box, get_geom_name(geom)) return box_from_geom
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--target_realtime_rate", type=float, default=1.0, help="Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details.") parser.add_argument("--simulation_time", type=float, default=10.0, help="Desired duration of the simulation in seconds.") parser.add_argument( "--time_step", type=float, default=0., help="If greater than zero, the plant is modeled as a system with " "discrete updates and period equal to this time_step. " "If 0, the plant is modeled as a continuous system.") args = parser.parse_args() file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step)) AddModelFromSdfFile(file_name=file_name, plant=cart_pole, scene_graph=scene_graph) cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) cart_pole.Finalize(scene_graph) assert cart_pole.geometry_source_is_registered() builder.Connect( cart_pole.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(cart_pole.get_source_id())) lcm = DrakeLcm() ConnectVisualization(scene_graph=scene_graph, builder=builder, lcm=lcm) diagram = builder.Build() DispatchLoadMessage(scene_graph=scene_graph, lcm=lcm) diagram_context = diagram.CreateDefaultContext() cart_pole_context = diagram.GetMutableSubsystemContext( cart_pole, diagram_context) cart_pole_context.FixInputPort( cart_pole.get_actuation_input_port().get_index(), [0]) cart_slider = cart_pole.GetJointByName("CartSlider") pole_pin = cart_pole.GetJointByName("PolePin") cart_slider.set_translation(context=cart_pole_context, translation=0.) pole_pin.set_angle(context=cart_pole_context, angle=2.) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.Initialize() simulator.StepTo(args.simulation_time)
def load(self): """ Loads ``meshcat`` visualization elements. Precondition: 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() mock_lcm_subscriber = Subscriber( lcm=mock_lcm, channel="DRAKE_VIEWER_LOAD_ROBOT", lcm_type=lcmt_viewer_load_robot) DispatchLoadMessage(self._scene_graph, mock_lcm) mock_lcm.HandleSubscriptions(0) assert mock_lcm_subscriber.count > 0 load_robot_msg = mock_lcm_subscriber.message # Translate elements to `meshcat`. for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] [source_name, frame_name] = self._parse_name(link.name) for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue meshcat_geom, material, element_local_tf = _convert_mesh(geom) if meshcat_geom is not None: cur_vis = ( self.vis[self.prefix][source_name][str(link.robot_num)] [frame_name][str(j)]) cur_vis.set_object(meshcat_geom, material) cur_vis.set_transform(element_local_tf) # Draw the frames in self.frames_to_draw. if "::" in frame_name: robot_name, link_name = self._parse_name(frame_name) else: robot_name = "world" link_name = frame_name if (robot_name in self.frames_to_draw.keys() and link_name in self.frames_to_draw[robot_name]): prefix = (self.prefix + '/' + source_name + '/' + str(link.robot_num) + '/' + frame_name) AddTriad( self.vis, name="frame", prefix=prefix, length=self.axis_length, radius=self.axis_radius, opacity=self.frames_opacity)
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 get_box_from_geom(scene_graph, visual_only=True): # https://github.com/RussTedrake/underactuated/blob/master/src/underactuated/meshcat_visualizer.py # https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_viewer_draw.lcm mock_lcm = DrakeMockLcm() DispatchLoadMessage(scene_graph, mock_lcm) load_robot_msg = lcmt_viewer_load_robot.decode( mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT")) box_from_geom = {} for body_index in range(load_robot_msg.num_links): # 'geom', 'name', 'num_geom', 'robot_num' link = load_robot_msg.link[body_index] [source_name, frame_name] = link.name.split("::") model_index = link.robot_num visual_index = 0 for geom in sorted(link.geom, key=lambda g: g.position[::-1]): # sort by z, y, x # 'color', 'float_data', 'num_float_data', 'position', 'quaternion', 'string_data', 'type' if visual_only and (geom.color[3] == 0): continue visual_index += 1 if geom.type == geom.BOX: assert geom.num_float_data == 3 [width, length, height] = geom.float_data extent = np.array([width, length, height]) / 2. elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 [radius] = geom.float_data extent = np.array([radius, radius, radius]) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 [radius, height] = geom.float_data extent = np.array([radius, radius, height / 2.]) # In Drake, cylinders are along +z else: continue link_from_box = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsIsometry3() box_from_geom[model_index, frame_name, visual_index-1] = \ (BoundingBox(np.zeros(3), extent), link_from_box, get_geom_name(geom)) return box_from_geom
def _build_body_patches(self, use_random_colors, substitute_collocated_mesh_files): """ 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 = {} mock_lcm = DrakeMockLcm() mock_lcm_subscriber = Subscriber(lcm=mock_lcm, channel="DRAKE_VIEWER_LOAD_ROBOT", lcm_type=lcmt_viewer_load_robot) # TODO(SeanCurtis-TRI): Use SceneGraph inspection instead of mocking # LCM and inspecting the generated message. DispatchLoadMessage(self._scene_graph, mock_lcm) mock_lcm.HandleSubscriptions(0) assert mock_lcm_subscriber.count > 0 load_robot_msg = mock_lcm_subscriber.message # Spawn a random color generator, in case we need to pick random colors # for some bodies. Each body will be given a unique color when using # this random generator, with each visual element of the body colored # the same. color = iter( plt.cm.rainbow(np.linspace(0, 1, load_robot_msg.num_links))) for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] this_body_patches = [] this_body_colors = [] this_color = next(color) for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue X_BG = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position) if geom.type == geom.BOX: assert geom.num_float_data == 3 # Draw a bounding box. patch_G = np.vstack( (geom.float_data[0] / 2. * np.array([-1, -1, 1, 1, -1, -1, 1, 1]), geom.float_data[1] / 2. * np.array([-1, 1, -1, 1, -1, 1, -1, 1]), geom.float_data[2] / 2. * np.array([-1, -1, -1, -1, 1, 1, 1, 1]))) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 radius = geom.float_data[0] 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 *= radius elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 radius = geom.float_data[0] length = geom.float_data[1] # 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 geom.type == geom.MESH: filename = geom.string_data base, ext = os.path.splitext(filename) if (ext.lower() is not ".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("The given file " + filename + " is not " "supported and no alternate " + base + ".obj could be found.") if not os.path.exists(filename): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), filename) mesh = ReadObjToSurfaceMesh(filename) patch_G = np.vstack([v.r_MV() for v in mesh.vertices()]).T else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) 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 use_random_colors: this_body_colors.append(this_color) else: this_body_colors.append(geom.color) self._patch_Blist[link.name] = this_body_patches self._patch_Blist_colors[link.name] = this_body_colors
def buildViewPatches(self, use_random_colors): ''' Generates view patches. self.viewPatches stores a list of viewPatches for each body (starting at body id 1). A viewPatch is a list of all 3D vertices of a piece of visual geometry. ''' self.viewPatches = {} self.viewPatchColors = {} mock_lcm = DrakeMockLcm() mock_lcm_subscriber = Subscriber(lcm=mock_lcm, channel="DRAKE_VIEWER_LOAD_ROBOT", lcm_type=lcmt_viewer_load_robot) DispatchLoadMessage(self._scene_graph, mock_lcm) mock_lcm.HandleSubscriptions(0) assert mock_lcm_subscriber.count > 0 load_robot_msg = mock_lcm_subscriber.message # Spawn a random color generator, in case we need to pick # random colors for some bodies. Each body will be given # a unique color when using this random generator, with # each visual element of the body colored the same. color = iter( plt.cm.rainbow(np.linspace(0, 1, load_robot_msg.num_links))) for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] this_body_patches = [] this_body_colors = [] this_color = next(color) for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently 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) if geom.type == geom.BOX: assert geom.num_float_data == 3 # Draw a bounding box. patch = np.vstack((geom.float_data[0] / 2. * np.array([-1, -1, 1, 1, -1, -1, 1, 1]), geom.float_data[1] / 2. * np.array([-1, 1, -1, 1, -1, 1, -1, 1]), geom.float_data[2] / 2. * np.array([-1, -1, -1, -1, 1, 1, 1, 1]))) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 radius = geom.float_data[0] 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 = np.vstack([ np.sin(longi) * np.cos(lati), np.sin(longi) * np.sin(lati), np.cos(lati) ]) patch *= radius elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 radius = geom.float_data[0] length = geom.float_data[1] # 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 = 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 geom.type == geom.MESH: # TODO(gizatt): Remove trimesh and shapely dependency when # vertex information is accessible from the SceneGraph # interface. mesh = trimesh.load(geom.string_data) patch = mesh.vertices.T # Apply scaling for i in range(3): patch[i, :] *= geom.float_data[i] else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) continue patch = np.vstack((patch, np.ones((1, patch.shape[1])))) patch = np.dot(element_local_tf.GetAsMatrix4(), patch) # Close path if not closed if (patch[:, -1] != patch[:, 0]).any(): patch = np.hstack((patch, patch[:, 0][np.newaxis].T)) this_body_patches.append(patch) if use_random_colors: this_body_colors.append(this_color) else: this_body_colors.append(geom.color) self.viewPatches[link.name] = this_body_patches self.viewPatchColors[link.name] = this_body_colors
def add_drake_visualizer(scene_graph, builder): lcm = DrakeLcm() ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph, lcm=lcm) DispatchLoadMessage(scene_graph, lcm) # TODO: only update viewer after a plan is found return lcm # Important that variable is saved
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 load1(self): """ Loads all visualization elements in the Blender server. @pre The `scene_graph` used to construct this object must be part of a fully constructed diagram (e.g. via `DiagramBuilder.Build()`). """ self.bsi.send_remote_call("initialize_scene") # Keeps track of registered bounding boxes, to keep us from # having to register + delete brand new objects every cycle. # If more than this number is needed in a given cycle, # more are registered and this number is increased. # If less are needed, the unused ones are made invisible. # The attributes of all bounding boxes are updated every cycle. self.num_registered_bounding_boxes = 0 self.num_visible_bounding_boxes = 0 # Intercept load message via mock LCM. mock_lcm = DrakeMockLcm() mock_lcm_subscriber = Subscriber(lcm=mock_lcm, channel="DRAKE_VIEWER_LOAD_ROBOT", lcm_type=lcmt_viewer_load_robot) # warnings.filterwarnings("ignore", message="(Advanced) Explicitly dispatches an LCM load message") warnings.simplefilter("ignore", DrakeDeprecationWarning) DispatchLoadMessage(self._scene_graph, mock_lcm) # SendLoadMessage() TODO mock_lcm.HandleSubscriptions(0) assert mock_lcm_subscriber.count > 0 load_robot_msg = mock_lcm_subscriber.message # Load all the elements over on the Blender side. self.num_link_geometries_by_link_name = {} self.link_subgeometry_local_tfs_by_link_name = {} for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] [source_name, frame_name] = self._parse_name(link.name) print('frame_name', frame_name) print('link.name', link.name) self.num_link_geometries_by_link_name[link.name] = link.num_geom tfs = [] for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue geom_name = self._format_geom_name(source_name, frame_name, j) tfs.append( RigidTransform(RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsMatrix4()) # It will have a material with this key. # We will set it up after deciding whether it's # a mesh with a texture or not... material_key = "material_" + geom_name material_key_assigned = False # Check overrides for override in self.material_overrides: if override[0].match(geom_name): print("Using override ", override[0].pattern, " on name ", geom_name, " with applied args ", override[1]) self.bsi.send_remote_call("register_material", name=material_key, **(override[1])) material_key_assigned = True if geom.type == geom.BOX: assert geom.num_float_data == 3 # Blender cubes are 2x2x2 by default do_load_geom = lambda: self.bsi.send_remote_call( "register_object", name="obj_" + geom_name, type="cube", scale=[x * 0.5 for x in geom.float_data[:3]], location=geom.position, quaternion=geom.quaternion, material=material_key) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 do_load_geom = lambda: self.bsi.send_remote_call( "register_object", name="obj_" + geom_name, type="sphere", scale=geom.float_data[0], location=geom.position, quaternion=geom.quaternion, material=material_key) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 # Blender cylinders are r=1, h=2 by default do_load_geom = lambda: self.bsi.send_remote_call( "register_object", name="obj_" + geom_name, type="cylinder", scale=[ geom.float_data[0], geom.float_data[0], 0.5 * geom. float_data[1] ], location=geom.position, quaternion=geom.quaternion, material=material_key) elif geom.type == geom.MESH: print('geom_name', geom_name) print('path', geom.string_data[0:-3]) print('scale', geom.float_data[:3]) print('loc', geom.position) print('quat', geom.quaternion) print('material_key', material_key) do_load_geom = lambda: self.bsi.send_remote_call( "register_object", name="obj_" + geom_name, type="obj", path=geom.string_data[0:-3] + "obj", scale=geom.float_data[:3], location=geom.position, quaternion=geom.quaternion, material=material_key) # Attempt to find a texture for the object by looking for an # identically-named *.png next to the model. # 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 not material_key_assigned and os.path.exists( candidate_texture_path_png): material_key_assigned = self.bsi.send_remote_call( "register_material", name=material_key, material_type="color_texture", path=candidate_texture_path_png) else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) continue if not material_key_assigned: material_key_assigned = self.bsi.send_remote_call( "register_material", name=material_key, material_type="color", color=geom.color[:4]) print('color', geom.color[:4]) # Finally actually load the geometry now that the material # is registered. do_load_geom() self.link_subgeometry_local_tfs_by_link_name[link.name] = tfs for i, camera_tf in enumerate(self.camera_tfs): camera_tf_post = self.global_transform.multiply(camera_tf) self.bsi.send_remote_call( "register_camera", name="cam_%d" % i, location=camera_tf_post.translation().tolist(), quaternion=camera_tf_post.quaternion().wxyz().tolist(), angle=np.pi / 2.) self.bsi.send_remote_call("configure_rendering", camera_name='cam_%d' % i, resolution=[640, 480], file_format="JPEG", taa_render_samples=20, cycles=True) if self.env_map_path: self.bsi.send_remote_call("set_environment_map", path=self.env_map_path)
def load(self): """ Loads all visualization elements in the Blender server. @pre The `scene_graph` used to construct this object must be part of a fully constructed diagram (e.g. via `DiagramBuilder.Build()`). """ self.bsi.send_remote_call("initialize_scene") # Intercept load message via mock LCM. mock_lcm = DrakeMockLcm() mock_lcm_subscriber = Subscriber(lcm=mock_lcm, channel="DRAKE_VIEWER_LOAD_ROBOT", lcm_type=lcmt_viewer_load_robot) DispatchLoadMessage(self._scene_graph, mock_lcm) mock_lcm.HandleSubscriptions(0) assert mock_lcm_subscriber.count > 0 load_robot_msg = mock_lcm_subscriber.message # Load all the elements over on the Blender side. self.num_link_geometries_by_link_name = {} self.link_subgeometry_local_tfs_by_link_name = {} self.geom_name_to_color_map = {} num_allocated_labels = 0 max_num_objs = sum([ load_robot_msg.link[i].num_geom for i in range(load_robot_msg.num_links) ]) cmap = plt.cm.get_cmap("hsv", max_num_objs + 1) for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] [source_name, frame_name] = self._parse_name(link.name) self.num_link_geometries_by_link_name[link.name] = link.num_geom tfs = [] for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue geom_name = self._format_geom_name(source_name, frame_name, link.robot_num, j) tfs.append( RigidTransform(RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsMatrix4()) # It will have a material with this key. material_key = "material_" + geom_name label_num = num_allocated_labels self.geom_name_to_color_map[geom_name] = cmap(label_num) num_allocated_labels += 1 material_key_assigned = self.bsi.send_remote_call( "register_material", name=material_key, material_type="emission", color=cmap(label_num)) if geom.type == geom.BOX: assert geom.num_float_data == 3 # Blender cubes are 2x2x2 by default self.bsi.send_remote_call( "register_object", name="obj_" + geom_name, type="cube", scale=[x * 0.5 for x in geom.float_data[:3]], location=geom.position, quaternion=geom.quaternion, material=material_key) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 self.bsi.send_remote_call("register_object", name="obj_" + geom_name, type="sphere", scale=geom.float_data[0], location=geom.position, quaternion=geom.quaternion, material=material_key) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 # Blender cylinders are r=1, h=2 by default self.bsi.send_remote_call("register_object", name="obj_" + geom_name, type="cylinder", scale=[ geom.float_data[0], geom.float_data[0], 0.5 * geom.float_data[1] ], location=geom.position, quaternion=geom.quaternion, material=material_key) elif geom.type == geom.MESH: self.bsi.send_remote_call("register_object", name="obj_" + geom_name, type="obj", path=geom.string_data[0:-3] + "obj", scale=geom.float_data[:3], location=geom.position, quaternion=geom.quaternion, material=material_key) else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) continue self.link_subgeometry_local_tfs_by_link_name[link.name] = tfs for i, camera_tf in enumerate(self.camera_tfs): camera_tf_post = self.global_transform.multiply(camera_tf) self.bsi.send_remote_call( "register_camera", name="cam_%d" % i, location=camera_tf_post.translation().tolist(), quaternion=camera_tf_post.rotation().ToQuaternion().wxyz( ).tolist(), angle=np.pi / 2.) self.bsi.send_remote_call("configure_rendering", camera_name='cam_%d' % i, resolution=[640, 480], file_format="BMP", configure_for_masks=True, taa_render_samples=10, cycles=False) self.bsi.send_remote_call("set_environment_map", path=None)
def load(self): """ Loads ``meshcat`` visualization elements. Precondition: The ``scene_graph`` used to construct this object must be part of a fully constructed diagram (e.g. via ``DiagramBuilder.Build()``). """ if self._delete_prefix_on_load: self.vis[self.prefix].delete() # Intercept load message via memq LCM. memq_lcm = DrakeLcm("memq://") memq_lcm_subscriber = Subscriber(lcm=memq_lcm, channel="DRAKE_VIEWER_LOAD_ROBOT", lcm_type=lcmt_viewer_load_robot) DispatchLoadMessage(self._scene_graph, memq_lcm) memq_lcm.HandleSubscriptions(0) assert memq_lcm_subscriber.count > 0 load_robot_msg = memq_lcm_subscriber.message # Translate elements to `meshcat`. for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] [_, frame_name] = self._parse_name(link.name) for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently sets alpha=0 to make collision # geometry "invisible". Ignore those geometries here. if geom.color[3] == 0: continue meshcat_geom, material, element_local_tf = _convert_mesh(geom) if meshcat_geom is not None: cur_vis = (self.vis[self.prefix][str( link.robot_num)][frame_name][str(j)]) # 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. if isinstance(meshcat_geom, meshcat.geometry.MeshGeometry): meshcat_geom.uuid = str( uuid.uuid5(uuid.NAMESPACE_X500, meshcat_geom.contents)) material.uuid = str( uuid.uuid5(uuid.NAMESPACE_X500, meshcat_geom.contents + "material")) mesh = meshcat.geometry.Mesh(meshcat_geom, material) mesh.uuid = str( uuid.uuid5(uuid.NAMESPACE_X500, meshcat_geom.contents + "mesh")) cur_vis.set_object(mesh) else: cur_vis.set_object(meshcat_geom, material) cur_vis.set_transform(element_local_tf) # Draw the frames in self.frames_to_draw. if "::" in frame_name: robot_name, link_name = self._parse_name(frame_name) else: robot_name = "world" link_name = frame_name if (robot_name in self.frames_to_draw.keys() and link_name in self.frames_to_draw[robot_name]): prefix = (self.prefix + '/' + str(link.robot_num) + '/' + frame_name) AddTriad(self.vis, name="frame", prefix=prefix, length=self.axis_length, radius=self.axis_radius, opacity=self.frames_opacity)
def buildViewPatches(self, use_random_colors): ''' Generates view patches. self.viewPatches stores a list of viewPatches for each body (starting at body id 1). A viewPatch is a list of 2D coordinates in counterclockwise order forming the boundary of a filled polygon representing a piece of visual geometry. ''' self.viewPatches = {} self.viewPatchColors = {} mock_lcm = DrakeMockLcm() mock_lcm_subscriber = Subscriber(lcm=mock_lcm, channel="DRAKE_VIEWER_LOAD_ROBOT", lcm_type=lcmt_viewer_load_robot) DispatchLoadMessage(self._scene_graph, mock_lcm) mock_lcm.HandleSubscriptions(0) assert mock_lcm_subscriber.count > 0 load_robot_msg = mock_lcm_subscriber.message # Spawn a random color generator, in case we need to pick # random colors for some bodies. Each body will be given # a unique color when using this random generator, with # each visual element of the body colored the same. color = iter(plt.cm.rainbow(np.linspace(0, 1, load_robot_msg.num_links))) for i in range(load_robot_msg.num_links): link = load_robot_msg.link[i] this_body_patches = [] this_body_colors = [] this_color = next(color) for j in range(link.num_geom): geom = link.geom[j] # MultibodyPlant currently 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) if geom.type == geom.BOX: assert geom.num_float_data == 3 # Draw a bounding box. patch = np.vstack(( geom.float_data[0]/2.*np.array([1, 1, 1, 1, -1, -1, -1, -1]), geom.float_data[1]/2.*np.array([1, 1, 1, 1, -1, -1, -1, -1]), geom.float_data[2]/2.*np.array([1, 1, -1, -1, -1, -1, 1, 1]))) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 radius = geom.float_data[0] sample_pts = np.arange(0., 2.*math.pi, 0.25) patch = np.vstack([math.cos(pt)*self.Tview[0, 0:3] + math.sin(pt)*self.Tview[1, 0:3] for pt in sample_pts]) patch = np.transpose(patch) patch *= radius elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 radius = geom.float_data[0] length = geom.float_data[1] # 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 # I don't have access to the body to world transform # yet; decide between drawing a box and circle assuming the # T_body_to_world is will not rotate us out of the # viewing plane. z_axis = np.matmul(self.Tview[0:2, 0:3], element_local_tf.multiply([0, 0, 1])) if np.linalg.norm(z_axis) < 0.01: # Draw a circle. sample_pts = np.arange(0., 2.*math.pi, 0.25) patch = np.vstack([math.cos(pt)*self.Tview[0, 0:3] + math.sin(pt)*self.Tview[1, 0:3] for pt in sample_pts]) patch = np.transpose(patch) patch *= radius else: # Draw a bounding box. patch = np.vstack(( radius*np.array([1, 1, 1, 1, -1, -1, -1, -1]), radius*np.array([1, 1, 1, 1, -1, -1, -1, -1]), (length/2)*np.array([1, 1, -1, -1, -1, -1, 1, 1]))) else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) continue patch = np.vstack((patch, np.ones((1, patch.shape[1])))) patch = np.dot(element_local_tf.GetAsMatrix4(), patch) # Project into 2D patch = np.dot(self.Tview, patch) # Close path if not closed if (patch[:, -1] != patch[:, 0]).any(): patch = np.hstack((patch, patch[:, 0][np.newaxis].T)) this_body_patches.append(patch) if use_random_colors: this_body_colors.append(this_color) else: this_body_colors.append(geom.color) self.viewPatches[link.name] = this_body_patches self.viewPatchColors[link.name] = this_body_colors