def test_state_interpolator_with_discrete_derivative(self): state_interpolator = StateInterpolatorWithDiscreteDerivative( num_positions=5, time_step=0.4) self.assertEqual(state_interpolator.get_input_port(0).size(), 5) self.assertEqual(state_interpolator.get_output_port(0).size(), 10) # test set_initial_position using context context = state_interpolator.CreateDefaultContext() state_interpolator.set_initial_position(context=context, position=5 * [1.1]) np.testing.assert_array_equal( context.get_discrete_state_vector().CopyToVector(), np.array(10 * [1.1])) # test set_initial_position using state context = state_interpolator.CreateDefaultContext() state_interpolator.set_initial_position(state=context.get_state(), position=5 * [1.3]) np.testing.assert_array_equal( context.get_discrete_state_vector().CopyToVector(), np.array(10 * [1.3]))
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 builder.Connect(inv_d_controller.get_output_port_control(), plant.get_actuation_input_port()) logger = LogOutput(diff_inv_controller.get_output_port(), builder) diagram = builder.Build() diagram.set_name("pick_and_place") simulator = Simulator(diagram) sim_context = simulator.get_mutable_context()
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)