ros python 四元数 转 欧拉角

 

import sys
import math

w = -0.99114048481
x = -0.00530699081719
y = 0.00178255140781
z = -0.133612662554

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))

angleR = r*180/math.pi
angleP = p*180/math.pi
angleY = y*180/math.pi

print (angleR)#翻滚
print (angleP)#俯仰
print (angleY)#偏航

输出：
0.575472843396
-2.24876083545
15.3574378019

https://blog.csdn.net/pengcan985632236/article/details/85230675
https://blog.csdn.net/u012700322/article/details/52252305
https://blog.csdn.net/zhouxinlin2009/article/details/81050028
https://blog.csdn.net/dinnerhowe/article/details/79569281
https://blog.csdn.net/super_mice/article/details/45619945
https://blog.csdn.net/wwlcsdn000/article/details/79421612#comments

 

转载于:https://www.cnblogs.com/sea-stream/p/11158056.html