def simulate(*, initial_state, controller_params, t_final, tape_period):
"""Simulates an Acrobot + Spong controller from the given initial state and
parameters until the given final time. Returns the state sampled at the
given tape_period.
"""
builder = DiagramBuilder()
plant = builder.AddSystem(AcrobotPlant())
controller = builder.AddSystem(AcrobotSpongController())
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
state_logger = LogOutput(plant.get_output_port(0), builder)
state_logger.set_publish_period(tape_period)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = diagram.GetMutableSubsystemContext(plant, context)
controller_context = diagram.GetMutableSubsystemContext(
controller, context)
plant_context.SetContinuousState(initial_state)
controller_context.get_mutable_numeric_parameter(0).SetFromVector(
controller_params)
simulator.AdvanceTo(t_final)
x_tape = state_logger.data()
return x_tape
def ConnectPortsAndAddLoggers(builder, station, plan_runner):
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.GetOutputPort("gripper_setpoint"),
station.GetInputPort("wsg_position"))
builder.Connect(plan_runner.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
builder.Connect(plan_runner.GetOutputPort("iiwa_position_command"),
station.GetInputPort("iiwa_position"))
builder.Connect(plan_runner.GetOutputPort("iiwa_torque_command"),
station.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(station.GetOutputPort("iiwa_position_measured"),
plan_runner.GetInputPort("iiwa_position"))
builder.Connect(station.GetOutputPort("iiwa_position_commanded"),
plan_runner.GetInputPort("iiwa_position_cmd"))
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.GetInputPort("iiwa_velocity"))
builder.Connect(station.GetOutputPort("iiwa_torque_external"),
plan_runner.GetInputPort("iiwa_torque_external"))
# Add loggers
iiwa_position_command_log = LogOutput(
plan_runner.GetOutputPort("iiwa_position_command"), builder)
iiwa_position_command_log.set_publish_period(0.01)
iiwa_external_torque_log = LogOutput(
station.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log.set_publish_period(0.01)
iiwa_position_measured_log = LogOutput(
station.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log.set_publish_period(0.01)
iiwa_velocity_estimated_log = LogOutput(
station.GetOutputPort("iiwa_velocity_estimated"), builder)
iiwa_velocity_estimated_log.set_publish_period(0.01)
return (iiwa_position_command_log, iiwa_position_measured_log,
iiwa_external_torque_log, iiwa_velocity_estimated_log)
def RunSimulation(self, q0_iiwa, door_angles, plan_list,
gripper_setpoint_list,
extra_time=0, real_time_rate=1.0,
is_visualizing=True,
gripper_setpoint_initial=0.025):
"""
Constructs a Diagram that sends commands to ManipulationStation.
@param plan_list: A list of Plans to be executed.
@param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan.
@param extra_time: the amount of time for which commands are sent,
in addition to the sum of the durations of all plans.
@param real_time_rate: 1.0 means realtime; 0 means as fast as possible.
@param q0_iiwa: initial configuration of the robot.
@param is_visualizing: if true, adds MeshcatVisualizer to the Diagram. It should be set to False
when running tests.
@param sim_duration: the duration of simulation in seconds. If unset, it is set to the sum of the durations of all
plans in plan_list plus extra_time.
@return: logs of robot configuration and MultibodyPlant, generated by simulation.
Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data().
"""
builder = DiagramBuilder()
builder.AddSystem(self.station)
# Add plan runner.
if is_visualizing:
print_period=1.0
else:
print_period=np.inf
plan_runner = IiwaPlanRunner(
iiwa_plans=plan_list,
gripper_setpoint_list=gripper_setpoint_list,
print_period=print_period,
gripper_setpoint_initial=gripper_setpoint_initial)
self.plan_runner = plan_runner
iiwa_position_command_log, iiwa_position_measured_log, \
iiwa_external_torque_log, iiwa_velocity_estimated_log = \
self.ConnectPortsAndAddLoggers(builder, self.station, plan_runner)
plant_state_log = LogOutput(
self.station.GetOutputPort("plant_continuous_state"), builder)
plant_state_log.set_publish_period(0.01)
if is_visualizing:
# Add meshcat visualizer
scene_graph = self.station.get_mutable_scene_graph()
viz = MeshcatVisualizer(scene_graph,
zmq_url="tcp://127.0.0.1:6000",
frames_to_draw=self.frames_to_draw,
frames_opacity=0.8)
self.meshcat_vis = viz
builder.AddSystem(viz)
builder.Connect(self.station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
contact_viz = MeshcatContactVisualizer(
meshcat_viz=viz, plant=self.plant)
builder.AddSystem(contact_viz)
builder.Connect(
self.station.GetOutputPort("pose_bundle"),
contact_viz.GetInputPort("pose_bundle"))
builder.Connect(
self.station.GetOutputPort("contact_results"),
contact_viz.GetInputPort("contact_results"))
# build diagram
diagram = builder.Build()
self.diagram = diagram
# RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
self.simulator = simulator
context = diagram.GetMutableSubsystemContext(
self.station, simulator.get_mutable_context())
# set initial state of the robot
self.station.SetIiwaPosition(context, q0_iiwa)
self.station.SetIiwaVelocity(context, np.zeros(7))
self.station.SetWsgPosition(context, 0.03)
self.station.SetWsgVelocity(context, 0)
# set initial hinge angles of the cupboard.
# setting hinge angle to exactly 0 or 90 degrees will result in
# intermittent contact with small contact forces between the door and
# the cupboard body.
left_hinge_joint = self.plant.GetJointByName("left_door_hinge")
left_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=door_angles[0])
right_hinge_joint = self.plant.GetJointByName("right_door_hinge")
right_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(self.plant,
context),
angle=door_angles[1])
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
# calculate starting time for all plans.
t_plan = GetPlanStartingTimes(plan_list)
sim_duration = t_plan[-1] + extra_time
if is_visualizing:
print("simulation duration", sim_duration)
print("plan starting times\n", t_plan)
self.SetInitialPlanRunnerState(plan_runner, simulator, diagram)
simulator.Initialize()
simulator.AdvanceTo(sim_duration)
return (iiwa_position_command_log, iiwa_position_measured_log,
iiwa_external_torque_log, iiwa_velocity_estimated_log,
plant_state_log, t_plan)
