四元数转欧拉角roll pitch yaw ,rpy的python实现

四元数对应 x，y,z,w ，欧拉角对应 roll pitch yaw 。

在轮式机器人中如果只想获取yaw

def toeuler_angles(w, x, y, z):
    """w、x、y、z to euler angles"""
    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= y*180/math.pi
    return angles

如果想获取pitch 角，上述中 p = math.asin(2(wy-z*z)) 会出现math domain error .
可以这么写

      double q2sqr = y * y;

      // roll (x-axis rotation)
      double t0 = +2.0 * (w * x + y * z);
      double t1 = +1.0 - 2.0 * (x * x + q2sqr);
      double roll = atan2(t0, t1) * 180.0 / PI;

      // pitch (y-axis rotation)
      double t2 = +2.0 * (w * y - z * x);
      t2 = t2 > 1.0 ? 1.0 : t2;
      t2 = t2 < -1.0 ? -1.0 : t2;
      double pitch = asin(t2) * 180.0 / PI;

      // yaw (z-axis rotation)
      double t3 = +2.0 * (w * z + x * y);
      double t4 = +1.0 - 2.0 * (q2sqr + z * z);
      double yaw = atan2(t3, t4) * 180.0 / PI;