python 四元数到欧拉角

vi quart_to_rpy.py

import sys
import math

w = float(sys.argv[4])
x = float(sys.argv[1])
y = float(sys.argv[2])
z = float(sys.argv[3])

r = math.atan2(2*(w*x+y*z),1-2*(x*x+y*y))
p = math.asin(2*(w*y-z*x))
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)