예제 #1
0
    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)
예제 #2
0
    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)
예제 #3
0
    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)