def finalize_func():
builder = self._builder
builder.Connect(gripper_controller.get_generalized_force_output_port(),
self._mbp.get_actuation_input_port(gripper_model_id))
# Add Gripper ports
if arm_end_frame is None:
# Gripper is not welded
# State goes [_, r, p, y, x, y, z, g1, g2, dr, dp, dy, dx, dy, dz, dg1, dg2]
# Gripper state
demux = self._builder.AddSystem(Demultiplexer([1, 6, 2, 6, 2]))
mux = self._builder.AddSystem(Multiplexer(input_sizes=[2, 2]))
builder.Connect(self._mbp.get_state_output_port(gripper_model_id),
demux.get_input_port(0))
builder.Connect(demux.get_output_port(2),
mux.get_input_port(0))
builder.Connect(demux.get_output_port(4),
mux.get_input_port(1))
builder.Connect(mux.get_output_port(0),
gripper_controller.get_state_input_port())
builder.Connect(mux.get_output_port(0),
mbp_to_gripper_state.get_input_port(0))
else:
builder.Connect(self._mbp.get_state_output_port(gripper_model_id),
gripper_controller.get_state_input_port())
builder.Connect(self._mbp.get_state_output_port(gripper_model_id),
mbp_to_gripper_state.get_input_port(0))
gripper_position_name = gripper_name + "_position"
gripper_force_limit_name = gripper_name + "_force_limit"
gripper_state_name = gripper_name + "_state_measured"
gripper_force_meas_name = gripper_name + "_force_measured"
gripper_gen_force_meas_name = gripper_name + "_gen_force_measured"
gripper_full_state = gripper_name + "_full_state"
self._port_names.extend([gripper_position_name, gripper_force_limit_name,
gripper_state_name, gripper_force_meas_name,
gripper_gen_force_meas_name, gripper_full_state])
builder.ExportInput(gripper_controller.get_desired_position_input_port(),
gripper_position_name)
builder.ExportInput(gripper_controller.get_force_limit_input_port(),
gripper_force_limit_name)
builder.ExportOutput(mbp_to_gripper_state.get_output_port(0),
gripper_state_name)
builder.ExportOutput(gripper_controller.get_grip_force_output_port(),
gripper_force_meas_name)
builder.ExportOutput(gripper_controller.get_generalized_force_output_port(),
gripper_gen_force_meas_name)
builder.ExportOutput(self._mbp.get_state_output_port(gripper_model_id),
gripper_full_state)
if arm_end_frame is None:
gripper_spatial_position = gripper_name + "_spatial_position"
gripper_spatial_velocity = gripper_name + "_spatial_velocity"
self._port_names.extend([gripper_spatial_position, gripper_spatial_velocity])
builder.ExportOutput(demux.get_output_port(1),
gripper_spatial_position)
builder.ExportOutput(demux.get_output_port(3),
gripper_spatial_velocity)
def test_demultiplexer(self):
# Test demultiplexer with scalar outputs.
demux = Demultiplexer(size=4)
context = demux.CreateDefaultContext()
self.assertEqual(demux.get_num_input_ports(), 1)
self.assertEqual(demux.get_num_output_ports(), 4)
input_vec = np.array([1., 2., 3., 4.])
context.FixInputPort(0, BasicVector(input_vec))
output = demux.AllocateOutput()
demux.CalcOutput(context, output)
for i in range(4):
self.assertTrue(
np.allclose(output.get_vector_data(i).get_value(),
input_vec[i]))
# Test demultiplexer with vector outputs.
demux = Demultiplexer(size=4, output_ports_sizes=2)
context = demux.CreateDefaultContext()
self.assertEqual(demux.get_num_input_ports(), 1)
self.assertEqual(demux.get_num_output_ports(), 2)
context.FixInputPort(0, BasicVector(input_vec))
output = demux.AllocateOutput()
demux.CalcOutput(context, output)
for i in range(2):
self.assertTrue(
np.allclose(output.get_vector_data(i).get_value(),
input_vec[2*i:2*i+2]))
def connect_plan_runner(builder, station, plan):
from manipulation_station.manipulation_station_plan_runner import ManipStationPlanRunner
plan_list, gripper_setpoints = plan
# TODO: disable the move home trajectories at the beginning of the plan
# Add plan runner.
plan_runner = ManipStationPlanRunner(station, plan_list, gripper_setpoints)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.hand_setpoint_output_port,
station.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
station.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.GetInputPort("iiwa_position"))
builder.Connect(demux.get_output_port(1),
station.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(station.GetOutputPort("iiwa_position_measured"),
plan_runner.iiwa_position_input_port)
builder.Connect(station.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_external_torque_log = LogOutput(
station.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
station.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
wsg_state_log = LogOutput(station.GetOutputPort("wsg_state_measured"),
builder)
wsg_state_log._DeclarePeriodicPublish(0.1)
wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port, builder)
wsg_command_log._DeclarePeriodicPublish(0.1)
return plan_runner
def connect_plan_runner(builder, station, plan):
from plan_runner.manipulation_station_plan_runner import ManipStationPlanRunner
plan_list, gripper_setpoints = plan
# Add plan runner.
plan_runner = ManipStationPlanRunner(plan_list, gripper_setpoints)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.hand_setpoint_output_port,
station.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
station.GetInputPort("wsg_force_limit"))
demux = builder.AddSystem(Demultiplexer(14, 7))
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"))
# Add logger
iiwa_position_command_log = LogOutput(
plan_runner.GetOutputPort("iiwa_position_command"), builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
station.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
station.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
wsg_state_log = LogOutput(station.GetOutputPort("wsg_state_measured"),
builder)
wsg_state_log._DeclarePeriodicPublish(0.1)
wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port, builder)
wsg_command_log._DeclarePeriodicPublish(0.1)
return plan_runner
def test_demultiplexer(self):
# Test demultiplexer with scalar outputs.
demux = Demultiplexer(size=4)
context = demux.CreateDefaultContext()
self.assertEqual(demux.get_num_input_ports(), 1)
self.assertEqual(demux.get_num_output_ports(), 4)
input_vec = np.array([1., 2., 3., 4.])
context.FixInputPort(0, BasicVector(input_vec))
output = demux.AllocateOutput()
demux.CalcOutput(context, output)
for i in range(4):
self.assertTrue(
np.allclose(
output.get_vector_data(i).get_value(), input_vec[i]))
# Test demultiplexer with vector outputs.
demux = Demultiplexer(size=4, output_ports_sizes=2)
context = demux.CreateDefaultContext()
self.assertEqual(demux.get_num_input_ports(), 1)
self.assertEqual(demux.get_num_output_ports(), 2)
context.FixInputPort(0, BasicVector(input_vec))
output = demux.AllocateOutput()
demux.CalcOutput(context, output)
for i in range(2):
self.assertTrue(
np.allclose(
output.get_vector_data(i).get_value(),
input_vec[2 * i:2 * i + 2]))
def test_demultiplexer(self):
# Test demultiplexer with scalar outputs.
demux = Demultiplexer(size=4)
context = demux.CreateDefaultContext()
self.assertEqual(demux.num_input_ports(), 1)
self.assertEqual(demux.num_output_ports(), 4)
numpy_compare.assert_equal(demux.get_output_ports_sizes(),
[1, 1, 1, 1])
input_vec = np.array([1., 2., 3., 4.])
demux.get_input_port(0).FixValue(context, input_vec)
output = demux.AllocateOutput()
demux.CalcOutput(context, output)
for i in range(4):
self.assertTrue(
np.allclose(
output.get_vector_data(i).get_value(), input_vec[i]))
# Test demultiplexer with vector outputs.
demux = Demultiplexer(size=4, output_ports_size=2)
context = demux.CreateDefaultContext()
self.assertEqual(demux.num_input_ports(), 1)
self.assertEqual(demux.num_output_ports(), 2)
numpy_compare.assert_equal(demux.get_output_ports_sizes(), [2, 2])
demux.get_input_port(0).FixValue(context, input_vec)
output = demux.AllocateOutput()
demux.CalcOutput(context, output)
for i in range(2):
self.assertTrue(
np.allclose(
output.get_vector_data(i).get_value(),
input_vec[2 * i:2 * i + 2]))
# Test demultiplexer with different output port sizes.
output_ports_sizes = np.array([1, 2, 1])
num_output_ports = output_ports_sizes.size
input_vec = np.array([1., 2., 3., 4.])
demux = Demultiplexer(output_ports_sizes=output_ports_sizes)
context = demux.CreateDefaultContext()
self.assertEqual(demux.num_input_ports(), 1)
self.assertEqual(demux.num_output_ports(), num_output_ports)
numpy_compare.assert_equal(demux.get_output_ports_sizes(),
output_ports_sizes)
demux.get_input_port(0).FixValue(context, input_vec)
output = demux.AllocateOutput()
demux.CalcOutput(context, output)
output_port_start = 0
for i in range(num_output_ports):
output_port_size = output.get_vector_data(i).size()
self.assertTrue(
np.allclose(
output.get_vector_data(i).get_value(),
input_vec[output_port_start:output_port_start +
output_port_size]))
output_port_start += output_port_size
def build_manipulation_station(station):
from underactuated.meshcat_visualizer import MeshcatVisualizer
from .manipulation_station.manipulation_station_plan_runner import ManipStationPlanRunner
builder = DiagramBuilder()
builder.AddSystem(station)
# Add plan runner.
plan_runner = ManipStationPlanRunner(station=station)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.hand_setpoint_output_port,
station.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
station.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.GetInputPort("iiwa_position"))
builder.Connect(demux.get_output_port(1),
station.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(station.GetOutputPort("iiwa_position_measured"),
plan_runner.iiwa_position_input_port)
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.iiwa_velocity_input_port)
# Add meshcat visualizer
plant = station.get_mutable_multibody_plant()
scene_graph = station.get_mutable_scene_graph()
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=True,
plant=plant)
builder.AddSystem(viz)
builder.Connect(station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
builder.Connect(station.GetOutputPort("contact_results"),
viz.GetInputPort("contact_results"))
# Add logger
iiwa_position_command_log = LogOutput(demux.get_output_port(0), builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
station.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
station.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
wsg_state_log = LogOutput(station.GetOutputPort("wsg_state_measured"),
builder)
wsg_state_log._DeclarePeriodicPublish(0.1)
wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port, builder)
wsg_command_log._DeclarePeriodicPublish(0.1)
# build diagram
diagram = builder.Build()
viz.load()
#time.sleep(2.0)
#RenderSystemWithGraphviz(diagram)
return diagram, plan_runner
def __init__(self):
#import pdb; pdb.set_trace()
# Create a block diagram containing our system.
builder = DiagramBuilder()
# add the two decoupled plants (x(s)=u/s^2; y(s)=u/s^2)
plant_x = builder.AddSystem(DoubleIntegrator())
plant_y = builder.AddSystem(DoubleIntegrator())
plant_x.set_name("double_integrator_x")
plant_y.set_name("double_integrator_y")
# add the controller, lead compensator for now just to stablize it
controller_x = builder.AddSystem(
Controller(tau_num=2., tau_den=.2, k=1.))
controller_y = builder.AddSystem(
Controller(tau_num=2., tau_den=.2, k=1.))
controller_x.set_name("controller_x")
controller_y.set_name("controller_y")
# the perception's "Beam" model with the four sources of noise
x_meas_fb = builder.AddSystem(Adder(num_inputs=4, size=1))
x_meas_fb.set_name("x_fb")
y_meas_fb = builder.AddSystem(Adder(num_inputs=4, size=1))
y_meas_fb.set_name("y_fb")
y_perception = builder.AddSystem(Adder(num_inputs=2, size=1))
y_perception.set_name("range_measurment_y")
neg_y_meas = builder.AddSystem(Gain(k=-1., size=1))
neg_y_meas.set_name("neg_y_meas")
wall_position = builder.AddSystem(ConstantVectorSource([Y_wall]))
p_hit = builder.AddSystem(RandomSource(distribution=RandomDistribution.kGaussian, num_outputs=2,\
sampling_interval_sec=0.01))
p_hit.set_name("GaussionNoise(0,1)")
p_short = builder.AddSystem(RandomSource(distribution=RandomDistribution.kExponential, num_outputs=2,\
sampling_interval_sec=0.01))
p_short.set_name("ExponentialNoise(1)")
#p_max = builder.AddSystem(RandomSource(distribution=RandomDistribution.kUniform, num_outputs=1,\
# sampling_interval_sec=0.01))
p_rand = builder.AddSystem(RandomSource(distribution=RandomDistribution.kUniform, num_outputs=2,\
sampling_interval_sec=0.01))
p_rand.set_name("UniformNoise(0,1)")
#import pdb; pdb.set_trace()
p_hit_Dx = builder.AddSystem(Demultiplexer(size=2))
p_hit_Dx.set_name('Dmux1')
p_short_Dx = builder.AddSystem(Demultiplexer(size=2))
p_short_Dx.set_name('Dmux2')
p_rand_Dx = builder.AddSystem(Demultiplexer(size=2))
p_rand_Dx.set_name('Dmux3')
normgain_x = builder.AddSystem(Gain(k=normal_coeff, size=1))
normgain_x.set_name("Sigma_x")
normgain_y = builder.AddSystem(Gain(k=normal_coeff, size=1))
normgain_y.set_name("Sigma_y")
expgain_x = builder.AddSystem(Gain(k=exp_coeff, size=1))
expgain_x.set_name("Exp_x")
expgain_y = builder.AddSystem(Gain(k=exp_coeff, size=1))
expgain_y.set_name("Exp_y")
randgain_x = builder.AddSystem(Gain(k=rand_coeff, size=1))
randgain_x.set_name("Rand_x")
randgain_y = builder.AddSystem(Gain(k=rand_coeff, size=1))
randgain_y.set_name("Rand_y")
#maxgain_x = builder.AddSystem(Adder(num_inputs=2, size=1))
#maxgain_x.set_name("Max_x")
#maxgain_y = builder.AddSystem(Adder(num_inputs=2, size=1))
#maxgain_y.set_name("Max_y")
# the summation to get the error (closing the loop)
summ_x = builder.AddSystem(Adder(num_inputs=2, size=1))
summ_y = builder.AddSystem(Adder(num_inputs=2, size=1))
summ_x.set_name("summation_x")
summ_y.set_name("summation_y")
neg_x = builder.AddSystem(Gain(k=-1., size=1))
neg_y = builder.AddSystem(Gain(k=-1., size=1))
neg_uy = builder.AddSystem(Gain(k=-1., size=1))
neg_x.set_name("neg_x")
neg_y.set_name("neg_y")
neg_uy.set_name("neg_uy")
# wire up all the blocks (summation to the controller to the plant ...)
builder.Connect(summ_x.get_output_port(0),
controller_x.get_input_port(0)) # e_x
builder.Connect(summ_y.get_output_port(0),
controller_y.get_input_port(0)) # e_y
builder.Connect(controller_x.get_output_port(0),
plant_x.get_input_port(0)) # u_x
builder.Connect(
controller_y.get_output_port(0),
neg_uy.get_input_port(0)) # u_y (to deal with directions)
builder.Connect(neg_uy.get_output_port(0),
plant_y.get_input_port(0)) # u_y
builder.Connect(
plant_x.get_output_port(0),
x_meas_fb.get_input_port(0)) # perception, nominal state meas
builder.Connect(wall_position.get_output_port(0),
y_perception.get_input_port(0)) # perception
builder.Connect(plant_y.get_output_port(0),
neg_y_meas.get_input_port(0)) # perception
builder.Connect(neg_y_meas.get_output_port(0),
y_perception.get_input_port(1)) # perception, z meas
builder.Connect(
y_perception.get_output_port(0),
y_meas_fb.get_input_port(0)) # perception, nominal state meas
builder.Connect(p_hit.get_output_port(0),
p_hit_Dx.get_input_port(0)) # demux the noise
builder.Connect(p_hit_Dx.get_output_port(0),
normgain_x.get_input_port(0)) # normalize Normal dist
builder.Connect(normgain_x.get_output_port(0),
x_meas_fb.get_input_port(1)) # Normal dist
builder.Connect(p_hit_Dx.get_output_port(1),
normgain_y.get_input_port(0)) # normalize Normal dist
builder.Connect(normgain_y.get_output_port(0),
y_meas_fb.get_input_port(1)) # Normal dist
builder.Connect(p_short.get_output_port(0),
p_short_Dx.get_input_port(0)) # demux the noise
builder.Connect(p_short_Dx.get_output_port(0),
expgain_x.get_input_port(0)) # normalize Exp dist
builder.Connect(expgain_x.get_output_port(0),
x_meas_fb.get_input_port(2)) # Exp dist
builder.Connect(p_short_Dx.get_output_port(1),
expgain_y.get_input_port(0)) # normalize Exp dist
builder.Connect(expgain_y.get_output_port(0),
y_meas_fb.get_input_port(2)) # Exp dist
#builder.Connect(p_max.get_output_port(0), x_meas_fb.get_input_port(3)) # Uniform dist
#builder.Connect(p_max.get_output_port(0), y_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(p_rand.get_output_port(0),
p_rand_Dx.get_input_port(0)) # normalize Uniform dist
builder.Connect(p_rand_Dx.get_output_port(0),
randgain_x.get_input_port(0)) # normalize Uniform dist
builder.Connect(randgain_x.get_output_port(0),
x_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(p_rand_Dx.get_output_port(1),
randgain_y.get_input_port(0)) # normalize Uniform dist
builder.Connect(randgain_y.get_output_port(0),
y_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(x_meas_fb.get_output_port(0),
neg_x.get_input_port(0)) # -x
builder.Connect(y_meas_fb.get_output_port(0),
neg_y.get_input_port(0)) # -y
builder.Connect(neg_x.get_output_port(0),
summ_x.get_input_port(1)) # r-x
builder.Connect(neg_y.get_output_port(0),
summ_y.get_input_port(1)) # r-y
# Make the desired_state input of the controller, an input to the diagram.
builder.ExportInput(summ_x.get_input_port(0))
builder.ExportInput(summ_y.get_input_port(0))
# get telemetry
logger_x = LogOutput(plant_x.get_output_port(0), builder)
logger_noise_x = LogOutput(x_meas_fb.get_output_port(0), builder)
logger_y = LogOutput(plant_y.get_output_port(0), builder)
logger_noise_y = LogOutput(y_meas_fb.get_output_port(0), builder)
logger_x.set_name("logger_x_state")
logger_y.set_name("logger_y_state")
logger_noise_x.set_name("x_state_meas")
logger_noise_y.set_name("y_meas")
# compile the system
diagram = builder.Build()
diagram.set_name("diagram")
# Create the simulator, and simulate for 10 seconds.
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# First we extract the subsystem context for the plant
plant_context_x = diagram.GetMutableSubsystemContext(plant_x, context)
plant_context_y = diagram.GetMutableSubsystemContext(plant_y, context)
# Then we can set the pendulum state, which is (x, y).
z0 = X_0
plant_context_x.get_mutable_continuous_state_vector().SetFromVector(z0)
z0 = Y_0
plant_context_y.get_mutable_continuous_state_vector().SetFromVector(z0)
# The diagram has a single input port (port index 0), which is the desired_state.
context.FixInputPort(
0,
[X_d]) # here we assume no perception, closing feedback on state X
context.FixInputPort(
1, [Z_d]) #Z_y, keep 3m away, basically we want to be at Y=0
# run the simulation
simulator.AdvanceTo(10)
t = logger_x.sample_times()
#import pdb; pdb.set_trace()
# Plot the results.
plt.figure()
plot_system_graphviz(diagram, max_depth=2)
plt.figure()
plt.plot(t, logger_noise_x.data().transpose(), label='x_state_meas')
plt.plot(t, logger_noise_y.data().transpose(), label='y_meas (z(y))')
plt.plot(t, logger_x.data().transpose(), label='x_state')
plt.plot(t, logger_y.data().transpose(), label='y_state')
plt.xlabel('t')
plt.ylabel('x(t),y(t)')
plt.legend()
plt.show(block=True)
controller_plant.WeldFrames(controller_plant.world_frame(), controller_plant.GetFrameByName("panda_link0", control_only_panda), X_AB=RigidTransform(np.array([0.0, 0.5, 0.0])))
controller_plant.Finalize()
diff_inv_controller = builder.AddSystem(
DifferentialInverseKinematicsIntegrator(controller_plant,
controller_plant.GetFrameByName("panda_hand", control_only_panda),
time_step,
DifferentialInverseKinematicsParameters(num_positions=9, num_velocities=9))
)
diff_inv_controller.set_name("Inverse Kinematics")
rb_conv = builder.AddSystem(TrajToRB(traj_p_G, traj_R_G))
rb_conv.set_name("RB Conv")
builder.Connect(rb_conv.get_output_port(), diff_inv_controller.get_input_port())
diff_arm_demux = builder.AddSystem(Demultiplexer(np.array([7, 2])))
diff_arm_demux.set_name("Diff Arm Demux")
builder.Connect(diff_inv_controller.get_output_port(), diff_arm_demux.get_input_port())
kp = np.full(9, 100)
ki = np.full(9, 1)
kd = 2 * np.sqrt(kp)
inv_d_controller = builder.AddSystem(InverseDynamicsController(controller_plant, kp, ki, kd, False))
inv_d_controller.set_name("inv_d")
builder.Connect(plant.get_state_output_port(panda), inv_d_controller.get_input_port_estimated_state())
hand_comms = builder.AddSystem(GripperTrajectoriesToPosition(controller_plant, traj_h))
arm_hand_mux = builder.AddSystem(Multiplexer(np.array([7, 2])))
arm_hand_mux.set_name("Arm-Hand Mux")
def add_arm_gripper(self, arm_name, arm_path, arm_base, X_arm,
gripper_path, arm_ee, gripper_base, X_gripper):
# Add arm
parser = Parser(self._mbp, self._sg)
arm_model_id = parser.AddModelFromFile(arm_path, arm_name)
arm_base_frame = self._mbp.GetFrameByName(arm_base, arm_model_id)
self._mbp.WeldFrames(self._mbp.world_frame(), arm_base_frame, X_arm)
self._model_ids[arm_name] = arm_model_id
# Add gripper
gripper_name = arm_name+"_gripper"
arm_end_frame = self._mbp.GetFrameByName(arm_ee, arm_model_id)
self.add_floating_gripper(gripper_name, gripper_path, arm_end_frame, gripper_base, X_gripper)
# Add arm controller stack
ctrl_plant = MultibodyPlant(0)
parser = Parser(ctrl_plant)
ctrl_arm_id = parser.AddModelFromFile(arm_path, arm_name)
arm_base_frame = ctrl_plant.GetFrameByName(arm_base, ctrl_arm_id)
ctrl_plant.WeldFrames(ctrl_plant.world_frame(), arm_base_frame, X_arm)
gripper_equivalent = ctrl_plant.AddRigidBody(
gripper_name+"_equivalent", ctrl_arm_id,
self.calculate_ee_composite_inertia(gripper_path))
arm_end_frame = ctrl_plant.GetFrameByName(arm_ee, ctrl_arm_id)
ctrl_plant.WeldFrames(arm_end_frame, gripper_equivalent.body_frame(),
X_gripper)
ctrl_plant.Finalize()
self._control_mbp[arm_name] = ctrl_plant
arm_num_positions = ctrl_plant.num_positions(ctrl_arm_id)
kp = 4000*np.ones(arm_num_positions)
ki = 0 * np.ones(arm_num_positions)
kd = 5*np.sqrt(kp)
arm_controller = self._builder.AddSystem(InverseDynamicsController(
ctrl_plant, kp, ki, kd, False))
adder = self._builder.AddSystem(Adder(2, arm_num_positions))
state_from_position = self._builder.AddSystem(
StateInterpolatorWithDiscreteDerivative(
arm_num_positions, self._mbp.time_step(), True))
# Add command pass through and state splitter
arm_command = self._builder.AddSystem(PassThrough(arm_num_positions))
state_split = self._builder.AddSystem(Demultiplexer(
2*arm_num_positions, arm_num_positions))
def finalize_func():
builder = self._builder
# Export positions commanded
command_input_name = arm_name + "_position"
command_output_name = arm_name + "_position_commanded"
self._port_names.extend([command_input_name, command_output_name])
builder.ExportInput(arm_command.get_input_port(0), command_input_name)
builder.ExportOutput(arm_command.get_output_port(0), command_output_name)
# Export arm state ports
builder.Connect(self._mbp.get_state_output_port(arm_model_id),
state_split.get_input_port(0))
arm_q_name = arm_name + "_position_measured"
arm_v_name = arm_name + "_velocity_estimated"
arm_state_name = arm_name + "_state_measured"
self._port_names.extend([arm_q_name, arm_v_name, arm_state_name])
builder.ExportOutput(state_split.get_output_port(0), arm_q_name)
builder.ExportOutput(state_split.get_output_port(1), arm_v_name)
builder.ExportOutput(self._mbp.get_state_output_port(arm_model_id),
arm_state_name)
# Export controller stack ports
builder.Connect(self._mbp.get_state_output_port(arm_model_id),
arm_controller.get_input_port_estimated_state())
builder.Connect(arm_controller.get_output_port_control(),
adder.get_input_port(0))
builder.Connect(adder.get_output_port(0),
self._mbp.get_actuation_input_port(arm_model_id))
builder.Connect(state_from_position.get_output_port(0),
arm_controller.get_input_port_desired_state())
builder.Connect(arm_command.get_output_port(0),
state_from_position.get_input_port(0))
torque_input_name = arm_name + "_feedforward_torque"
torque_output_cmd_name = arm_name + "_torque_commanded"
torque_output_est_name = arm_name + "_torque_measured"
self._port_names.extend([torque_input_name, torque_output_cmd_name,
torque_output_est_name])
builder.ExportInput(adder.get_input_port(1), torque_input_name)
builder.ExportOutput(adder.get_output_port(0), torque_output_cmd_name)
builder.ExportOutput(adder.get_output_port(0), torque_output_est_name)
external_torque_name = arm_name + "_torque_external"
self._port_names.append(external_torque_name)
builder.ExportOutput(
self._mbp.get_generalized_contact_forces_output_port(arm_model_id),
external_torque_name)
self._finalize_functions.append(finalize_func)
def RunSimulation(self,
plans_list,
gripper_setpoint_list,
extra_time=0,
real_time_rate=1.0,
q0_kuka=np.zeros(7),
is_visualizing=True):
"""
Constructs a Diagram that sends commands to ManipulationStation.
@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.
@param extra_time: Time in seconds that the simulation is run,
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_kuka: initial configuration of the robot.
@param is_visualizing: if true, adds MeshcatVisualizer to the Diagram. It should be set to False
when running tests.
@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.
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,
self.station.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
self.station.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),
self.station.GetInputPort("iiwa_position"))
builder.Connect(demux.get_output_port(1),
self.station.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(self.station.GetOutputPort("iiwa_position_measured"),
plan_runner.iiwa_position_input_port)
builder.Connect(self.station.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.iiwa_velocity_input_port)
# Add meshcat visualizer
if is_visualizing:
scene_graph = self.station.get_mutable_scene_graph()
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=True,
plant=self.plant)
builder.AddSystem(viz)
builder.Connect(self.station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
builder.Connect(self.station.GetOutputPort("contact_results"),
viz.GetInputPort("contact_results"))
# Add logger
iiwa_position_command_log = LogOutput(demux.get_output_port(0),
builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
self.station.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
self.station.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
plant_state_log = LogOutput(
self.station.GetOutputPort("plant_continuous_state"), builder)
plant_state_log._DeclarePeriodicPublish(0.005)
# build diagram
diagram = builder.Build()
if is_visualizing:
viz.load()
time.sleep(2.0)
# RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
context = diagram.GetMutableSubsystemContext(
self.station, simulator.get_mutable_context())
# set initial state of the robot
self.station.SetIiwaPosition(q0_kuka, context)
self.station.SetIiwaVelocity(np.zeros(7), context)
self.station.SetWsgPosition(0.05, context)
self.station.SetWsgVelocity(0, context)
# 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=-0.001)
right_hinge_joint = self.plant.GetJointByName("right_door_hinge")
right_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=0.001)
# set initial pose of the object
if self.object_base_link_name is not None:
self.plant.tree().SetFreeBodyPoseOrThrow(
self.plant.GetBodyByName(self.object_base_link_name,
self.object), self.X_WObject,
self.station.GetMutableSubsystemContext(self.plant, context))
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
sim_duration += extra_time
print "simulation duration", sim_duration
simulator.StepTo(sim_duration)
return iiwa_position_command_log, \
iiwa_position_measured_log, \
iiwa_external_torque_log, \
plant_state_log
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 build(self, real_time_rate=0, is_visualizing=False):
# Create manipulation station simulator
self.manip_station_sim = ManipulationStationSimulator(
time_step=5e-3,
object_file_path=object_file_path,
object_base_link_name="base_link",
)
# Create plan runner
plan_runner, self.plan_scheduler = CreateManipStationPlanRunnerDiagram(
station=self.manip_station_sim.station,
kuka_plans=[],
gripper_setpoint_list=[],
rl_environment=True)
self.manip_station_sim.plan_runner = plan_runner
# Create builder and add systems
builder = DiagramBuilder()
builder.AddSystem(self.manip_station_sim.station)
builder.AddSystem(plan_runner)
# Connect systems
builder.Connect(
plan_runner.GetOutputPort("gripper_setpoint"),
self.manip_station_sim.station.GetInputPort("wsg_position"))
builder.Connect(
plan_runner.GetOutputPort("force_limit"),
self.manip_station_sim.station.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),
self.manip_station_sim.station.GetInputPort("iiwa_position"))
builder.Connect(
demux.get_output_port(1),
self.manip_station_sim.station.GetInputPort(
"iiwa_feedforward_torque"))
builder.Connect(
self.manip_station_sim.station.GetOutputPort(
"iiwa_position_measured"),
plan_runner.GetInputPort("iiwa_position"))
builder.Connect(
self.manip_station_sim.station.GetOutputPort(
"iiwa_velocity_estimated"),
plan_runner.GetInputPort("iiwa_velocity"))
# Add meshcat visualizer
if is_visualizing:
scene_graph = self.manip_station_sim.station.get_mutable_scene_graph(
)
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=False,
plant=self.manip_station_sim.plant)
builder.AddSystem(viz)
builder.Connect(
self.manip_station_sim.station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
# Build diagram
self.diagram = builder.Build()
if is_visualizing:
print("Setting up visualizer...")
viz.load()
time.sleep(2.0)
# Construct Simulator
self.simulator = Simulator(self.diagram)
self.manip_station_sim.simulator = self.simulator
self.simulator.set_publish_every_time_step(False)
self.simulator.set_target_realtime_rate(real_time_rate)
self.context = self.diagram.GetMutableSubsystemContext(
self.manip_station_sim.station,
self.simulator.get_mutable_context())
self.left_hinge_joint = self.manip_station_sim.plant.GetJointByName(
"left_door_hinge")
self.right_hinge_joint = self.manip_station_sim.plant.GetJointByName(
"right_door_hinge")
# Goal for training
self.goal_position = np.array([0.85, 0, 0.31])
# Object body
self.obj = self.manip_station_sim.plant.GetBodyByName(
self.manip_station_sim.object_base_link_name,
self.manip_station_sim.object)
# Properties for RL
max_action = np.ones(8) * 0.1
max_action[-1] = 0.03
low_action = -1 * max_action
low_action[-1] = 0
self.action_space = ActionSpace(low=low_action, high=max_action)
self.state_dim = self._getObservation().shape[0]
self._episode_steps = 0
self._max_episode_steps = 75
# Set initial state of the robot
self.reset_sim = False
self.reset()
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 = CreateManipStationPlanRunnerDiagram(
station=self.station,
kuka_plans=plan_list,
gripper_setpoint_list=gripper_setpoint_list,
print_period=0,)
else:
plan_runner = ManipStationPlanRunner(
station=self.station,
kuka_plans=plan_list,
gripper_setpoint_list=gripper_setpoint_list,
print_period=0,)
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"))
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.GetInputPort("iiwa_position"))
builder.Connect(station_hardware.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.GetInputPort("iiwa_velocity"))
# 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)
self.simulator = simulator
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
t_plan = GetPlanStartingTimes(plan_list)
if sim_duration is None:
sim_duration = t_plan[-1] + extra_time
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 RunSimulation(self,
plans_list,
gripper_setpoint_list,
extra_time=0,
real_time_rate=1.0,
q0_kuka=np.zeros(7)):
'''
Constructs a Diagram that sends commands to ManipulationStation.
:param plans_list:
:param gripper_setpoint_list:
:param extra_time:
:param real_time_rate:
:param q0_kuka:
:return:
'''
builder = DiagramBuilder()
builder.AddSystem(self.station)
# 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,
self.station.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
self.station.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),
self.station.GetInputPort("iiwa_position"))
builder.Connect(demux.get_output_port(1),
self.station.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(self.station.GetOutputPort("iiwa_position_measured"),
plan_runner.iiwa_position_input_port)
builder.Connect(self.station.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.iiwa_velocity_input_port)
# Add meshcat visualizer
scene_graph = self.station.get_mutable_scene_graph()
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=True,
plant=self.plant)
self.viz = viz
builder.AddSystem(viz)
builder.Connect(self.station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
builder.Connect(self.station.GetOutputPort("contact_results"),
viz.GetInputPort("contact_results"))
# Add logger
iiwa_position_command_log = LogOutput(demux.get_output_port(0),
builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
self.station.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
self.station.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
wsg_state_log = LogOutput(
self.station.GetOutputPort("wsg_state_measured"), builder)
wsg_state_log._DeclarePeriodicPublish(0.1)
wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port,
builder)
wsg_command_log._DeclarePeriodicPublish(0.1)
# build diagram
diagram = builder.Build()
viz.load()
time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
context = diagram.GetMutableSubsystemContext(
self.station, simulator.get_mutable_context())
# set initial state of the robot
self.station.SetIiwaPosition(q0_kuka, context)
self.station.SetIiwaVelocity(np.zeros(7), context)
self.station.SetWsgPosition(0.05, context)
self.station.SetWsgVelocity(0, context)
# 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=-0.001)
right_hinge_joint = self.plant.GetJointByName("right_door_hinge")
right_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=0.001)
# set initial pose of the object
self.plant.tree().SetFreeBodyPoseOrThrow(
self.plant.GetBodyByName(self.object_base_link_name, self.object),
self.X_WObject,
self.station.GetMutableSubsystemContext(self.plant, context))
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
sim_duration += extra_time
print "simulation duration", sim_duration
simulator.StepTo(sim_duration)
return iiwa_position_command_log, \
iiwa_position_measured_log, \
iiwa_external_torque_log, \
wsg_state_log, \
wsg_command_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
