def test_multiplexer(self):
my_vector = MyVector2(data=[1., 2.])
test_cases = [
dict(has_vector=False,
mux=Multiplexer(num_scalar_inputs=4),
data=[[5.], [3.], [4.], [2.]]),
dict(has_vector=False,
mux=Multiplexer(input_sizes=[2, 3]),
data=[[8., 4.], [3., 6., 9.]]),
dict(has_vector=True,
mux=Multiplexer(model_vector=my_vector),
data=[[42.], [3.]]),
]
for case in test_cases:
mux = case['mux']
port_size = sum([len(vec) for vec in case['data']])
self.assertEqual(mux.get_output_port(0).size(), port_size)
context = mux.CreateDefaultContext()
output = mux.AllocateOutput()
num_ports = len(case['data'])
self.assertEqual(context.get_num_input_ports(), num_ports)
for j, vec in enumerate(case['data']):
context.FixInputPort(j, BasicVector(vec))
mux.CalcOutput(context, output)
self.assertTrue(
np.allclose(
output.get_vector_data(0).get_value(),
[elem for vec in case['data'] for elem in vec]))
if case['has_vector']:
# Check the type matches MyVector2.
value = output.get_vector_data(0)
self.assertTrue(isinstance(value, MyVector2))
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)
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")
builder.Connect(diff_arm_demux.get_output_port(0), arm_hand_mux.get_input_port(0))
builder.Connect(hand_comms.get_output_port(), arm_hand_mux.get_input_port(1))
# builder.Connect(s_interp.get_output_port(), arm_hand_mux.get_input_port(0))
# builder.Connect(h_interp.get_output_port(), arm_hand_mux.get_input_port(1))
# arm_hand_mux.get_input_port(1).FixValue(np.array([0.0, 0.0, 0.0, 0.0]))
# Connect result of integrator to a derivative getter
s_interp = builder.AddSystem(StateInterpolatorWithDiscreteDerivative(9, time_step, True))
s_interp.set_name("s_interp")
builder.Connect(arm_hand_mux.get_output_port(), s_interp.get_input_port())
builder.Connect(s_interp.get_output_port(), inv_d_controller.GetInputPort("desired_state"))
# Connect inverse dynamics control into desired actuation of panda