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 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)
# 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)