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)
