def ManipulatorDynamics(plant, q, v=None):
if isinstance(plant, RigidBodyTree):
# TODO(russt): Zap this branch once I'm finished porting to MBP.
if v is None:
v = np.zeros((plant.get_num_velocities(), 1))
kinsol = plant.doKinematics(q, v)
M = plant.massMatrix(kinsol)
tauG = -plant.dynamicsBiasTerm(kinsol, {}, False)
Cv = plant.dynamicsBiasTerm(kinsol, {}, True) + tauG
B = plant.B
tauExt = 0 * tauG
else:
# MultibodyPlant version
assert (isinstance(plant, MultibodyPlant))
context = plant.CreateDefaultContext()
plant.SetPositions(context, q)
if v is not None:
plant.SetVelocities(context, v)
M = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
tauG = plant.CalcGravityGeneralizedForces(context)
B = plant.MakeActuationMatrix()
forces = MultibodyForces(plant)
plant.CalcForceElementsContribution(context, forces)
tauExt = forces.generalized_forces()
return (M, Cv, tauG, B, tauExt)
def test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
H = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(H.shape == (2, 2))
self.assert_sane(H)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2, ))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
self.assertEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv, ))
self.assert_sane(tau_g, nonzero=False)
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
def test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
# Getting ready for when we set foot on Mars :-).
plant.AddForceElement(UniformGravityFieldElement([0.0, 0.0, -3.71]))
plant.Finalize()
context = plant.CreateDefaultContext()
# Set an arbitrary configuration away from the model's fixed point.
plant.SetPositions(context, [0.1, 0.2])
H = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(H.shape == (2, 2))
self.assert_sane(H)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2, ))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
# Gravity leads to non-zero potential energy.
self.assertNotEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv, ))
self.assert_sane(tau_g, nonzero=True)
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
def ManipulatorDynamics(plant, q, v=None):
if isinstance(plant, RigidBodyTree):
# TODO(russt): Zap this branch once I'm finished porting to MBP.
if v is None:
v = np.zeros((plant.get_num_velocities(),1))
kinsol = plant.doKinematics(q,v)
M = plant.massMatrix(kinsol)
tauG = -plant.dynamicsBiasTerm(kinsol, {}, False)
Cv = plant.dynamicsBiasTerm(kinsol, {}, True) + tauG
B = plant.B
tauExt = 0*tauG
else:
# MultibodyPlant version
assert(isinstance(plant, MultibodyPlant))
context = plant.CreateDefaultContext()
plant.SetPositions(context, q)
if v is not None:
plant.SetVelocities(context, v)
M = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
tauG = plant.CalcGravityGeneralizedForces(context)
B = plant.MakeActuationMatrix()
forces = MultibodyForces(plant)
plant.CalcForceElementsContribution(context, forces)
tauExt = forces.generalized_forces()
return (M, Cv, tauG, B, tauExt)
def test_deprecated_tree_api(self):
plant = MultibodyPlant()
plant.Finalize()
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
num_expected_warnings = [0]
def expect_new_warning(msg_part):
num_expected_warnings[0] += 1
self.assertEqual(len(w), num_expected_warnings[0])
self.assertIn(msg_part, str(w[-1].message))
tree = plant.tree()
expect_new_warning("`tree()`")
MobilizerIndex(0)
expect_new_warning("`MobilizerIndex`")
BodyNodeIndex(0)
expect_new_warning("`BodyNodeIndex`")
MultibodyForces(model=tree)
expect_new_warning("`MultibodyForces(plant)`")
element = plant.world_body()
self.assertIsInstance(element.get_parent_tree(), MultibodyTree)
expect_new_warning("`get_parent_tree()`")
# Check old spellings (no deprecation warnings).
self.check_old_spelling_exists(tree.CalcRelativeTransform)
self.check_old_spelling_exists(tree.CalcPointsPositions)
self.check_old_spelling_exists(
tree.CalcFrameGeometricJacobianExpressedInWorld)
self.check_old_spelling_exists(tree.EvalBodyPoseInWorld)
self.check_old_spelling_exists(tree.SetFreeBodyPoseOrThrow)
self.check_old_spelling_exists(tree.SetFreeBodySpatialVelocityOrThrow)
self.check_old_spelling_exists(tree.EvalBodySpatialVelocityInWorld)
self.check_old_spelling_exists(tree.GetPositionsFromArray)
self.check_old_spelling_exists(tree.GetVelocitiesFromArray)
self.check_old_spelling_exists(tree.CalcMassMatrixViaInverseDynamics)
self.check_old_spelling_exists(tree.CalcBiasTerm)
self.check_old_spelling_exists(tree.CalcInverseDynamics)
self.check_old_spelling_exists(tree.num_frames)
self.check_old_spelling_exists(tree.get_body)
self.check_old_spelling_exists(tree.get_joint)
self.check_old_spelling_exists(tree.get_joint_actuator)
self.check_old_spelling_exists(tree.get_frame)
self.check_old_spelling_exists(tree.GetModelInstanceName)
context = plant.CreateDefaultContext()
# All body poses.
X_WB, = tree.CalcAllBodyPosesInWorld(context)
self.assertIsInstance(X_WB, Isometry3)
v_WB, = tree.CalcAllBodySpatialVelocitiesInWorld(context)
self.assertIsInstance(v_WB, SpatialVelocity)
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_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
# Getting ready for when we set foot on Mars :-).
gravity_vector = np.array([0.0, 0.0, -3.71])
plant.mutable_gravity_field().set_gravity_vector(gravity_vector)
np.testing.assert_equal(plant.gravity_field().gravity_vector(),
gravity_vector)
plant.Finalize()
context = plant.CreateDefaultContext()
# Set an arbitrary configuration away from the model's fixed point.
plant.SetPositions(context, [0.1, 0.2])
M = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(M.shape == (2, 2))
self.assert_sane(M)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(
context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2,))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
# Gravity leads to non-zero potential energy.
self.assertNotEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv,))
self.assert_sane(tau_g, nonzero=True)
B = plant.MakeActuationMatrix()
np.testing.assert_equal(B, np.array([[0.], [1.]]))
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
# Test generalized forces.
forces.mutable_generalized_forces()[:] = 1
np.testing.assert_equal(forces.generalized_forces(), 1)
forces.SetZero()
np.testing.assert_equal(forces.generalized_forces(), 0)
# Test body force accessors and mutators.
link2 = plant.GetBodyByName("Link2")
self.assertIsInstance(
link2.GetForceInWorld(context, forces), SpatialForce)
forces.SetZero()
F_expected = np.array([1, 2, 3, 4, 5, 6])
link2.AddInForceInWorld(
context, F_Bo_W=SpatialForce(F=F_expected), forces=forces)
np.testing.assert_equal(
link2.GetForceInWorld(context, forces).get_coeffs(), F_expected)
link2.AddInForce(
context, p_BP_E=[0, 0, 0], F_Bp_E=SpatialForce(F=F_expected),
frame_E=plant.world_frame(), forces=forces)
# Also check accumulation.
np.testing.assert_equal(
link2.GetForceInWorld(context, forces).get_coeffs(),
2 * F_expected)
def test_multibody_dynamics(self):
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
# Getting ready for when we set foot on Mars :-).
gravity_vector = np.array([0.0, 0.0, -3.71])
plant.mutable_gravity_field().set_gravity_vector(gravity_vector)
np.testing.assert_equal(plant.gravity_field().gravity_vector(),
gravity_vector)
plant.Finalize()
context = plant.CreateDefaultContext()
# Set an arbitrary configuration away from the model's fixed point.
plant.SetPositions(context, [0.1, 0.2])
M = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(M.shape == (2, 2))
self.assert_sane(M)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2, ))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
# Gravity leads to non-zero potential energy.
self.assertNotEqual(plant.CalcPotentialEnergy(context), 0)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv, ))
self.assert_sane(tau_g, nonzero=True)
B = plant.MakeActuationMatrix()
np.testing.assert_equal(B, np.array([[0.], [1.]]))
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
# Test generalized forces.
forces.mutable_generalized_forces()[:] = 1
np.testing.assert_equal(forces.generalized_forces(), 1)
forces.SetZero()
np.testing.assert_equal(forces.generalized_forces(), 0)
# Test body force accessors and mutators.
link2 = plant.GetBodyByName("Link2")
self.assertIsInstance(link2.GetForceInWorld(context, forces),
SpatialForce)
forces.SetZero()
F_expected = np.array([1, 2, 3, 4, 5, 6])
link2.AddInForceInWorld(context,
F_Bo_W=SpatialForce(F=F_expected),
forces=forces)
np.testing.assert_equal(
link2.GetForceInWorld(context, forces).get_coeffs(), F_expected)
link2.AddInForce(context,
p_BP_E=[0, 0, 0],
F_Bp_E=SpatialForce(F=F_expected),
frame_E=plant.world_frame(),
forces=forces)
# Also check accumulation.
np.testing.assert_equal(
link2.GetForceInWorld(context, forces).get_coeffs(),
2 * F_expected)
print("Acrobot bias term at q = ", q, "and v = ", v, "is Cv = ")
print(Cv)
# We can separately get the gravitational effects
N = plant.CalcGravityGeneralizedForces(context)
print("Acrobot gravitational generalized forces at q = ", q, " is N = ")
print(N)
# Evaluating the controls
nU = plant.num_actuated_dofs()
nA = plant.num_actuators()
print('Acrobot has ', nU, ' actuated joint(s) and ', nA, ' actuator(s)')
# We can get the actuation matrix, a permutation matrix as:
B = plant.MakeActuationMatrix()
print('The acutator selection matrix for acrobot is B = ')
print(B)
# Note that calculating the dynamics in this fashion is not computationally efficient. It would be more efficient to use plant.CalcInverseDynamics instead, given the generalized acceleration and applied forces
# Create empty generalized applied forces
forces = MultibodyForces(plant)
forces.SetZero()
# Create some generalized accelerations
dv = [0.2, 0.6]
# Do inverse dynamics to find the generalized forces needed to achieve these accelerations
# NOTE: INVERSE DYNAMICS DOES NOT AUTOMATICALLY ENCODE THE GRAVITATIONAL FORCES
tau = plant.CalcInverseDynamics(context, dv, forces)
print(f"Inverse dynamics without gravity f = {tau}")
# To encode generalized forces - including gravity - we can add them in after the fact, or add them in to the MultibodyForces
force = forces.mutable_generalized_forces()
force[:] = N
tau_2 = plant.CalcInverseDynamics(context, dv, forces)
print(f"Inverse dynamics with gravity f = {tau_2}")