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)