Function test_kinodynamics_diff

tests/python/test_continuous_dynamics.py:360–420

()

    not HAS_PINOCCHIO, reason="Aligator was compiled without Pinocchio."
)
def test_kinodynamics_diff():
    import pinocchio as pin

    model = pin.buildSampleModelHumanoid()
    contact_ids = [
        model.getFrameId("lleg_effector_body"),
        model.getFrameId("rleg_effector_body"),
        model.getFrameId("rarm_effector_body"),
    ]
    # Test for 3D contact
    force_size = 3
    nk = 3
    nu = force_size * nk + model.nv - 6
    space = manifolds.MultibodyPhaseSpace(model)
    mass = 0
    for inertia in model.inertias:
        mass += inertia.mass
    gravity = np.array([0, 0, -9.81])
    contact_states = [True, True, False]

    ode = dynamics.KinodynamicsFwdDynamics(
        space, model, gravity, contact_states, contact_ids, force_size
    )
    data = ode.createData()

    x0 = space.neutral()
    ndx = space.ndx
    dx = np.random.randn(ndx)
    x0 = space.integrate(x0, dx)
    u0 = np.random.randn(nu)
    epsilon = 1e-6

    ode.forward(x0, u0, data)
    ode.dForward(x0, u0, data)

    Jx0 = data.Jx.copy()
    Ju0 = data.Ju.copy()
    Jxdiff, Judiff = ode_finite_difference(ode, space, x0, u0, epsilon)

    assert np.linalg.norm(Jxdiff - Jx0) <= epsilon
    assert np.linalg.norm(Judiff - Ju0) <= epsilon

    # Test for 6D contact
    force_size = 6
    nu = force_size * nk + model.nv - 6

    ode = dynamics.KinodynamicsFwdDynamics(
        space, model, gravity, contact_states, contact_ids, force_size
    )
    data = ode.createData()
    u0 = np.random.randn(nu)

    ode.forward(x0, u0, data)
    ode.dForward(x0, u0, data)

    Jx0 = data.Jx.copy()
    Ju0 = data.Ju.copy()
    Jxdiff, Judiff = ode_finite_difference(ode, space, x0, u0, epsilon)