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}")