Function function_finite_difference

tests/python/utils.py:113–143

Use finite differences to compute Jacobians of a `aligator.StageFunction`.

(
    fun: aligator.StageFunction,
    space: manifolds.ManifoldAbstract,
    x0,
    u0,
    eps=1e-8,
)

def function_finite_difference(
    fun: aligator.StageFunction,
    space: manifolds.ManifoldAbstract,
    x0,
    u0,
    eps=1e-8,
):
    """Use finite differences to compute Jacobians
    of a `aligator.StageFunction`.
    """
    data = fun.createData()
    Jx_nd = np.zeros((fun.nr, fun.ndx1))
    ei = np.zeros(fun.ndx1)
    fun.evaluate(x0, u0, data)
    r0 = data.value.copy()
    for i in range(fun.ndx1):
        ei[i] = eps
        xplus = space.integrate(x0, ei)
        fun.evaluate(xplus, u0, data)
        Jx_nd[:, i] = (data.value - r0) / eps
        ei[i] = 0.0

    ei = np.zeros(fun.nu)
    Ju_nd = np.zeros((fun.nr, fun.nu))
    for i in range(fun.nu):
        ei[i] = eps
        fun.evaluate(x0, u0 + ei, data)
        Ju_nd[:, i] = (data.value - r0) / eps
        ei[i] = 0.0

    return Jx_nd, Ju_nd


def set_baumgarte_params(cm: "pin.RigidConstraintModel", Kp: float, Kd: float = None):