def test_connect_drake_visualizer(self): # Test visualization API. # Use a mockable so that we can make a smoke test without side # effects. T = float SceneGraph = mut.SceneGraph_[T] DiagramBuilder = DiagramBuilder_[T] Simulator = Simulator_[T] lcm = DrakeMockLcm() role = mut.Role.kIllustration test_prefix = "TEST_PREFIX_" def normal(builder, scene_graph): mut.ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph, lcm=lcm, role=role) mut.DispatchLoadMessage(scene_graph=scene_graph, lcm=lcm, role=role) def port(builder, scene_graph): mut.ConnectDrakeVisualizer( builder=builder, scene_graph=scene_graph, pose_bundle_output_port=( scene_graph.get_pose_bundle_output_port()), lcm=lcm, role=role) mut.DispatchLoadMessage(scene_graph=scene_graph, lcm=lcm, role=role) for func in [normal, port]: # Create subscribers. load_channel = "DRAKE_VIEWER_LOAD_ROBOT" draw_channel = "DRAKE_VIEWER_DRAW" load_subscriber = Subscriber(lcm, load_channel, lcmt_viewer_load_robot) draw_subscriber = Subscriber(lcm, draw_channel, lcmt_viewer_draw) # Test sequence. builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) # Only load will be published by `DispatchLoadMessage`. func(builder, scene_graph) lcm.HandleSubscriptions(0) self.assertEqual(load_subscriber.count, 1) self.assertEqual(draw_subscriber.count, 0) diagram = builder.Build() # Load and draw will be published. Simulator(diagram).Initialize() lcm.HandleSubscriptions(0) self.assertEqual(load_subscriber.count, 2) self.assertEqual(draw_subscriber.count, 1)
def test_mock_lcm_roundtrip(self): dut = DrakeMockLcm() self.assertIsInstance(dut, DrakeLcmInterface) dut.Subscribe(channel="TEST_CHANNEL", handler=self._handler) dut.Publish(channel="TEST_CHANNEL", buffer=self.quat.encode()) self.assertEqual(self.count, 0) dut.HandleSubscriptions(0) self.assertEqual(self.count, 1)
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 test_subscriber(self): lcm = DrakeMockLcm() dut = mut.LcmSubscriberSystem.Make(channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm) model_message = self._model_message() lcm.Publish(channel="TEST_CHANNEL", buffer=model_message.encode()) lcm.HandleSubscriptions(0) actual_message = self._calc_output(dut).get_value() self.assert_lcm_equal(actual_message, model_message)
def test_subscriber(self): lcm = DrakeMockLcm() dut = mut.LcmSubscriberSystem.Make( channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm) model_message = self._model_message() lcm.Publish(channel="TEST_CHANNEL", buffer=model_message.encode()) lcm.HandleSubscriptions(0) context = self._process_event(dut) actual_message = dut.get_output_port(0).Eval(context) self.assert_lcm_equal(actual_message, model_message)
def test_publisher(self): lcm = DrakeMockLcm() dut = mut.LcmPublisherSystem.Make( channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm, publish_period=0.1) subscriber = Subscriber(lcm, "TEST_CHANNEL", quaternion_t) model_message = self._model_message() self._fix_and_publish(dut, AbstractValue.Make(model_message)) lcm.HandleSubscriptions(0) self.assert_lcm_equal(subscriber.message, model_message)
def test_publisher_cpp(self): lcm = DrakeMockLcm() dut = mut.LcmPublisherSystem.Make( channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm, use_cpp_serializer=True) subscriber = Subscriber(lcm, "TEST_CHANNEL", quaternion_t) model_message = self._model_message() model_value = self._model_value_cpp() self._fix_and_publish(dut, model_value) lcm.HandleSubscriptions(0) self.assert_lcm_equal(subscriber.message, model_message)
def test_subscriber(self): mock = DrakeMockLcm() dut = Subscriber(lcm=mock, channel="CHANNEL", lcm_type=quaternion_t) mock.Publish(channel="CHANNEL", buffer=self.quat.encode()) self.assertEqual(dut.count, 0) self.assertEqual(len(dut.raw), 0) self.assertEqual(dut.message.w, 0) self.assertEqual(dut.message.x, 0) self.assertEqual(dut.message.y, 0) self.assertEqual(dut.message.z, 0) mock.HandleSubscriptions(0) self.assertEqual(dut.count, 1) self.assertEqual(dut.raw, self.quat.encode()) self.assertEqual(dut.message.w, self.quat.w) self.assertEqual(dut.message.x, self.quat.x) self.assertEqual(dut.message.y, self.quat.y) self.assertEqual(dut.message.z, self.quat.z)
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. def handle_load_robot(raw): load_robot_msgs.append(lcmt_viewer_load_robot.decode(raw)) load_robot_msgs = [] mock_lcm = DrakeMockLcm() mock_lcm.Subscribe( channel="DRAKE_VIEWER_LOAD_ROBOT", handler=handle_load_robot) DispatchLoadMessage(self._scene_graph, mock_lcm) mock_lcm.HandleSubscriptions(0) load_robot_msg = load_robot_msgs[0] # 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 _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 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 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