Python MultibodyPlant.mutable_gravity_field方法代码示例

# 需要导入模块: from pydrake.multibody.plant import MultibodyPlant [as 别名]
# 或者: from pydrake.multibody.plant.MultibodyPlant import mutable_gravity_field [as 别名]
    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)

# 需要导入模块: from pydrake.multibody.plant import MultibodyPlant [as 别名]
# 或者: from pydrake.multibody.plant.MultibodyPlant import mutable_gravity_field [as 别名]
    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 = 0
        desired_state_port = 1
        desired_acceleration_port = 2
        control_port = 0

        self.assertEqual(
            controller.get_input_port(desired_acceleration_port).size(),
            kNumVelocities)
        self.assertEqual(
            controller.get_input_port(estimated_state_port).size(), kStateSize)
        self.assertEqual(
            controller.get_input_port(desired_state_port).size(), kStateSize)
        self.assertEqual(
            controller.get_output_port(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

        context.FixInputPort(estimated_state_port, BasicVector(x))
        context.FixInputPort(desired_state_port, BasicVector(x_r))
        context.FixInputPort(desired_acceleration_port, BasicVector(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))