def make_diagram(): # Use a nested function to ensure that all locals get garbage # collected quickly. # Construct a trivial plant and ID controller. # N.B. We explicitly do *not* add this plant to the diagram. controller_plant = MultibodyPlant(time_step=0.002) controller_plant.Finalize() builder = DiagramBuilder() controller = builder.AddSystem( InverseDynamicsController( controller_plant, kp=[], ki=[], kd=[], has_reference_acceleration=False, ) ) # Forward ports for ease of testing. builder.ExportInput( controller.get_input_port_estimated_state(), "x_estimated") builder.ExportInput( controller.get_input_port_desired_state(), "x_desired") builder.ExportOutput(controller.get_output_port_control(), "u") diagram = builder.Build() return diagram
def test_inverse_dynamics_controller(self): urdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/urdf/iiwa14_primitive_collision.urdf") tree = RigidBodyTree( urdf_path, floating_base_type=FloatingBaseType.kFixed) kp = np.array([1., 2., 3., 4., 5., 6., 7.]) ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5]) controller = InverseDynamicsController(robot=tree, kp=kp, ki=ki, kd=kd, has_reference_acceleration=True) context = controller.CreateDefaultContext() output = controller.AllocateOutput() estimated_state_port = 0 desired_state_port = 1 desired_acceleration_port = 2 control_port = 0 self.assertEqual( controller.get_input_port(desired_acceleration_port).size(), 7) self.assertEqual( controller.get_input_port(estimated_state_port).size(), 14) self.assertEqual( controller.get_input_port(desired_state_port).size(), 14) self.assertEqual( controller.get_output_port(control_port).size(), 7) # current state q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3]) v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9]) x = np.concatenate([q, v]) # reference state and acceleration q_r = q + 0.1*np.ones_like(q) v_r = v + 0.1*np.ones_like(v) x_r = np.concatenate([q_r, v_r]) vd_r = np.array([1., 2., 3., 4., 5., 6., 7.]) integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.]) vd_d = vd_r + kp*(q_r-q) + kd*(v_r-v) + ki*integral_term context.FixInputPort(estimated_state_port, BasicVector(x)) context.FixInputPort(desired_state_port, BasicVector(x_r)) context.FixInputPort(desired_acceleration_port, BasicVector(vd_r)) controller.set_integral_value(context, integral_term) # compute the expected torque cache = tree.doKinematics(q, v) expected_torque = tree.massMatrix(cache).dot(vd_d) + \ tree.dynamicsBiasTerm(cache, {}) controller.CalcOutput(context, output) self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(), expected_torque))
def test_inverse_dynamics_controller(self): sdf_path = FindResourceOrThrow( "drake/manipulation/models/" "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant(time_step=0.01) Parser(plant).AddModelFromFile(sdf_path) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) plant.mutable_gravity_field().set_gravity_vector([0.0, 0.0, 0.0]) plant.Finalize() # We verify the (known) size of the model. kNumPositions = 7 kNumVelocities = 7 kNumActuators = 7 kStateSize = kNumPositions + kNumVelocities self.assertEqual(plant.num_positions(), kNumPositions) self.assertEqual(plant.num_velocities(), kNumVelocities) self.assertEqual(plant.num_actuators(), kNumActuators) kp = np.array([1., 2., 3., 4., 5., 6., 7.]) ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5]) controller = InverseDynamicsController(robot=plant, kp=kp, ki=ki, kd=kd, has_reference_acceleration=True) context = controller.CreateDefaultContext() output = controller.AllocateOutput() estimated_state_port = controller.get_input_port(0) desired_state_port = controller.get_input_port(1) desired_acceleration_port = controller.get_input_port(2) control_port = controller.get_output_port(0) self.assertEqual(desired_acceleration_port.size(), kNumVelocities) self.assertEqual(estimated_state_port.size(), kStateSize) self.assertEqual(desired_state_port.size(), kStateSize) self.assertEqual(control_port.size(), kNumVelocities) # Current state. q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3]) v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9]) x = np.concatenate([q, v]) # Reference state and acceleration. q_r = q + 0.1*np.ones_like(q) v_r = v + 0.1*np.ones_like(v) x_r = np.concatenate([q_r, v_r]) vd_r = np.array([1., 2., 3., 4., 5., 6., 7.]) integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.]) vd_d = vd_r + kp*(q_r-q) + kd*(v_r-v) + ki*integral_term estimated_state_port.FixValue(context, x) desired_state_port.FixValue(context, x_r) desired_acceleration_port.FixValue(context, vd_r) controller.set_integral_value(context, integral_term) # Set the plant's context. plant_context = plant.CreateDefaultContext() x_plant = plant.GetMutablePositionsAndVelocities(plant_context) x_plant[:] = x # Compute the expected value of the generalized forces using # inverse dynamics. tau_id = plant.CalcInverseDynamics( plant_context, vd_d, MultibodyForces(plant)) # Verify the result. controller.CalcOutput(context, output) self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(), tau_id))
def test_decomposition_controller_like_workflow(self): """Tests subgraphs (post-finalize) for decomposition, with a scene-graph. Also shows a workflow of replacing joints / welding joints.""" builder = DiagramBuilder() # N.B. I (Eric) am using ManipulationStation because it's currently # the simplest way to get a complex scene in pydrake. station = ManipulationStation(time_step=0.001) station.SetupManipulationClassStation() station.Finalize() builder.AddSystem(station) plant = station.get_multibody_plant() scene_graph = station.get_scene_graph() iiwa = plant.GetModelInstanceByName("iiwa") wsg = plant.GetModelInstanceByName("gripper") if VISUALIZE: print("test_decomposition_controller_like_workflow") DrakeVisualizer.AddToBuilder(builder, station.GetOutputPort("query_object")) diagram = builder.Build() # Set the context with which things should be computed. d_context = diagram.CreateDefaultContext() context = plant.GetMyContextFromRoot(d_context) q_iiwa = [0.3, 0.7, 0.3, 0.6, 0.5, 0.6, 0.7] ndof = 7 q_wsg = [-0.03, 0.03] plant.SetPositions(context, iiwa, q_iiwa) plant.SetPositions(context, wsg, q_wsg) # Add copy of only the IIWA to a control diagram. control_builder = DiagramBuilder() control_plant = control_builder.AddSystem(MultibodyPlant(time_step=0)) # N.B. This has the same scene, but with all joints outside of the # IIWA frozen # TODO(eric.cousineau): Re-investigate weird "build_with_no_control" # behavior (with scene graph) with default conditions and time_step=0 # - see Anzu commit 2cf08cfc3. to_control = mut.add_plant_with_articulated_subset_to( plant_src=plant, scene_graph_src=scene_graph, articulated_models_src=[iiwa], context_src=context, plant_dest=control_plant) self.assertIsInstance(to_control, mut.MultibodyPlantElementsMap) control_iiwa = to_control.model_instances[iiwa] control_plant.Finalize() self.assertEqual(control_plant.num_positions(), plant.num_positions(iiwa)) kp = np.array([2000., 1500, 1500, 1500, 1500, 500, 500]) kd = np.sqrt(2 * kp) ki = np.zeros(7) controller = control_builder.AddSystem( InverseDynamicsController(robot=control_plant, kp=kp, ki=ki, kd=kd, has_reference_acceleration=False)) # N.B. Rather than use model instances for direct correspence, we could # use the mappings themselves within a custom system. control_builder.Connect( control_plant.get_state_output_port(control_iiwa), controller.get_input_port_estimated_state()) control_builder.Connect( controller.get_output_port_control(), control_plant.get_actuation_input_port(control_iiwa)) # Control to having the elbow slightly bent. q_iiwa_final = np.zeros(7) q_iiwa_final[3] = -np.pi / 2 t_start = 0. t_end = 1. nx = 2 * ndof def q_desired_interpolator(t: ContextTimeArg) -> VectorArg(nx): s = (t - t_start) / (t_end - t_start) ds = 1 / (t_end - t_start) q = q_iiwa + s * (q_iiwa_final - q_iiwa) v = ds * (q_iiwa_final - q_iiwa) x = np.hstack((q, v)) return x interpolator = control_builder.AddSystem( FunctionSystem(q_desired_interpolator)) control_builder.Connect(interpolator.get_output_port(0), controller.get_input_port_desired_state()) control_diagram = control_builder.Build() control_d_context = control_diagram.CreateDefaultContext() control_context = control_plant.GetMyContextFromRoot(control_d_context) to_control.copy_state(context, control_context) util.compare_frame_poses(plant, context, control_plant, control_context, "iiwa_link_0", "iiwa_link_7") util.compare_frame_poses(plant, context, control_plant, control_context, "body", "left_finger") from_control = to_control.inverse() def viz_monitor(control_d_context): # Simulate control, visualizing in original diagram. assert (control_context is control_plant.GetMyContextFromRoot(control_d_context)) from_control.copy_state(control_context, context) d_context.SetTime(control_d_context.get_time()) diagram.Publish(d_context) return EventStatus.DidNothing() simulator = Simulator(control_diagram, control_d_context) simulator.Initialize() if VISUALIZE: simulator.set_monitor(viz_monitor) simulator.set_target_realtime_rate(1.) simulator.AdvanceTo(t_end)
def test_inverse_dynamics_controller(self): sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant(time_step=0.01) Parser(plant).AddModelFromFile(sdf_path) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0")) plant.Finalize() # We verify the (known) size of the model. kNumPositions = 7 kNumVelocities = 7 kNumActuators = 7 kStateSize = kNumPositions + kNumVelocities self.assertEqual(plant.num_positions(), kNumPositions) self.assertEqual(plant.num_velocities(), kNumVelocities) self.assertEqual(plant.num_actuators(), kNumActuators) kp = np.array([1., 2., 3., 4., 5., 6., 7.]) ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5]) controller = InverseDynamicsController(robot=plant, kp=kp, ki=ki, kd=kd, has_reference_acceleration=True) context = controller.CreateDefaultContext() output = controller.AllocateOutput() estimated_state_port = 0 desired_state_port = 1 desired_acceleration_port = 2 control_port = 0 self.assertEqual( controller.get_input_port(desired_acceleration_port).size(), kNumVelocities) self.assertEqual( controller.get_input_port(estimated_state_port).size(), kStateSize) self.assertEqual( controller.get_input_port(desired_state_port).size(), kStateSize) self.assertEqual( controller.get_output_port(control_port).size(), kNumVelocities) # Current state. q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3]) v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9]) x = np.concatenate([q, v]) # Reference state and acceleration. q_r = q + 0.1*np.ones_like(q) v_r = v + 0.1*np.ones_like(v) x_r = np.concatenate([q_r, v_r]) vd_r = np.array([1., 2., 3., 4., 5., 6., 7.]) integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.]) vd_d = vd_r + kp*(q_r-q) + kd*(v_r-v) + ki*integral_term context.FixInputPort(estimated_state_port, BasicVector(x)) context.FixInputPort(desired_state_port, BasicVector(x_r)) context.FixInputPort(desired_acceleration_port, BasicVector(vd_r)) controller.set_integral_value(context, integral_term) # Set the plant's context. plant_context = plant.CreateDefaultContext() x_plant = plant.GetMutablePositionsAndVelocities(plant_context) x_plant[:] = x # Compute the expected value of the generalized forces using # inverse dynamics. tau_id = plant.CalcInverseDynamics( plant_context, vd_d, MultibodyForces(plant)) # Verify the result. controller.CalcOutput(context, output) self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(), tau_id))
def test_inverse_dynamics_controller(self): urdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/urdf/iiwa14_primitive_collision.urdf") tree = RigidBodyTree( urdf_path, floating_base_type=FloatingBaseType.kFixed) kp = np.array([1., 2., 3., 4., 5., 6., 7.]) ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5]) controller = InverseDynamicsController(robot=tree, kp=kp, ki=ki, kd=kd, has_reference_acceleration=True) context = controller.CreateDefaultContext() output = controller.AllocateOutput() estimated_state_port = 0 desired_state_port = 1 desired_acceleration_port = 2 control_port = 0 self.assertEqual( controller.get_input_port(desired_acceleration_port).size(), 7) self.assertEqual( controller.get_input_port(estimated_state_port).size(), 14) self.assertEqual( controller.get_input_port(desired_state_port).size(), 14) self.assertEqual( controller.get_output_port(control_port).size(), 7) # current state q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3]) v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9]) x = np.concatenate([q, v]) # reference state and acceleration q_r = q + 0.1*np.ones_like(q) v_r = v + 0.1*np.ones_like(v) x_r = np.concatenate([q_r, v_r]) vd_r = np.array([1., 2., 3., 4., 5., 6., 7.]) integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.]) vd_d = vd_r + kp*(q_r-q) + kd*(v_r-v) + ki*integral_term context.FixInputPort(estimated_state_port, BasicVector(x)) context.FixInputPort(desired_state_port, BasicVector(x_r)) context.FixInputPort(desired_acceleration_port, BasicVector(vd_r)) controller.set_integral_value(context, integral_term) # compute the expected torque cache = tree.doKinematics(q, v) expected_torque = tree.massMatrix(cache).dot(vd_d) + \ tree.dynamicsBiasTerm(cache, {}) controller.CalcOutput(context, output) self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(), expected_torque))
) 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") 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
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)