26 lines
750 B
Python
26 lines
750 B
Python
|
import math
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
def rotation_matrix_to_euler_zyx(R):
|
||
|
|
if abs(R[2,0]) != 1:
|
||
|
|
theta = -math.asin(R[2,0])
|
||
|
|
cos_theta = math.cos(theta)
|
||
|
|
phi = math.atan2(R[2,1]/cos_theta, R[2,2]/cos_theta)
|
||
|
|
psi = math.atan2(R[1,0]/cos_theta, R[0,0]/cos_theta)
|
||
|
|
else:
|
||
|
|
phi = 0
|
||
|
|
if R[2,0] == -1:
|
||
|
|
theta = math.pi/2
|
||
|
|
psi = phi + math.atan2(R[0,1], R[0,2])
|
||
|
|
else:
|
||
|
|
theta = -math.pi/2
|
||
|
|
psi = -phi + math.atan2(-R[0,1], -R[0,2])
|
||
|
|
return phi, theta, psi # roll, pitch, yaw
|
||
|
|
|
||
|
|
|
||
|
|
R = np.array([[-1, 0, 0],
|
||
|
|
[0, 0, -1],
|
||
|
|
[0, -1, 0]])
|
||
|
|
|
||
|
|
roll, pitch, yaw = rotation_matrix_to_euler_zyx(R)
|
||
|
|
print("roll:", roll, "pitch:", pitch, "yaw:", yaw)
|