等效旋转矢量转换为欧拉角 python代码编写

以下是一个简单的python函数，将等效旋转矢量（axis-angle）转换为欧拉角（ZYX顺序）：

import numpy as np

def axis_angle_to_euler(axis_angle):
    """
    Convert axis-angle representation to Euler angles in ZYX order.
    """
    # Normalize axis
    axis = axis_angle[:3] / np.linalg.norm(axis_angle[:3])
    # Compute angle
    angle = axis_angle[3]
    # Compute cosine and sine of angle
    c = np.cos(angle)
    s = np.sin(angle)
    # Compute rotation matrix
    R = np.array([[c + axis[0]**2*(1-c), axis[0]*axis[1]*(1-c) - axis[2]*s, axis[0]*axis[2]*(1-c) + axis[1]*s],
                  [axis[1]*axis[0]*(1-c) + axis[2]*s, c + axis[1]**2*(1-c), axis[1]*axis[2]*(1-c) - axis[0]*s],
                  [axis[2]*axis[0]*(1-c) - axis[1]*s, axis[2]*axis[1]*(1-c) + axis[0]*s, c + axis[2]**2*(1-c)]])
    # Extract Euler angles from rotation matrix
    sy = np.sqrt(R[0,0]**2 + R[1,0]**2)
    if sy > 1e-6:
        # Non-singular case
        x = np.arctan2(R[2,1], R[2,2])
        y = np.arctan2(-R[2,0], sy)
        z = np.arctan2(R[1,0], R[0,0])
    else:
        # Singular case
        x = np.arctan2(-R[1,2], R[1,1])
        y = np.arctan2(-R[2,0], sy)
        z = 0
    # Return Euler angles in radians
    return np.array([z, y, x])

其中，axis_angle是四元组 [x, y, z, theta]，分别表示旋转轴的方向向量（x,y,z）和旋转角度（theta）。欧拉角以ZYX顺序返回，即yaw、pitch、roll。注意，这个函数假设输入的旋转矢量已经被单位化了。