python from rotation matrix to euler angle

可以使用Python的NumPy库来将旋转矩阵转换为欧拉角。以下是一个示例代码：

import numpy as np

def rotation_matrix_to_euler_angles(R):
    # Extract sin and cos of pitch angle
    sin_pitch = R[2,0]
    cos_pitch = np.sqrt(1 - sin_pitch**2)

    # Check if pitch is close to +/- 90 degrees
    if np.abs(cos_pitch) < 1e-6:
        # Roll is zero so just set yaw
        yaw = np.arctan2(R[1,2], R[0,2])
        pitch = np.pi/2 if sin_pitch > 0 else -np.pi/2
        roll = 0
    else:
        # Compute roll and yaw
        yaw = np.arctan2(R[1,0]/cos_pitch, R[0,0]/cos_pitch)
        roll = np.arctan2(R[2,1]/cos_pitch, R[2,2]/cos_pitch)

        # Ensure yaw stays within [-pi, pi]
        if yaw < -np.pi:
            yaw += 2*np.pi
        elif yaw > np.pi:
            yaw -= 2*np.pi

        # Ensure roll stays within [-pi/2, pi/2]
        if roll < -np.pi/2:
            roll += np.pi
            yaw += np.pi
            pitch = np.pi - np.arctan2(-sin_pitch, np.sqrt(R[2,1]**2 + R[2,2]**2))
        elif roll > np.pi/2:
            roll -= np.pi
            yaw += np.pi
            pitch = -np.pi + np.arctan2(-sin_pitch, np.sqrt(R[2,1]**2 + R[2,2]**2))
        else:
            pitch = np.arcsin(sin_pitch)

    return np.array([roll, pitch, yaw])

其中，输入参数 `R` 是一个 $3 \times 3$ 的旋转矩阵，输出是一个包含三个欧拉角（roll、pitch、yaw）的 NumPy 数组。请注意，欧拉角的顺序是 roll-pitch-yaw。