欧拉角和四元数之间的python转换代码

1、欧拉角转换到四元数，转换顺序是ZYX

def rpy2quaternion(roll, pitch, yaw):
    x=sin(roll/2)*cos(pitch/2)*cos(yaw/2)-cos(roll/2)*sin(pitch/2)*sin(yaw/2)
    y=cos(roll/2)*sin(pitch/2)*cos(yaw/2)+sin(roll/2)*cos(pitch/2)*sin(yaw/2)
    z=cos(roll/2)*cos(pitch/2)*sin(yaw/2)-sin(roll/2)*sin(pitch/2)*cos(yaw/2)
    w=cos(roll/2)*cos(pitch/2)*cos(yaw/2)+sin(roll/2)*sin(pitch/2)*sin(yaw/2)
    return np.array([w, x, y, z ])

2、四元数转换到欧拉角，转换顺序是ZYX

def quart_to_rpy(w,x, y, z):
    roll = atan2(2 * (w * x + y * z), 1 - 2 * (x ** 2 + y ** 2))
    pitch = asin(2 * (w * y - z * x))
    yaw = atan2(2 * (w * z + x * y), 1 - 2 * (y ** 2 + z ** 2))

    return np.array([roll, pitch, yaw])

实验的时候发现，欧拉角转到四元数，再从四元数转回来，有些角度存在问题。