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