utils.py 文件源码

def quat2euler(q):
    """
    Returns the euler representation (in degrees) of a quaternion. Note, the
    heading is wrapped between 0-360 degrees.

    In:
        [q0 q1 q2 q3] = [w x y z]
    out:
        (roll, pitch, yaw) in degrees
    """
    q0, q1, q2, q3 = q
    roll = atan2(2.0*q2*q3-2.0*q0*q1, 2.0*q0*q0+2.0*q3*q3-1.0)
    pitch = -asin(2.0*q1*q3+2.0*q0*q2)
    heading = atan2(2.0*q1*q2-2.0*q0*q3, 2.0*q0*q0+2.0*q1*q1-1.0)

    heading = heading if heading <= 2*pi else heading-2*pi
    heading = heading if heading >= 0 else heading+2*pi

    return map(rad2deg, (roll, pitch, heading))