Function test_angular_acceleration

tests/python/test_centroidal.py:312–403

()

def test_angular_acceleration():
    x, d, x0 = sample_gauss(space)
    force_size = 3
    nu = force_size * nk
    u0 = np.random.randn(nu)

    contact_names = ["c1", "c2", "c3", "c4"]
    contact_states = [True, False, True, True]
    contact_poses = [
        np.array([0.2, 0.1, 0.0]),
        np.array([0.2, 0.0, 0.0]),
        np.array([0.0, 0.1, 0.0]),
        np.array([0.0, 0.0, 0]),
    ]
    contact_map = aligator.ContactMap(contact_names, contact_states, contact_poses)

    fun = aligator.AngularAccelerationResidual(
        ndx, nu, mass, gravity, contact_map, force_size
    )
    fdata = fun.createData()

    fun.evaluate(x0, u0, fdata)

    Ldot = np.zeros(3)
    for i in range(nk):
        if contact_states[i]:
            Ldot += np.cross(contact_poses[i] - x0[:3], u0[i * 3 : (i + 1) * 3])

    assert np.allclose(fdata.value, Ldot)

    fun_fd = aligator.FiniteDifferenceHelper(space, fun, FD_EPS)
    fdata2 = fun_fd.createData()
    fun_fd.evaluate(x0, u0, fdata2)
    assert np.allclose(fdata.value, fdata2.value)

    fun_fd.computeJacobians(x0, u0, fdata2)
    J_fd = fdata2.Jx
    J_fd_u = fdata2.Ju
    assert fdata.Jx.shape == J_fd.shape
    assert fdata.Ju.shape == J_fd_u.shape

    for i in range(100):
        du = np.random.randn(nu) * sample_factor
        u1 = u0 + du
        x, d, x0 = sample_gauss(space)
        fun.evaluate(x0, u1, fdata)
        fun.computeJacobians(x0, u1, fdata)
        fun_fd.evaluate(x0, u1, fdata2)
        fun_fd.computeJacobians(x0, u1, fdata2)
        assert np.allclose(fdata.Ju, fdata2.Ju, THRESH)
        assert np.allclose(fdata.Jx, fdata2.Jx, THRESH)

    force_size = 6
    nu = force_size * nk
    u0 = np.random.randn(nu)

    fun = aligator.AngularAccelerationResidual(
        ndx, nu, mass, gravity, contact_map, force_size