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())