def test_idm_controller(self): rg = make_two_lane_road() idm = IdmController( road=rg, path_or_branches=ScanStrategy.kPath, road_position_strategy=RoadPositionStrategy.kExhaustiveSearch, period_sec=0.) context = idm.CreateDefaultContext() output = idm.AllocateOutput() # Fix the inputs. pose_vector1 = PoseVector() pose_vector1.set_translation([1., 2., 3.]) ego_pose_index = idm.ego_pose_input().get_index() context.FixInputPort(ego_pose_index, pose_vector1) w = [0., 0., 0.] v = [1., 0., 0.] frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v)) ego_velocity_index = idm.ego_velocity_input().get_index() context.FixInputPort(ego_velocity_index, frame_velocity1) pose_vector2 = PoseVector() pose_vector2.set_translation([6., 0., 0.]) bundle = PoseBundle(num_poses=1) bundle.set_pose( 0, Isometry3(Quaternion(), pose_vector2.get_translation())) traffic_index = idm.traffic_input().get_index() context.FixInputPort(traffic_index, framework.AbstractValue.Make(bundle)) # Verify the inputs. pose_vector_eval = idm.EvalVectorInput(context, 0) self.assertTrue( np.allclose(pose_vector1.get_translation(), pose_vector_eval.get_translation())) frame_velocity_eval = idm.EvalVectorInput(context, 1) self.assertTrue( np.allclose(frame_velocity1.get_velocity().translational(), frame_velocity_eval.get_velocity().translational())) self.assertTrue( np.allclose(frame_velocity1.get_velocity().rotational(), frame_velocity_eval.get_velocity().rotational())) # Verify the outputs. idm.CalcOutput(context, output) accel_command_index = idm.acceleration_output().get_index() accel = output.get_vector_data(accel_command_index) self.assertEqual(len(accel.get_value()), 1) self.assertTrue(accel.get_value() < 0.)
def test_pose_selector(self): kScanDistance = 4. rg = make_two_lane_road() lane = rg.junction(0).segment(0).lane(0) pose0 = PoseVector() pose0.set_translation([1., 0., 0.]) pose1 = PoseVector() # N.B. Set pose1 3 meters ahead of pose0. pose1.set_translation([4., 0., 0.]) bundle = PoseBundle(num_poses=2) bundle.set_pose(0, Isometry3(Quaternion(), pose0.get_translation())) bundle.set_pose(1, Isometry3(Quaternion(), pose1.get_translation())) closest_pose = PoseSelector.FindSingleClosestPose( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, side=AheadOrBehind.kAhead, path_or_branches=ScanStrategy.kPath) self.assertEqual(closest_pose.odometry.lane.id().string(), lane.id().string()) self.assertTrue(closest_pose.distance == 3.) closest_pair = PoseSelector.FindClosestPair( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, path_or_branches=ScanStrategy.kPath) self.assertEqual( closest_pair[AheadOrBehind.kAhead].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kAhead].distance, 3.) self.assertEqual( closest_pair[AheadOrBehind.kBehind].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kBehind].distance, float('inf')) lane_pos = LanePosition(s=1., r=0., h=0.) road_pos = RoadPosition(lane=lane, pos=lane_pos) w = [1., 2., 3.] v = [-4., -5., -6.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) road_odom = RoadOdometry(road_position=road_pos, frame_velocity=frame_velocity) sigma_v = PoseSelector.GetSigmaVelocity(road_odom) self.assertEqual(sigma_v, v[0])
def test_idm_controller(self): rg = make_two_lane_road() idm = IdmController( road=rg, path_or_branches=ScanStrategy.kPath, road_position_strategy=RoadPositionStrategy.kExhaustiveSearch, period_sec=0.) context = idm.CreateDefaultContext() output = idm.AllocateOutput() # Fix the inputs. pose_vector1 = PoseVector() pose_vector1.set_translation([1., 2., 3.]) ego_pose_index = idm.ego_pose_input().get_index() context.FixInputPort(ego_pose_index, pose_vector1) w = [0., 0., 0.] v = [1., 0., 0.] frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v)) ego_velocity_index = idm.ego_velocity_input().get_index() context.FixInputPort(ego_velocity_index, frame_velocity1) pose_vector2 = PoseVector() pose_vector2.set_translation([6., 0., 0.]) bundle = PoseBundle(num_poses=1) bundle.set_pose( 0, Isometry3(Quaternion(), pose_vector2.get_translation())) traffic_index = idm.traffic_input().get_index() context.FixInputPort( traffic_index, framework.AbstractValue.Make(bundle)) # Verify the inputs. pose_vector_eval = idm.EvalVectorInput(context, 0) self.assertTrue(np.allclose(pose_vector1.get_translation(), pose_vector_eval.get_translation())) frame_velocity_eval = idm.EvalVectorInput(context, 1) self.assertTrue(np.allclose( frame_velocity1.get_velocity().translational(), frame_velocity_eval.get_velocity().translational())) self.assertTrue(np.allclose( frame_velocity1.get_velocity().rotational(), frame_velocity_eval.get_velocity().rotational())) # Verify the outputs. idm.CalcOutput(context, output) accel_command_index = idm.acceleration_output().get_index() accel = output.get_vector_data(accel_command_index) self.assertEqual(len(accel.get_value()), 1) self.assertTrue(accel.get_value() < 0.)
def __init__(self, scene_graph, draw_period=0.033333, Tview=np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]), xlim=[-1., 1], ylim=[-1, 1], facecolor=[1, 1, 1], use_random_colors=False, ax=None): default_size = matplotlib.rcParams['figure.figsize'] scalefactor = (ylim[1]-ylim[0])/(xlim[1]-xlim[0]) figsize = (default_size[0], default_size[0]*scalefactor) PyPlotVisualizer.__init__(self, facecolor=facecolor, figsize=figsize, ax=ax, draw_timestep=draw_period) self.set_name('planar_multibody_visualizer') self._scene_graph = scene_graph self.Tview = Tview self.Tview_pinv = np.linalg.pinv(self.Tview) # Pose bundle (from SceneGraph) input port. self.DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) self.ax.axis('equal') self.ax.axis('off') # Achieve the desired view limits self.ax.set_xlim(xlim) self.ax.set_ylim(ylim) default_size = self.fig.get_size_inches() scalefactor = (ylim[1]-ylim[0])/(xlim[1]-xlim[0]) self.fig.set_size_inches(default_size[0], default_size[0]*scalefactor) # Populate body patches self.buildViewPatches(use_random_colors) # Populate the body fill list -- which requires doing most of # a draw pass, but with an ax.fill() command rather than # an in-place replacement of vertex positions to initialize # the draw patches. # The body fill list stores the ax patch objects in the # order they were spawned (i.e. by body, and then by # order of viewPatches). Drawing the tree should update them # by iterating over bodies and patches in the same order. self.body_fill_dict = {} n_bodies = len(self.viewPatches.keys()) tf = np.eye(4) for full_name in self.viewPatches.keys(): viewPatches, viewColors = self.getViewPatches(full_name, tf) self.body_fill_dict[full_name] = [] for patch, color in zip(viewPatches, viewColors): self.body_fill_dict[full_name] += self.ax.fill( patch[0, :], patch[1, :], zorder=0, edgecolor='k', facecolor=color, closed=True)
def test_pose_selector(self): kScanDistance = 4. rg = make_two_lane_road() lane = rg.junction(0).segment(0).lane(0) pose0 = PoseVector() pose0.set_translation([1., 0., 0.]) pose1 = PoseVector() # N.B. Set pose1 3 meters ahead of pose0. pose1.set_translation([4., 0., 0.]) bundle = PoseBundle(num_poses=2) bundle.set_pose(0, Isometry3(Quaternion(), pose0.get_translation())) bundle.set_pose(1, Isometry3(Quaternion(), pose1.get_translation())) closest_pose = PoseSelector.FindSingleClosestPose( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, side=AheadOrBehind.kAhead, path_or_branches=ScanStrategy.kPath) self.assertEqual(closest_pose.odometry.lane.id().string(), lane.id().string()) self.assertTrue(closest_pose.distance == 3.) closest_pair = PoseSelector.FindClosestPair( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, path_or_branches=ScanStrategy.kPath) self.assertEqual( closest_pair[AheadOrBehind.kAhead].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kAhead].distance, 3.) self.assertEqual( closest_pair[AheadOrBehind.kBehind].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kBehind].distance, float('inf')) lane_pos = LanePosition(s=1., r=0., h=0.) road_pos = RoadPosition(lane=lane, pos=lane_pos) w = [1., 2., 3.] v = [-4., -5., -6.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) road_odom = RoadOdometry( road_position=road_pos, frame_velocity=frame_velocity) sigma_v = PoseSelector.GetSigmaVelocity(road_odom) self.assertEqual(sigma_v, v[0])
def ReadPosesFromFile(filename): """ Reads in 4x4 poses from a text file and wraps them into a PoseBundle. @param filename str. The path of the .txt file to read. @return a PoseBundle with all of the poses. """ pose_dict = {} row_num = 0 object_name = "" cur_matrix = np.eye(4) with open(filename, "r") as f: for line in f: line = line.rstrip() if not line.lstrip(" ").startswith("["): object_name = line else: row = np.matrix(line) cur_matrix[row_num, :] = row row_num += 1 if row_num == 4: pose_dict[object_name] = RigidTransform(cur_matrix) cur_matrix = np.eye(4) row_num %= 4 pose_bundle = PoseBundle(num_poses=len(pose_dict)) i = 0 for object_name in pose_dict: pose_bundle.set_name(i, object_name) pose_bundle.set_pose(i, pose_dict[object_name]) i += 1 return pose_bundle
def __init__(self, meshcat_viz, force_threshold=1e-2, contact_force_scale=10, plant=None, contact_force_radius=0.01): """ Args: meshcat_viz: A pydrake MeshcatVisualizer instance. force_threshold: contact forces whose norms are smaller than force_threshold are not displayed. contact_force_scale: a contact force with norm F (in Newtons) is displayed as a cylinder with length F/contact_force_scale (in meters). plant: the MultibodyPlant associated with meshcat_viz.scene_graph. contact_force_radius: sets the constant radius of the cylinder used to visualize the forces. """ LeafSystem.__init__(self) assert plant is not None self._meshcat_viz = meshcat_viz self._force_threshold = force_threshold self._contact_force_scale = contact_force_scale self._plant = plant self._radius = contact_force_radius self.set_name('meshcat_contact_visualizer') self.DeclarePeriodicPublish(self._meshcat_viz.draw_period, 0.0) # Pose bundle (from SceneGraph) input port. self.DeclareAbstractInputPort("pose_bundle", AbstractValue.Make(PoseBundle(0))) # Contact results input port from MultibodyPlant self.DeclareAbstractInputPort("contact_results", AbstractValue.Make(ContactResults())) # Make force cylinders smaller at initialization. self._force_cylinder_radial_scale = 1. self._force_cylinder_longitudinal_scale = 100. # This system has undeclared states, see #4330. # - All contacts (previous and current), of type `_ContactState`. self._contacts = [] # - Unique key for contacts in meshcat. self._contact_key_counter = 0 # - Previous time at which contact was published. self._t_previous = 0.
def __init__(self, meshcat_viz, force_threshold=1e-2, contact_force_scale=10, plant=None, contact_force_radius=0.01): """ Args: meshcat_viz: A pydrake MeshcatVisualizer instance. force_threshold: contact forces whose norms are smaller than force_threshold are not displayed. contact_force_scale: a contact force with norm F (in Newtons) is displayed as a cylinder with length F/contact_force_scale (in meters). plant: the MultibodyPlant associated with meshcat_viz.scene_graph. contact_force_radius: sets the constant radius of the cylinder used to visualize the forces. """ LeafSystem.__init__(self) assert plant is not None self._meshcat_viz = meshcat_viz self._force_threshold = force_threshold self._contact_force_scale = contact_force_scale self._plant = plant self._radius = contact_force_radius self.set_name('meshcat_contact_visualizer') self.DeclarePeriodicPublish(self._meshcat_viz.draw_period, 0.0) # Pose bundle (from SceneGraph) input port. self.DeclareAbstractInputPort("pose_bundle", AbstractValue.Make(PoseBundle(0))) # Contact results input port from MultibodyPlant self.DeclareAbstractInputPort("contact_results", AbstractValue.Make(ContactResults())) # This system has undeclared states, see #4330. self._warned_pose_bundle_input_port_connected = False self._published_contacts = [] # Zap any previous contact forces on this prefix vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"] vis.delete()
def test_pose_aggregator(self): aggregator = PoseAggregator() # - Set-up. instance_id1 = 5 # Supply a random instance id. port1 = aggregator.AddSingleInput("pose_only", instance_id1) self.assertEqual(port1.get_data_type(), PortDataType.kVectorValued) self.assertEqual(port1.size(), PoseVector.kSize) instance_id2 = 42 # Supply another random, but unique, id. ports2 = aggregator.AddSinglePoseAndVelocityInput( "pose_and_velocity", instance_id2) self.assertEqual(ports2.pose_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.pose_input_port.size(), PoseVector.kSize) self.assertEqual(ports2.velocity_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.velocity_input_port.size(), FrameVelocity.kSize) num_poses = 1 port3 = aggregator.AddBundleInput("pose_bundle", num_poses) self.assertEqual(port3.get_data_type(), PortDataType.kAbstractValued) # - CalcOutput. self.assertEqual(aggregator.get_output_port(0).get_data_type(), PortDataType.kAbstractValued) context = aggregator.CreateDefaultContext() output = aggregator.AllocateOutput() p1 = [0, 1, 2] pose1 = PoseVector() pose1.set_translation(p1) p2 = [5, 7, 9] pose2 = PoseVector() pose2.set_translation(p2) w = [0.3, 0.4, 0.5] v = [0.5, 0.6, 0.7] velocity = FrameVelocity() velocity.set_velocity(SpatialVelocity(w=w, v=v)) p3 = [50, 70, 90] q3 = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) bundle = PoseBundle(num_poses) bundle.set_pose(0, Isometry3(q3, p3)) bundle_value = AbstractValue.Make(bundle) context.FixInputPort(0, pose1) context.FixInputPort(1, pose2) context.FixInputPort(2, velocity) context.FixInputPort(3, bundle_value) aggregator.CalcOutput(context, output) value = output.get_data(0).get_value() self.assertEqual(value.get_num_poses(), 3) isom1_actual = Isometry3() isom1_actual.set_translation(p1) self.assertTrue( (value.get_pose(0).matrix() == isom1_actual.matrix()).all()) isom2_actual = Isometry3() isom2_actual.set_translation(p2) self.assertTrue( (value.get_pose(1).matrix() == isom2_actual.matrix()).all()) vel_actual = value.get_velocity(1).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) self.assertTrue( (value.get_pose(2).matrix() == Isometry3(q3, p3).matrix()).all())
def test_pose_bundle(self): num_poses = 7 bundle = PoseBundle(num_poses) # - Accessors. self.assertEqual(bundle.get_num_poses(), num_poses) self.assertTrue(isinstance(bundle.get_pose(0), Isometry3)) self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity)) # - Mutators. kIndex = 5 p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) bundle.set_pose(kIndex, Isometry3(q, p)) w = [0.1, 0.3, 0.5] v = [0., 1., 2.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) bundle.set_velocity(kIndex, frame_velocity) self.assertTrue((bundle.get_pose(kIndex).matrix() == Isometry3(q, p).matrix()).all()) vel_actual = bundle.get_velocity(kIndex).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) name = "Alice" bundle.set_name(kIndex, name) self.assertEqual(bundle.get_name(kIndex), name) instance_id = 42 # Supply a random instance id. bundle.set_model_instance_id(kIndex, instance_id) self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def __init__(self, scene_graph, draw_period=0.033333, prefix="drake", zmq_url="default", open_browser=None): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes to the visualizer. prefix: Appears as the root of the tree structure in the meshcat data structure zmq_url: Optionally set a url to connect to the visualizer. Use zmp_url="default" to the value obtained by running a single `meshcat-server` in another terminal. Use zmp_url=None or zmq_url="new" to start a new server (as a child of this process); a new web browser will be opened (the url will also be printed to the console). Use e.g. zmq_url="tcp://127.0.0.1:6000" to specify a specific address. open_browser: Set to True to open the meshcat browser url in your default web browser. The default value of None will open the browser iff a new meshcat server is created as a subprocess. Set to False to disable this. Note: This call will not return until it connects to the meshcat-server. """ LeafSystem.__init__(self) self.set_name('meshcat_visualizer') self._DeclarePeriodicPublish(draw_period, 0.0) # Pose bundle (from SceneGraph) input port. self._DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) if zmq_url == "default": zmq_url = "tcp://127.0.0.1:6000" elif zmq_url == "new": zmq_url = None if zmq_url is None and open_browser is None: open_browser = True # Set up meshcat. self.prefix = prefix if zmq_url is not None: print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...") self.vis = meshcat.Visualizer(zmq_url=zmq_url) print("Connected to meshcat-server.") self._scene_graph = scene_graph if open_browser: webbrowser.open(self.vis.url()) def on_initialize(context, event): self.load() self._DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize))
def test_pose_aggregator(self): aggregator = PoseAggregator() # - Set-up. instance_id1 = 5 # Supply a random instance id. port1 = aggregator.AddSingleInput("pose_only", instance_id1) self.assertEqual(port1.get_data_type(), PortDataType.kVectorValued) self.assertEqual(port1.size(), PoseVector.kSize) instance_id2 = 42 # Supply another random, but unique, id. ports2 = aggregator.AddSinglePoseAndVelocityInput( "pose_and_velocity", instance_id2) self.assertEqual(ports2.pose_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.pose_input_port.size(), PoseVector.kSize) self.assertEqual(ports2.velocity_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.velocity_input_port.size(), FrameVelocity.kSize) num_poses = 1 port3 = aggregator.AddBundleInput("pose_bundle", num_poses) self.assertEqual(port3.get_data_type(), PortDataType.kAbstractValued) # - CalcOutput. self.assertEqual( aggregator.get_output_port(0).get_data_type(), PortDataType.kAbstractValued) context = aggregator.CreateDefaultContext() output = aggregator.AllocateOutput() p1 = [0, 1, 2] pose1 = PoseVector() pose1.set_translation(p1) p2 = [5, 7, 9] pose2 = PoseVector() pose2.set_translation(p2) w = [0.3, 0.4, 0.5] v = [0.5, 0.6, 0.7] velocity = FrameVelocity() velocity.set_velocity(SpatialVelocity(w=w, v=v)) p3 = [50, 70, 90] q3 = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) bundle = PoseBundle(num_poses) bundle.set_transform(0, RigidTransform(quaternion=q3, p=p3)) bundle_value = AbstractValue.Make(bundle) aggregator.get_input_port(0).FixValue(context, pose1) aggregator.get_input_port(1).FixValue(context, pose2) aggregator.get_input_port(2).FixValue(context, velocity) aggregator.get_input_port(3).FixValue(context, bundle_value) aggregator.CalcOutput(context, output) value = output.get_data(0).get_value() self.assertEqual(value.get_num_poses(), 3) pose1_actual = RigidTransform(p=p1) self.assertTrue((value.get_transform(0).GetAsMatrix34() == pose1_actual.GetAsMatrix34()).all()) pose2_actual = RigidTransform(p=p2) self.assertTrue((value.get_transform(1).GetAsMatrix34() == pose2_actual.GetAsMatrix34()).all()) vel_actual = value.get_velocity(1).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) pose3_actual = RigidTransform(quaternion=q3, p=p3) self.assertTrue((value.get_transform(2).GetAsMatrix34() == pose3_actual.GetAsMatrix34()).all())
def __init__(self, scene_graph, draw_period=_DEFAULT_PUBLISH_PERIOD, prefix="drake", zmq_url="default", open_browser=None, frames_to_draw={}, frames_opacity=1., axis_length=0.15, axis_radius=0.006, delete_prefix_on_load=True, **kwargs): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes to the visualizer. prefix: Appears as the root of the tree structure in the meshcat data structure zmq_url: Optionally set a url to connect to the visualizer. Use ``zmp_url="default"`` to the value obtained by running a single ``meshcat-server`` in another terminal. Use ``zmp_url=None`` or ``zmq_url="new"`` to start a new server (as a child of this process); a new web browser will be opened (the url will also be printed to the console). Use e.g. ``zmq_url="tcp://127.0.0.1:6000"`` to specify a specific address. open_browser: Set to True to open the meshcat browser url in your default web browser. The default value of None will open the browser iff a new meshcat server is created as a subprocess. Set to False to disable this. frames_to_draw: a dictionary describing which body frames to draw. It is keyed on the names of model instances, and the values are sets of the names of the bodies whose body frames are shown. For example, if we want to draw the frames of body "A" and "B" in model instance "1", then frames_to_draw is {"1": {"A", "B"}}. frames_opacity, axis_length and axis_radius are the opacity, length and radius of the coordinate axes to be drawn. delete_prefix_on_load: Specifies whether we should delete the visualizer path associated with prefix on our load initialization event. True ensures a clean slate for every simulation. False allows for the possibility of caching object meshes on the zmqserver/clients, to avoid repeatedly downloading meshes over the websockets link, but will cause geometry from previous simulations to remain in the scene. You may call ``delete_prefix()`` manually to clear the scene. Additional kwargs will be passed to the MeshcatVisualizer constructor. Note: This call will not return until it connects to the ``meshcat-server``. """ LeafSystem.__init__(self) self.set_name('meshcat_visualizer') self.DeclarePeriodicPublish(draw_period, 0.0) self.draw_period = draw_period self._delete_prefix_on_load = delete_prefix_on_load # Recording. self._is_recording = False self.reset_recording() # Pose bundle (from SceneGraph) input port. # TODO(tehbelinda): Rename the `lcm_visualization` port to match # SceneGraph once its output port has been updated. See #12214. self.DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) if zmq_url == "default": zmq_url = "tcp://127.0.0.1:6000" elif zmq_url == "new": zmq_url = None if zmq_url is None and open_browser is None: open_browser = True # Set up meshcat. self.prefix = prefix if zmq_url is not None: print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...") self.vis = meshcat.Visualizer(zmq_url=zmq_url, **kwargs) print("Connected to meshcat-server.") self._scene_graph = scene_graph # Set background color (to match drake-visualizer). self.vis['/Background'].set_property("top_color", [242, 242, 255]) self.vis['/Background'].set_property("bottom_color", [77, 77, 89]) if open_browser: webbrowser.open(self.vis.url()) def on_initialize(context, event): self.load() self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize)) # drawing coordinate frames self.frames_to_draw = frames_to_draw self.frames_opacity = frames_opacity self.axis_length = axis_length self.axis_radius = axis_radius
def __init__(self, scene_graph, draw_period=1. / 30, T_VW=np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]), xlim=[-1., 1], ylim=[-1, 1], facecolor=[1, 1, 1], use_random_colors=False, substitute_collocated_mesh_files=True, ax=None, show=None): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes to the visualizer. T_VW: The view projection matrix from world to view coordinates. xlim: View limit into the scene. ylim: View limit into the scene. facecolor: Passed through to figure() and sets background color. Both color name strings and RGB triplets are allowed. Defaults to white. use_random_colors: If set to True, will render each body with a different color. (Multiple visual elements on the same body will be the same color.) substitute_collocated_mesh_files: If True, then a mesh file specified with an unsupported filename extension may be replaced by a file of the same base name in the same directory, but with a supported filename extension. Currently only .obj files are supported. ax: If supplied, the visualizer will draw onto those axes instead of creating a new set of axes. The visualizer will still change the view range and figure size of those axes. show: Opens a window during initialization / publish iff True. Default is None, which implies show=True unless matplotlib.get_backend() is 'template'. """ default_size = matplotlib.rcParams['figure.figsize'] scalefactor = (ylim[1] - ylim[0]) / (xlim[1] - xlim[0]) figsize = (default_size[0], default_size[0] * scalefactor) PyPlotVisualizer.__init__(self, facecolor=facecolor, figsize=figsize, ax=ax, draw_period=draw_period, show=show) self.set_name('planar_multibody_visualizer') self._scene_graph = scene_graph self._T_VW = T_VW # Pose bundle (from SceneGraph) input port. # TODO(tehbelinda): Rename the `lcm_visualization` port to match # SceneGraph once its output port has been updated. See #12214. self._pose_bundle_port = self.DeclareAbstractInputPort( "lcm_visualization", AbstractValue.Make(PoseBundle(0))) self.ax.axis('equal') self.ax.axis('off') # Achieve the desired view limits. self.ax.set_xlim(xlim) self.ax.set_ylim(ylim) default_size = self.fig.get_size_inches() self.fig.set_size_inches(figsize[0], figsize[1]) # Populate body patches. self._build_body_patches(use_random_colors, substitute_collocated_mesh_files) # Populate the body fill list -- which requires doing most of a draw # pass, but with an ax.fill() command to initialize the draw patches. # After initialization, we can then use in-place replacement of vertex # positions. The body fill list stores the ax patch objects in the # order they were spawned (i.e. by body, and then by order of view_ # patches). Drawing the tree should update them by iterating over # bodies and patches in the same order. self._body_fill_dict = {} X_WB_initial = RigidTransform.Identity() for full_name in self._patch_Blist.keys(): patch_Wlist, view_colors = self._get_view_patches( full_name, X_WB_initial) self._body_fill_dict[full_name] = [] for patch_W, color in zip(patch_Wlist, view_colors): # Project the full patch the first time, to initialize a vertex # list with enough space for any possible convex hull of this # vertex set. patch_V = self._project_patch(patch_W) body_fill = self.ax.fill(patch_V[0, :], patch_V[1, :], zorder=0, edgecolor='k', facecolor=color, closed=True)[0] self._body_fill_dict[full_name].append(body_fill) # Then update the vertices for a more accurate initial draw. self._update_body_fill_verts(body_fill, patch_V)
def __init__(self, scene_graph, zmq_url="default", draw_period=0.033333, camera_tfs=[RigidTransform()], global_transform=RigidTransform(), out_prefix=None, show_figure=False, save_scene=False): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes rendered images. zmq_url: Optionally set a url to connect to the blender server. Use zmp_url="default" to the value obtained by running a single blender server in another terminal. TODO(gizatt): Use zmp_url=None or zmq_url="new" to start a new server (as a child of this process); a new web browser will be opened (the url will also be printed to the console). Use e.g. zmq_url="tcp://127.0.0.1:5556" to specify a specific address. camera tfs: List of RigidTransform camera tfs. global transform: RigidTransform that gets premultiplied to every object. Note: This call will not return until it connects to the Blender server. TODO(gizatt) Consolidate functionality with BlenderColorCamera into a single BlenderCamera. Needs careful architecting to still support rendering color + label images with different Blender servers in parallel. """ LeafSystem.__init__(self) if out_prefix is not None: self.out_prefix = out_prefix else: self.out_prefix = "/tmp/drake_blender_vis_labels_" self.current_publish_num = 0 self.set_name('blender_label_camera') self.DeclarePeriodicPublish(draw_period, 0.) self.draw_period = draw_period self.save_scene = save_scene # Pose bundle (from SceneGraph) input port. self.DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) if zmq_url == "default": zmq_url = "tcp://127.0.0.1:5556" elif zmq_url == "new": raise NotImplementedError("TODO") if zmq_url is not None: print("Connecting to blender server at zmq_url=" + zmq_url + "...") self.bsi = BlenderServerInterface(zmq_url=zmq_url) print("Connected to Blender server.") self._scene_graph = scene_graph self.global_transform = global_transform self.camera_tfs = camera_tfs def on_initialize(context, event): self.load() self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize)) self.show_figure = show_figure if self.show_figure: plt.figure()
def test_pose_bundle(self): num_poses = 7 bundle = PoseBundle(num_poses) # - Accessors. self.assertEqual(bundle.get_num_poses(), num_poses) self.assertTrue(isinstance(bundle.get_transform(0), RigidTransform)) self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity)) # - Mutators. kIndex = 5 p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) pose = RigidTransform(quaternion=q, p=p) bundle.set_transform(kIndex, pose) self.assertTrue((bundle.get_transform(kIndex).GetAsMatrix34() == pose.GetAsMatrix34()).all()) w = [0.1, 0.3, 0.5] v = [0., 1., 2.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) bundle.set_velocity(kIndex, frame_velocity) vel_actual = bundle.get_velocity(kIndex).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) name = "Alice" bundle.set_name(kIndex, name) self.assertEqual(bundle.get_name(kIndex), name) instance_id = 42 # Supply a random instance id. bundle.set_model_instance_id(kIndex, instance_id) self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def __init__(self, scene_graph, zmq_url="default", draw_period=0.033333, camera_tfs=[RigidTransform()], material_overrides=[], global_transform=RigidTransform(), env_map_path=None, out_prefix=None, show_figure=False, role=Role.kIllustration, save_scene=False): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes rendered images. zmq_url: Optionally set a url to connect to the blender server. Use zmp_url="default" to the value obtained by running a single blender server in another terminal. TODO(gizatt): Use zmp_url=None or zmq_url="new" to start a new server (as a child of this process); a new web browser will be opened (the url will also be printed to the console). Use e.g. zmq_url="tcp://127.0.0.1:5556" to specify a specific address. camera tfs: List of RigidTransform camera tfs. material_overrides: A list of tuples of regex rules and material override arguments to be passed into a a register material call, not including names. (Those are assigned automatically by this class.) global transform: RigidTransform that gets premultiplied to every object. Note: This call will not return until it connects to the Blender server. """ LeafSystem.__init__(self) if out_prefix is not None: self.out_prefix = out_prefix else: self.out_prefix = "/tmp/drake_blender_vis_" self.current_publish_num = 0 self.set_name('blender_color_camera') self.DeclarePeriodicPublish(draw_period, 0.) self.draw_period = draw_period self.save_scene = save_scene # Pose bundle (from SceneGraph) input port. self.DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) # Optional pose bundle of bounding boxes. self.DeclareAbstractInputPort("bounding_box_bundle", AbstractValue.Make(BoundingBoxBundle(0))) if zmq_url == "default": zmq_url = "tcp://127.0.0.1:5556" elif zmq_url == "new": raise NotImplementedError("TODO") if zmq_url is not None: print("Connecting to blender server at zmq_url=" + zmq_url + "...") self.bsi = BlenderServerInterface(zmq_url=zmq_url) print("Connected to Blender server.") self._scene_graph = scene_graph self._role = role self.env_map_path = env_map_path # Compile regex for the material overrides self.material_overrides = [(re.compile(x[0]), x[1]) for x in material_overrides] self.global_transform = global_transform self.camera_tfs = camera_tfs def on_initialize(context, event): self.load() self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize)) self.show_figure = show_figure if self.show_figure: plt.figure()
def __init__(self, scene_graph, draw_period=_DEFAULT_PUBLISH_PERIOD, prefix="drake", zmq_url="default", open_browser=None, frames_to_draw={}, frames_opacity=1., axis_length=0.15, axis_radius=0.006): """ Args: scene_graph: A SceneGraph object. draw_period: The rate at which this class publishes to the visualizer. prefix: Appears as the root of the tree structure in the meshcat data structure zmq_url: Optionally set a url to connect to the visualizer. Use ``zmp_url="default"`` to the value obtained by running a single ``meshcat-server`` in another terminal. Use ``zmp_url=None`` or ``zmq_url="new"`` to start a new server (as a child of this process); a new web browser will be opened (the url will also be printed to the console). Use e.g. ``zmq_url="tcp://127.0.0.1:6000"`` to specify a specific address. open_browser: Set to True to open the meshcat browser url in your default web browser. The default value of None will open the browser iff a new meshcat server is created as a subprocess. Set to False to disable this. frames_to_draw: a dictionary describing which body frames to draw. It is keyed on the names of model instances, and the values are sets of the names of the bodies whose body frames are shown. For example, if we want to draw the frames of body "A" and "B" in model instance "1", then frames_to_draw is {"1": {"A", "B"}}. frames_opacity, axis_length and axis_radius are the opacity, length and radius of the coordinate axes to be drawn. Note: This call will not return until it connects to the ``meshcat-server``. """ LeafSystem.__init__(self) self.set_name('meshcat_visualizer') self.DeclarePeriodicPublish(draw_period, 0.0) self.draw_period = draw_period # Pose bundle (from SceneGraph) input port. self.DeclareAbstractInputPort("lcm_visualization", AbstractValue.Make(PoseBundle(0))) if zmq_url == "default": zmq_url = "tcp://127.0.0.1:6000" elif zmq_url == "new": zmq_url = None if zmq_url is None and open_browser is None: open_browser = True # Set up meshcat. self.prefix = prefix if zmq_url is not None: print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...") self.vis = meshcat.Visualizer(zmq_url=zmq_url) print("Connected to meshcat-server.") self._scene_graph = scene_graph if open_browser: webbrowser.open(self.vis.url()) def on_initialize(context, event): self.load() self.DeclareInitializationEvent( event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=on_initialize)) # drawing coordinate frames self.frames_to_draw = frames_to_draw self.frames_opacity = frames_opacity self.axis_length = axis_length self.axis_radius = axis_radius
def __init__(self, mbp, sg, all_manipulable_body_ids=[], zmq_url="default"): LeafSystem.__init__(self) self.all_manipulable_body_ids = all_manipulable_body_ids self.set_name('HydraInteractionLeafSystem') # Pose bundle (from SceneGraph) input port. #default_sg_context = sg.CreateDefaultContext() #print("Default sg context: ", default_sg_context) #query_object = sg.get_query_output_port().Eval(default_sg_context) #print("Query object: ", query_object) #self.DeclareAbstractInputPort("query_object", # AbstractValue.Make(query_object)) self.pose_bundle_input_port = self.DeclareAbstractInputPort( "pose_bundle", AbstractValue.Make(PoseBundle(0))) self.robot_state_input_port = self.DeclareVectorInputPort( "robot_state", BasicVector(mbp.num_positions() + mbp.num_velocities())) self.spatial_forces_output_port = self.DeclareAbstractOutputPort( "spatial_forces_vector", lambda: AbstractValue.Make(VectorExternallyAppliedSpatialForced()), self.DoCalcAbstractOutput) self.DeclarePeriodicPublish(0.01, 0.0) if zmq_url == "default": zmq_url = "tcp://127.0.0.1:6000" if zmq_url is not None: print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...") self.vis = meshcat.Visualizer(zmq_url=zmq_url) fwd_pt_in_hydra_frame = RigidTransform(p=[0.0, 0.0, 0.0]) self.vis["hydra_origin"]["hand"].set_object( meshcat_geom.ObjMeshGeometry.from_file( os.path.join(os.getcwd(), "hand-regularfinal-scaled-1.obj"))) self.vis["hydra_origin"]["hand"].set_transform( meshcat_tf.compose_matrix(scale=[0.001, 0.001, 0.001], angles=[np.pi / 2, 0., np.pi / 2], translate=[-0.25, 0., 0.])) #self.vis["hydra_origin"]["center"].set_object(meshcat_geom.Sphere(0.02)) #self.vis["hydra_origin"]["center"].set_transform(meshcat_tf.translation_matrix([-0.025, 0., 0.])) #self.vis["hydra_origin"]["mid"].set_object(meshcat_geom.Sphere(0.015)) #self.vis["hydra_origin"]["mid"].set_transform(meshcat_tf.translation_matrix([0.0, 0., 0.])) #self.vis["hydra_origin"]["fwd"].set_object(meshcat_geom.Sphere(0.01)) #self.vis["hydra_origin"]["fwd"].set_transform(fwd_pt_in_hydra_frame.matrix()) #self.vis["hydra_grab"].set_object(meshcat_geom.Sphere(0.01), # meshcat_geom.MeshLambertMaterial( # color=0xff22dd, # alphaMap=0.1)) self.vis["hydra_grab"]["grab_point"].set_object( meshcat_geom.Sphere(0.01), meshcat_geom.MeshLambertMaterial(color=0xff22dd, alphaMap=0.1)) # Hide it sketchily self.vis["hydra_grab"].set_transform( meshcat_tf.translation_matrix([0., 0., -1000.])) # State for selecting objects self.grab_needs_update = False self.grab_in_progress = False self.grab_update_hydra_pose = None self.selected_body = None self.selected_pose_in_body_frame = None self.desired_pose_in_world_frame = None self.stop = False self.freeze_rotation = False self.previously_freezing_rotation = False # Set up subscription to Razer Hydra self.mbp = mbp self.mbp_context = mbp.CreateDefaultContext() self.sg = sg self.hasNewMessage = False self.lastMsg = None self.hydra_origin = RigidTransform(p=[1.0, 0., -0.1], rpy=RollPitchYaw([0., 0., 0.])) self.hydra_prescale = 3.0 self.callback_lock = Lock() self.hydraSubscriber = rospy.Subscriber("/hydra_calib", razer_hydra.msg.Hydra, self.callback, queue_size=1) print("Waiting for hydra startup...") while not self.hasNewMessage and not rospy.is_shutdown(): rospy.sleep(0.01) print("Got hydra.")