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

以下是Python代码实现欧拉角转等效旋转矢量的方法：

import math
import numpy as np

def euler_to_vector(phi, theta, psi):
    c_phi = math.cos(phi / 2)
    s_phi = math.sin(phi / 2)
    c_theta = math.cos(theta / 2)
    s_theta = math.sin(theta / 2)
    c_psi = math.cos(psi / 2)
    s_psi = math.sin(psi / 2)

    qw = c_phi * c_theta * c_psi + s_phi * s_theta * s_psi
    qx = s_phi * c_theta * c_psi - c_phi * s_theta * s_psi
    qy = c_phi * s_theta * c_psi + s_phi * c_theta * s_psi
    qz = c_phi * c_theta * s_psi - s_phi * s_theta * c_psi

    return np.array([qw, qx, qy, qz])

其中，`phi`、`theta`和`psi`分别代表绕x轴、y轴和z轴的旋转角度（单位为弧度）。函数返回一个四元数，表示等效旋转矢量。可以使用NumPy库中的数组来表示四元数。

如果需要将四元数转换为旋转矩阵或欧拉角，可以使用以下代码：

def quaternion_to_matrix(q):
    qw, qx, qy, qz = q
    return np.array([
        [1 - 2*qy**2 - 2*qz**2, 2*qx*qy - 2*qz*qw, 2*qx*qz + 2*qy*qw],
        [2*qx*qy + 2*qz*qw, 1 - 2*qx**2 - 2*qz**2, 2*qy*qz - 2*qx*qw],
        [2*qx*qz - 2*qy*qw, 2*qy*qz + 2*qx*qw, 1 - 2*qx**2 - 2*qy**2]
    ])

def quaternion_to_euler(q):
    qw, qx, qy, qz = q
    phi = math.atan2(2*(qw*qx + qy*qz), 1 - 2*(qx**2 + qy**2))
    theta = math.asin(2*(qw*qy - qz*qx))
    psi = math.atan2(2*(qw*qz + qx*qy), 1 - 2*(qy**2 + qz**2))
    return np.array([phi, theta, psi])

这些函数分别为将四元数转换为旋转矩阵和欧拉角。