def euler_from_quaternion(quaternion):
import math
x, y, z, w = quaternion[0], quaternion[1], quaternion[2], quaternion[3]
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll_x = math.atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch_y = math.asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
roll_z = math.atan2(t3, t4)
return roll_x, pitch_y, roll_z
python 四元数转欧拉角RPY
最新推荐文章于 2024-05-12 20:43:33 发布