Function test_centroidal_diff

tests/python/test_continuous_dynamics.py:159–208

()

def test_centroidal_diff():
    # Test for 3D contact
    nx = 9
    space = manifolds.VectorSpace(nx)
    force_size = 3
    nk = 3
    nu = force_size * nk
    mass = 10.5
    gravity = np.array([0, 0, -9.81])
    contact_names = ["foot1", "foot2", "foot3"]
    contact_states = [True, True, False]
    contact_poses = [
        np.array([0, 0.1, 0]),
        np.array([0.1, -0.1, 0]),
        np.array([0.1, 0.2, 0]),
    ]
    contact_map = aligator.ContactMap(contact_names, contact_states, contact_poses)
    ode = dynamics.CentroidalFwdDynamics(space, mass, gravity, contact_map, force_size)
    data = ode.createData()

    x0 = np.random.randn(nx)
    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)

    assert np.allclose(Jxdiff, Jx0, epsilon), "err={}".format(infNorm(Jxdiff - Jx0))
    assert np.allclose(Judiff, Ju0, epsilon), "err={}".format(infNorm(Judiff - Ju0))

    # Test for 6D contact
    force_size = 6
    nu = force_size * nk
    u0 = np.random.randn(nu)

    ode = dynamics.CentroidalFwdDynamics(space, mass, gravity, contact_map, force_size)
    data = ode.createData()

    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.allclose(Jxdiff, Jx0, epsilon), "err={}".format(infNorm(Jxdiff - Jx0))
    assert np.allclose(Judiff, Ju0, epsilon), "err={}".format(infNorm(Judiff - Ju0))


def test_continuous_centroidal():