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)