ros Python 四元數轉歐拉角

Python 版本

def to_euler_angles(w, x, y, z):
    """w、x、y、z to euler angles"""
    angles = {'pitch': 0.0, 'roll': 0.0, 'yaw': 0.0}
    r = math.atan2(2*(w*x+y*z),1-2*(x*x+y*y))
    p = math.asin(2*(w*y-z*z))
    y = math.atan2(2*(w*z+x*y),1-2*(z*z+y*y))

    angles['roll'] = r*180/math.pi
    angles['pitch'] = p*180/math.pi
    angles['yaw'] = y*180/math.pi

    return angles

C++ 版本

ROS中quaternion四元數和RPY歐拉角轉換

---

參考：
ros python 四元數 轉 歐拉角