# parse urdf with scene graph compass_gait = MultibodyPlant(time_step=0) scene_graph = SceneGraph() compass_gait.RegisterAsSourceForSceneGraph(scene_graph) file_name = FindResource('models/compass_gait_limit_cycle.urdf') Parser(compass_gait).AddModelFromFile(file_name) compass_gait.Finalize() # build block diagram and drive system state with # the trajectory from the optimization problem builder = DiagramBuilder() source = builder.AddSystem(TrajectorySource(x_opt_poly)) builder.AddSystem(scene_graph) pos_to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(compass_gait, input_multibody_state=True)) builder.Connect(source.get_output_port(0), pos_to_pose.get_input_port()) builder.Connect(pos_to_pose.get_output_port(), scene_graph.get_source_pose_port(compass_gait.get_source_id())) # add visualizer xlim = [-.75, 1.] ylim = [-.2, 1.5] visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph, xlim=xlim, ylim=ylim, show=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) simulator = Simulator(builder.Build()) # generate and display animation visualizer.start_recording() simulator.AdvanceTo(x_opt_poly.end_time()) ani = visualizer.get_recording_as_animation() HTML(ani.to_jshtml()) """## Plot the Results
class Visualizer(): def __init__(self, urdf): self._create_plant(urdf) def _create_plant(self, urdf): self.plant = MultibodyPlant(time_step=0.0) self.scenegraph = SceneGraph() self.plant.RegisterAsSourceForSceneGraph(self.scenegraph) self.model_index = Parser(self.plant).AddModelFromFile(FindResource(urdf)) self.builder = DiagramBuilder() self.builder.AddSystem(self.scenegraph) def _finalize_plant(self): self.plant.Finalize() def _add_trajectory_source(self, traj): # Create the trajectory source source = self.builder.AddSystem(TrajectorySource(traj)) pos2pose = self.builder.AddSystem(MultibodyPositionToGeometryPose(self.plant, input_multibody_state=True)) # Wire the source to the scene graph self.builder.Connect(source.get_output_port(0), pos2pose.get_input_port()) self.builder.Connect(pos2pose.get_output_port(), self.scenegraph.get_source_pose_port(self.plant.get_source_id())) def _add_renderer(self): renderer_name = "renderer" self.scenegraph.AddRenderer(renderer_name, MakeRenderEngineVtk(RenderEngineVtkParams())) # Add a camera depth_camera = DepthRenderCamera( RenderCameraCore( renderer_name, CameraInfo(width=640, height=480, fov_y=np.pi/4), ClippingRange(0.01, 10.0), RigidTransform()), DepthRange(0.01,10.0) ) world_id = self.plant.GetBodyFrameIdOrThrow(self.plant.world_body().index()) X_WB = xyz_rpy_deg([4,0,0],[-90,0,90]) sensor = RgbdSensor(world_id, X_PB=X_WB, depth_camera=depth_camera) self.builder.AddSystem(sensor) self.builder.Connect(self.scenegraph.get_query_output_port(), sensor.query_object_input_port()) def _connect_visualizer(self): DrakeVisualizer.AddToBuilder(self.builder, self.scenegraph) self.meshcat = ConnectMeshcatVisualizer(self.builder, self.scenegraph, zmq_url="new", open_browser=False) self.meshcat.vis.jupyter_cell() def _make_visualization(self, stop_time): simulator = Simulator(self.builder.Build()) simulator.Initialize() self.meshcat.vis.render_static() # Set simulator context simulator.get_mutable_context().SetTime(0.0) # Start recording and simulate the diagram self.meshcat.reset_recording() self.meshcat.start_recording() simulator.AdvanceTo(stop_time) # Publish the recording self.meshcat.publish_recording() # Render self.meshcat.vis.render_static() input("View visualization. Press <ENTER> to end") print("Finished") def visualize_trajectory(self, xtraj=None): self._finalize_plant() print("Adding trajectory source") xtraj = self._check_trajectory(xtraj) self._add_trajectory_source(xtraj) print("Adding renderer") self._add_renderer() print("Connecting to MeshCat") self._connect_visualizer() print("Making visualization") self._make_visualization(xtraj.end_time()) def _check_trajectory(self, traj): if traj is None: plant_context = self.plant.CreateDefaultContext() pose = self.plant.GetPositions(plant_context) pose = np.column_stack((pose, pose)) pose = zero_pad_rows(pose, self.plant.num_positions() + self.plant.num_velocities()) return PiecewisePolynomial.FirstOrderHold([0., 1.], pose) elif type(traj) is np.ndarray: if traj.ndim == 1: traj = np.column_stack(traj, traj) traj = zero_pad_rows(traj, self.plant.num_positions() + self.plant.num_velocities()) return PiecewisePolynomial.FirstOrderHold([0.,1.], traj) elif type(traj) is PiecewisePolynomial: breaks = traj.get_segment_times() values = traj.vector_values(breaks) values = zero_pad_rows(values, self.plant.num_positions() + self.plant.num_velocities()) return PiecewisePolynomial.FirstOrderHold(breaks, values) else: raise ValueError("Trajectory must be a piecewise polynomial, an ndarray, or None")
# A Diagram is a collection of systems in a directed graph. To visualize the results, we need to attach the solution trajectory, a SceneGraph, and a visualizer together. # After constructing the Diagram, we need to # 1. Add systems to the Diagram # 2. Connect the systems in the Diagram # AddSystem is the generic method to add components to the Diagram. # TrajectorySource is a type of System whose output is the value of a trajectory at a time in the system's context builder = DiagramBuilder() source = builder.AddSystem(TrajectorySource(x_traj)) builder.AddSystem(scene_graph) to_pose = builder.AddSystem( MultibodyPositionToGeometryPose(plant, input_multibody_state=True)) # Wire the ports of hte systems together builder.Connect(source.get_output_port(0), to_pose.get_input_port()) builder.Connect(to_pose.get_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) # Add a visualizer T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, T_VW=T_VW, xlim=[-4., 4.], ylim=[-4., 4.], show=True)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) # build and run the simulator simulator = Simulator(builder.Build()) simulator.Initialize() simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(x_traj.end_time())