fip_dynamics.py 文件源码

def integrate_fip(p, v, z, dt, omega2):
    """
    Integrate the equation of motion of the Floating-base Inverted Pendulum.

    Parameters
    ----------
    p : array, shape=(3,)
        Initial position.
    v : array, shape=(3,)
        Initial velocity.
    z : array, shape=(3,)
        ZMP location throughout the integration.
    dt : scalar
        Integration step.
    omega2 : scalar
        FIP constant.

    Returns
    -------
    p_next : array, shape=(3,)
        Position at the end of the integration step.
    v_next : array, shape=(3,)
        Velocity at the end of the integration step.

    Note
    ----
    The Linear Inverted Pendulum Mode (LIPM) is a special case of the FIP, so
    this function also applies to COP-based controllers.
    """
    omega = sqrt(omega2)
    a = omega2 * (p - z) + gravity
    p_next = p + v / omega * sinh(omega * dt) \
        + a / omega2 * (cosh(omega * dt) - 1.)
    v_next = v * cosh(omega * dt) + a / omega * sinh(omega * dt)
    return p_next, v_next