def RunRealRobot(
self,
plan_list,
gripper_setpoint_list,
sim_duration=None,
extra_time=2.0,
is_plan_runner_diagram=False,
):
"""
Constructs a Diagram that sends commands to ManipulationStationHardwareInterface.
@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 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.
@param extra_time: the amount of time for which commands are sent, in addition to the duration of all plans.
@param is_plan_runner_diagram: True: use the diagram version of PlanRunner; False: use the leaf version
of PlanRunner.
@return: logs of robot configuration and torque, decoded from LCM messges sent by the robot's driver.
Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data().
"""
builder = DiagramBuilder()
camera_ids = ["805212060544"]
station_hardware = ManipulationStationHardwareInterface(camera_ids)
station_hardware.Connect(wait_for_cameras=False)
builder.AddSystem(station_hardware)
# Add plan runner.
if is_plan_runner_diagram:
plan_runner, duration_multiplier = CreateManipStationPlanRunnerDiagram(
kuka_plans=plan_list,
gripper_setpoint_list=gripper_setpoint_list,
print_period=0,
)
else:
plan_runner = ManipStationPlanRunner(
kuka_plans=plan_list,
gripper_setpoint_list=gripper_setpoint_list,
print_period=0,
)
duration_multiplier = plan_runner.kPlanDurationMultiplier
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.GetOutputPort("gripper_setpoint"),
station_hardware.GetInputPort("wsg_position"))
builder.Connect(plan_runner.GetOutputPort("force_limit"),
station_hardware.GetInputPort("wsg_force_limit"))
builder.Connect(plan_runner.GetOutputPort("iiwa_position_command"),
station_hardware.GetInputPort("iiwa_position"))
builder.Connect(
plan_runner.GetOutputPort("iiwa_torque_command"),
station_hardware.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(
station_hardware.GetOutputPort("iiwa_position_measured"),
plan_runner.GetInputPort("iiwa_position"))
builder.Connect(
station_hardware.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.GetInputPort("iiwa_velocity"))
builder.Connect(station_hardware.GetOutputPort("iiwa_torque_external"),
plan_runner.GetInputPort("iiwa_torque_external"))
# Add logger
iiwa_position_command_log = LogOutput(
plan_runner.GetOutputPort("iiwa_position_command"), builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
station_hardware.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
iiwa_velocity_estimated_log = LogOutput(
station_hardware.GetOutputPort("iiwa_velocity_estimated"), builder)
iiwa_velocity_estimated_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
station_hardware.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
# build diagram
diagram = builder.Build()
# RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
self.simulator = simulator
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
t_plan = GetPlanStartingTimes(plan_list, duration_multiplier)
if sim_duration is None:
sim_duration = t_plan[-1] + extra_time
print "simulation duration", sim_duration
print "plan starting times\n", t_plan
print "sending trajectories in 2 seconds..."
time.sleep(1.0)
print "sending trajectories in 1 second..."
time.sleep(1.0)
print "sending trajectories now!"
simulator.Initialize()
self.SetInitialPlanRunnerState(plan_runner, simulator, diagram)
simulator.StepTo(sim_duration)
return iiwa_position_command_log, iiwa_position_measured_log, \
iiwa_velocity_estimated_log, iiwa_external_torque_log, t_plan
def RunRealRobot(self, plan_list, gripper_setpoint_list, extra_time=2.0):
"""
Constructs a Diagram that sends commands to
ManipulationStationHardwareInterface.
@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 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.
@param extra_time: the amount of time for which commands are sent, in
addition to the duration of all plans.
@param is_plan_runner_diagram: True: use the diagram version of
PlanRunner; False: use the leaf version of PlanRunner.
@return: logs of robot configuration and torque, decoded from LCM
messges sent by the robot's driver. Logs are SignalLogger
systems, whose data can be accessed by SignalLogger.data().
"""
builder = DiagramBuilder()
camera_ids = ["805212060544"]
station_hardware = ManipulationStationHardwareInterface(camera_ids)
station_hardware.Connect(wait_for_cameras=False)
builder.AddSystem(station_hardware)
# Add plan runner.
plan_runner = IiwaPlanRunner(
iiwa_plans=plan_list,
gripper_setpoint_list=gripper_setpoint_list,
print_period=np.inf)
iiwa_position_command_log, iiwa_position_measured_log, \
iiwa_external_torque_log, iiwa_velocity_estimated_log = \
self.ConnectPortsAndAddLoggers(
builder, station_hardware, plan_runner)
# build diagram
diagram = builder.Build()
# RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
self.simulator = simulator
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
t_plan = GetPlanStartingTimes(plan_list)
sim_duration = t_plan[-1] + extra_time
print("simulation duration", sim_duration)
print("plan starting times\n", t_plan)
# set initial command to be the same as current robot joint angles.
q0 = self.GetCurrentJointAngles()
plan_runner_context = \
diagram.GetMutableSubsystemContext(
plan_runner, simulator.get_mutable_context())
state = plan_runner_context.get_mutable_discrete_state_vector().get_mutable_value()
state[:self.nq] = q0
simulator.Initialize()
simulator.AdvanceTo(sim_duration)
return (iiwa_position_command_log, iiwa_position_measured_log,
iiwa_velocity_estimated_log, iiwa_external_torque_log, t_plan)
def build_station_real_world(builder, camera_ids):
station_hardware = ManipulationStationHardwareInterface(camera_ids)
station_hardware.Connect()
return station_hardware
def RunRealRobot(self, plans_list, gripper_setpoint_list):
"""
Constructs a Diagram that sends commands to ManipulationStationHardwareInterface.
@param plans_list: A list of Plans to be executed.
@param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan.
@return: logs of robot configuration and torque, decoded from LCM messges sent by the robot's driver.
Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data().
"""
builder = DiagramBuilder()
camera_ids = ["805212060544"]
station_hardware = ManipulationStationHardwareInterface(camera_ids)
station_hardware.Connect(wait_for_cameras=False)
builder.AddSystem(station_hardware)
# Add plan runner
# Add plan runner.
plan_runner = ManipStationPlanRunner(
station=self.station,
kuka_plans=plans_list,
gripper_setpoint_list=gripper_setpoint_list)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.hand_setpoint_output_port,
station_hardware.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
station_hardware.GetInputPort("wsg_force_limit"))
demux = builder.AddSystem(Demultiplexer(14, 7))
builder.Connect(
plan_runner.GetOutputPort("iiwa_position_and_torque_command"),
demux.get_input_port(0))
builder.Connect(demux.get_output_port(0),
station_hardware.GetInputPort("iiwa_position"))
builder.Connect(
demux.get_output_port(1),
station_hardware.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(
station_hardware.GetOutputPort("iiwa_position_measured"),
plan_runner.iiwa_position_input_port)
builder.Connect(
station_hardware.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.iiwa_velocity_input_port)
# Add logger
iiwa_position_command_log = LogOutput(demux.get_output_port(0),
builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
station_hardware.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
station_hardware.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
# build diagram
diagram = builder.Build()
# RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
print "sending trajectories in 2 seconds..."
time.sleep(1.0)
print "sending trajectories in 1 second..."
time.sleep(1.0)
print "sending trajectories now!"
simulator.StepTo(sim_duration)
return iiwa_position_command_log, \
iiwa_position_measured_log, iiwa_external_torque_log
def RunRealRobot(self, plans_list, gripper_setpoint_list):
'''
Constructs a Diagram that sends commands to ManipulationStationHardwareInterface.
:param plans_list:
:param gripper_setpoint_list:
:param extra_time:
:param real_time_rate:
:param q0_kuka:
:return:
'''
builder = DiagramBuilder()
camera_ids = ["805212060544"]
station_hardware = ManipulationStationHardwareInterface(camera_ids)
station_hardware.Connect(wait_for_cameras=False)
builder.AddSystem(station_hardware)
# Add plan runner
# Add plan runner.
plan_runner = ManipStationPlanRunner(
station=self.station,
kuka_plans=plans_list,
gripper_setpoint_list=gripper_setpoint_list)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.hand_setpoint_output_port,
station_hardware.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
station_hardware.GetInputPort("wsg_force_limit"))
demux = builder.AddSystem(Demultiplexer(14, 7))
builder.Connect(
plan_runner.GetOutputPort("iiwa_position_and_torque_command"),
demux.get_input_port(0))
builder.Connect(demux.get_output_port(0),
station_hardware.GetInputPort("iiwa_position"))
builder.Connect(
demux.get_output_port(1),
station_hardware.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(
station_hardware.GetOutputPort("iiwa_position_measured"),
plan_runner.iiwa_position_input_port)
builder.Connect(
station_hardware.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.iiwa_velocity_input_port)
# Add logger
iiwa_position_measured_log = LogOutput(
station_hardware.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
station_hardware.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
wsg_state_log = LogOutput(
station_hardware.GetOutputPort("wsg_state_measured"), builder)
wsg_state_log._DecalrePeriodicPublish(0.1)
wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port,
builder)
wsg_command_log._DeclarePeriodicPublish(0.1)
# build diagram
diagram = builder.Build()
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
print "sending trajectories in 2 seconds..."
time.sleep(1.0)
print "sending trajectories in 1 second..."
time.sleep(1.0)
print "sending trajectories now!"
simulator.StepTo(sim_duration)
return iiwa_position_measured_log, iiwa_external_torque_log, wsg_state_log, wsg_command_log
