def test_inverse_dynamics(self):
urdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/urdf/iiwa14_primitive_collision.urdf")
tree = RigidBodyTree(urdf_path,
floating_base_type=FloatingBaseType.kFixed)
num_v = tree.get_num_velocities()
def compute_torque(tree, q, v, v_dot):
cache = tree.doKinematics(q, v)
return tree.massMatrix(cache).dot(v_dot) + \
tree.dynamicsBiasTerm(cache, {})
estimated_state_port = 0
desired_acceleration_port = 1
def check_torque_example(controller, q, v, v_dot_desired=None):
if controller.is_pure_gravity_compensation():
v_dot_desired = np.zeros(num_v)
context = controller.CreateDefaultContext()
x = np.concatenate([q, v])
context.FixInputPort(estimated_state_port, BasicVector(x))
if not controller.is_pure_gravity_compensation():
context.FixInputPort(desired_acceleration_port,
BasicVector(v_dot_desired))
output = controller.AllocateOutput()
controller.CalcOutput(context, output)
expected_torque = compute_torque(tree, q, v, v_dot_desired)
self.assertTrue(
np.allclose(
output.get_vector_data(0).CopyToVector(), expected_torque))
# Test with pure gravity compensation.
controller = InverseDynamics(
tree=tree,
mode=InverseDynamics.InverseDynamicsMode.kGravityCompensation)
q = np.array([.1, .2, .3, .4, .5, .6, .7])
v = np.zeros(num_v)
check_torque_example(controller, q, v)
# Test with desired acceleration.
controller = InverseDynamics(
tree=tree,
mode=InverseDynamics.InverseDynamicsMode.kInverseDynamics)
q = np.array([.7, .6, .5, .4, .3, .2, .1])
v = np.array([-.1, -.2, -.3, -.4, -.5, -.6, -.7])
v_dot_desired = np.array([-.1, .1, -.1, .1, -.1, .1, -.1])
check_torque_example(controller, q, v, v_dot_desired)
def test_inverse_dynamics(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()
# Just test that the constructor doesn't throw.
controller = InverseDynamics(
plant=plant,
mode=InverseDynamics.InverseDynamicsMode.kGravityCompensation)