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 Here are two plots to visualize the results of the trajectory optimization. In the first we plot the limit cycle we found in the plane of the leg angles. To show a complete cycle, we "mirror" the trajectory of the first step and we plot it too ("Red leg swinging"). """
# 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())