def build_block_diagram(actuators_off=False,
desired_lateral_velocity=0.0,
desired_height=3.0,
print_period=0.0):
builder = DiagramBuilder()
# Build the plant
plant = builder.AddSystem(MultibodyPlant(0.0005))
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterAsSourceForSceneGraph(scene_graph)
builder.Connect(plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
# Build the robot
parser = Parser(plant)
parser.AddModelFromFile("raibert_hopper_2d.sdf")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground"))
plant.Finalize()
plant.set_name('plant')
# Create a logger to log at 30hz
state_dim = plant.num_positions() + plant.num_velocities()
state_log = builder.AddSystem(SignalLogger(state_dim))
state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging
builder.Connect(plant.get_state_output_port(), state_log.get_input_port(0))
state_log.set_name('state_log')
# The controller
controller = builder.AddSystem(
Hopper2dController(plant,
desired_lateral_velocity=desired_lateral_velocity,
desired_height=desired_height,
actuators_off=actuators_off,
print_period=print_period))
builder.Connect(plant.get_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
controller.set_name('controller')
# Create visualizer
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
xlim=[-1, 10],
ylim=[-.2, 4.5],
show=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
visualizer.set_name('visualizer')
diagram = builder.Build()
return diagram
def animate(plant, x_trajectory):
builder = DiagramBuilder()
source = builder.AddSystem(TrajectorySource(x_trajectory))
builder.AddSystem(scene_graph)
pos_to_pose = builder.AddSystem(
MultibodyPositionToGeometryPose(plant, 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(plant.get_source_id()))
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2, 2],
ylim=[-1.25, 2],
ax=ax[0]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
simulator = Simulator(builder.Build())
simulator.AdvanceTo(x_trajectory.end_time())
def main():
goal_position = np.array([0.5, 0., 0.025])
blue_box_clean_position = [0.4, 0., 0.05]
red_box_clean_position = [0.6, 0., 0.05]
goal_delta = 0.05
parser = argparse.ArgumentParser(description=__doc__)
MeshcatVisualizer.add_argparse_argument(parser)
parser.add_argument('--use_meshcat',
action='store_true',
help="Must be set for meshcat to be used.")
parser.add_argument('--disable_planar_viz',
action='store_true',
help="Don't create a planar visualizer. Probably"
" breaks something that assumes the planar"
" vis exists.")
parser.add_argument('--teleop',
action='store_true',
help="Enable teleop, so *don't* use the state machine"
" and motion primitives.")
args = parser.parse_args()
builder = DiagramBuilder()
# Set up the ManipulationStation
station = builder.AddSystem(ManipulationStation(0.001))
mbp = station.get_multibody_plant()
station.SetupManipulationClassStation()
add_goal_region_visual_geometry(mbp, goal_position, goal_delta)
add_box_at_location(mbp,
name="blue_box",
color=[0.25, 0.25, 1., 1.],
pose=RigidTransform(p=[0.4, 0.0, 0.025]))
add_box_at_location(mbp,
name="red_box",
color=[1., 0.25, 0.25, 1.],
pose=RigidTransform(p=[0.6, 0.0, 0.025]))
station.Finalize()
iiwa_q0 = np.array([0.0, 0.6, 0.0, -1.75, 0., 1., np.pi / 2.])
# Attach a visualizer.
if args.use_meshcat:
meshcat = builder.AddSystem(
MeshcatVisualizer(station.get_scene_graph(), zmq_url=args.meshcat))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
if not args.disable_planar_viz:
plt.gca().clear()
viz = builder.AddSystem(
PlanarSceneGraphVisualizer(station.get_scene_graph(),
xlim=[0.25, 0.8],
ylim=[-0.1, 0.5],
ax=plt.gca()))
builder.Connect(station.GetOutputPort("pose_bundle"),
viz.get_input_port(0))
plt.draw()
# Hook up DifferentialIK, since both control modes use it.
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
time_step = 0.005
params.set_timestep(time_step)
# True velocity limits for the IIWA14 (in rad, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
# Stay within a small fraction of those limits for this teleop demo.
factor = 1.0
params.set_joint_velocity_limits(
(-factor * iiwa14_velocity_limits, factor * iiwa14_velocity_limits))
differential_ik = builder.AddSystem(
DifferentialIK(robot, robot.GetFrameByName("iiwa_link_7"), params,
time_step))
differential_ik.set_name("Differential IK")
differential_ik.parameters.set_nominal_joint_position(iiwa_q0)
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
if not args.teleop:
symbol_list = [
SymbolL2Close("blue_box_in_goal", "blue_box", goal_position,
goal_delta),
SymbolL2Close("red_box_in_goal", "red_box", goal_position,
goal_delta),
SymbolRelativePositionL2("blue_box_on_red_box",
"blue_box",
"red_box",
l2_thresh=0.01,
offset=np.array([0., 0., 0.05])),
SymbolRelativePositionL2("red_box_on_blue_box",
"red_box",
"blue_box",
l2_thresh=0.01,
offset=np.array([0., 0., 0.05])),
]
primitive_list = [
MoveBoxPrimitive("put_blue_box_in_goal", mbp, "blue_box",
goal_position),
MoveBoxPrimitive("put_red_box_in_goal", mbp, "red_box",
goal_position),
MoveBoxPrimitive("put_blue_box_away", mbp, "blue_box",
blue_box_clean_position),
MoveBoxPrimitive("put_red_box_away", mbp, "red_box",
red_box_clean_position),
MoveBoxPrimitive("put_red_box_on_blue_box", mbp, "red_box",
np.array([0., 0., 0.05]), "blue_box"),
MoveBoxPrimitive("put_blue_box_on_red_box", mbp, "blue_box",
np.array([0., 0., 0.05]), "red_box"),
]
task_execution_system = builder.AddSystem(
TaskExectionSystem(
mbp,
symbol_list=symbol_list,
primitive_list=primitive_list,
dfa_json_file="specifications/red_and_blue_boxes_stacking.json"
))
builder.Connect(station.GetOutputPort("plant_continuous_state"),
task_execution_system.GetInputPort("mbp_state_vector"))
builder.Connect(task_execution_system.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(task_execution_system.get_output_port(1),
station.GetInputPort("wsg_position"))
#movebox = MoveBoxPrimitive("test_move_box", mbp, "red_box", goal_position)
#rpy_xyz_trajectory, gripper_traj = movebox.generate_rpyxyz_and_gripper_trajectory(mbp.CreateDefaultContext())
#rpy_xyz_trajectory_source = builder.AddSystem(TrajectorySource(rpy_xyz_trajectory))
#builder.Connect(rpy_xyz_trajectory_source.get_output_port(0),
# differential_ik.GetInputPort("rpy_xyz_desired"))
#wsg_position_source = builder.AddSystem(TrajectorySource(gripper_traj))
#builder.Connect(wsg_position_source.get_output_port(0),
# station.GetInputPort("wsg_position"))
# Target zero feedforward residual torque at all times.
fft = builder.AddSystem(ConstantVectorSource(np.zeros(7)))
builder.Connect(fft.get_output_port(0),
station.GetInputPort("iiwa_feedforward_torque"))
input_force_fix = builder.AddSystem(ConstantVectorSource([40.0]))
builder.Connect(input_force_fix.get_output_port(0),
station.GetInputPort("wsg_force_limit"))
end_time = 10000
else: # Set up teleoperation.
# Hook up a pygame-based keyboard+mouse interface for
# teleoperation, and low pass its output to drive the EE target
# for the differential IK.
print_instructions()
teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=True))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=0.005, size=6))
builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0))
builder.Connect(filter_.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(teleop.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(teleop.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
# Target zero feedforward residual torque at all times.
fft = builder.AddSystem(ConstantVectorSource(np.zeros(7)))
builder.Connect(fft.get_output_port(0),
station.GetInputPort("iiwa_feedforward_torque"))
# Simulate functionally forever.
end_time = 10000
# Create symbol log
#symbol_log = SymbolFromTransformLog(
# [SymbolL2Close('at_goal', 'red_box', goal_position, .025),
# SymbolL2Close('at_goal', 'blue_box', goal_position, .025)])
#
#symbol_logger_system = builder.AddSystem(
# SymbolLoggerSystem(
# station.get_multibody_plant(), symbol_logger=symbol_log))
#builder.Connect(
# station.GetOutputPort("plant_continuous_state"),
# symbol_logger_system.GetInputPort("mbp_state_vector"))
# Remaining input ports need to be tied up.
diagram = builder.Build()
g = pydot.graph_from_dot_data(diagram.GetGraphvizString())[0]
g.write_png("system_diagram.png")
diagram_context = diagram.CreateDefaultContext()
station_context = diagram.GetMutableSubsystemContext(
station, diagram_context)
station.SetIiwaPosition(station_context, iiwa_q0)
differential_ik.SetPositions(
diagram.GetMutableSubsystemContext(differential_ik, diagram_context),
iiwa_q0)
if args.teleop:
teleop.SetPose(differential_ik.ForwardKinematics(iiwa_q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter_, diagram_context),
teleop.get_output_port(0).Eval(
diagram.GetMutableSubsystemContext(teleop, diagram_context)))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(end_time)
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
file_name = FindResource("cartpole/cartpole.urdf")
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10000.0)
args = parser.parse_args()
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.5, 2.5], ylim=[-1, 2.5]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, "Force", -30., 30.))
builder.Connect(torque_system.get_output_port(0),
plant.get_actuation_input_port())
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
context = simulator.get_mutable_context()
context.SetContinuousState([0., 1., 0., 0.])
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10000.0)
args = parser.parse_args()
builder = DiagramBuilder()
acrobot = builder.AddSystem(AcrobotPlant())
scene_graph = builder.AddSystem(SceneGraph())
AcrobotGeometry.AddToBuilder(builder, acrobot.get_output_port(0), scene_graph)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-4., 4.], ylim=[-4., 4.]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, "Torque", -5., 5.))
builder.Connect(torque_system.get_output_port(0), acrobot.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
context = simulator.get_mutable_context()
context.SetContinuousState([1., 0., 0., 0.])
We aim to construct the diagram shown above, with the addition of a visualizer, which will be connected with the system state.
**Troubleshooting:**
Note that we already initialized the `builder` of the block diagram when parsing the `.urdf` file.
Hence, by running the following cell multiple times, you would actually try to wire the blocks in the diagram more than once.
This is not acceptable, and Drake will raise the error `RuntimeError: Input port is already wired.`
If you wish to modify the next cell and rerun the program to see the effects of your modification, you must rerun the cell where the `builder` is initialized first (i.e. the cell with the line `builder = DiagramBuilder()`).
"""
# instantiate controllers
inner_controller = builder.AddSystem(InnerController(vibrating_pendulum))
outer_controller = builder.AddSystem(OuterController(vibrating_pendulum, pendulum_torque))
# instantiate visualizer
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.5, 2.5], ylim=[-1.5, 2.5], show=False)
)
# logger that records the state trajectory during simulation
logger = LogOutput(vibrating_pendulum.get_state_output_port(), builder)
# mux block to squeeze the (base + pendulum) state and
# the outer control signal in a single cable
mux = builder.AddSystem(Multiplexer([4, 1]))
# selector that extracts the pendulum state from the state of the base and the pendulum
selector = builder.AddSystem(MatrixGain(np.array([
[0, 1, 0, 0], # selects the angle of the pendulum
[0, 0, 0, 1] # selects the angular velocity of the pendulum
])))
# synthesize lqr controller directly from
# the nonlinear system and the operating point
lqr = LinearQuadraticRegulator(cartpole, context, Q, R, input_port_index=input_i)
lqr = builder.AddSystem(lqr)
# the following two lines are not needed here...
output_i = cartpole.get_state_output_port().get_index()
cartpole_lin = Linearize(cartpole, context, input_port_index=input_i, output_port_index=output_i)
# wire cart-pole and lqr
builder.Connect(cartpole.get_state_output_port(), lqr.get_input_port(0))
builder.Connect(lqr.get_output_port(0), cartpole.get_actuation_input_port())
# add a visualizer and wire it
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-3., 3.], ylim=[-1.2, 1.2], show=False)
)
builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0))
# finish building the block diagram
diagram = builder.Build()
# instantiate a simulator
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False) # makes sim faster
"""The following cell contains a function that simulates the closed-loop system and produces a video of the sim."""
# function that given the cart-pole initial state
# and the simulation time, simulates the system
# and produces a video
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
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").
type=float,
help="Ramp angle (in radians)",
default=0.08)
args = parser.parse_args()
params = RimlessWheelParams()
params.set_slope(args.slope)
builder = DiagramBuilder()
rimless_wheel = builder.AddSystem(RimlessWheel())
scene_graph = builder.AddSystem(SceneGraph())
RimlessWheelGeometry.AddToBuilder(
builder, rimless_wheel.get_floating_base_state_output_port(), params,
scene_graph)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-8., 8.], ylim=[-2., 3.]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
diagram.Publish(context) # draw once to get the window open
diagram.GetMutableSubsystemContext(
rimless_wheel, context).get_numeric_parameter(0).set_slope(args.slope)
context.SetAccuracy(1e-4)
context.SetContinuousState([args.initial_angle, args.initial_angular_velocity])
simulator.AdvanceTo(args.duration)
min_flipper_x = np.min(q_opt[:, 0])
min_pancake_x = np.min(q_opt[:, 1])
max_x = max(max_flipper_x, max_pancake_x)
min_x = min(min_flipper_x, min_pancake_x)
max_flipper_y = np.max(q_opt[:, 2])
max_pancake_y = np.max(q_opt[:, 3])
min_flipper_y = np.min(q_opt[:, 2])
min_pancake_y = np.min(q_opt[:, 3])
max_y = max(max_flipper_y, max_pancake_y)
min_y = min(min_flipper_y, min_pancake_y)
xlim = [min_x - 3, max_x + 3]
ylim = [min_y - 2, max_y + 2]
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=xlim, ylim=ylim))
# Connect the output of the scene graph to the visualizer
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
# Build the diagram!
diagram = builder.Build()
# Reset the loggers
u_logger.reset()
q_logger.reset()
# start recording the video for the animation of the simulation
visualizer.start_recording()
# 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())
u_lookup = np.vectorize(u_trajectory.value)
u_values = u_lookup(times)
ax.plot(times, u_values)
ax.set_xlabel("time (seconds)")
ax.set_ylabel("force (Newtons)")
x_trajectory = dircol.ReconstructStateTrajectory(result)
# TODO(russt): Add some helper methods to make this workflow cleaner.
builder = DiagramBuilder()
source = builder.AddSystem(TrajectorySource(x_trajectory))
builder.AddSystem(scene_graph)
pos_to_pose = builder.AddSystem(
MultibodyPositionToGeometryPose(plant, 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(plant.get_source_id()))
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2, 2],
ylim=[-1.25, 2],
show=True))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
simulator = Simulator(builder.Build())
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(x_trajectory.end_time())
hip_torque = builder.AddSystem(ConstantVectorSource([0.0]))
builder.Connect(hip_torque.get_output_port(0), compass_gait.get_input_port(0))
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
scene_graph = builder.AddSystem(SceneGraph())
CompassGaitGeometry.AddToBuilder(
builder, compass_gait.get_floating_base_state_output_port(), scene_graph)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-1., 5.], ylim=[-1., 2.]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
diagram.Publish(context) # draw once to get the window open
context.SetAccuracy(1e-4)
context.SetContinuousState([0., 0., 0.4, -2.])
simulator.AdvanceTo(args.duration)