python四元数转rx，ry，rz

在Python中，四元数（Quaternion）是一种复数扩展，用于表示三维空间中的旋转。要将四元数转换为旋转矩阵（Rx, Ry, Rz），通常涉及到两个步骤：

1. **四元数到欧拉角**：
四元数包含一个实部（w分量）和三个虚部（x, y, z分量），可以映射到三个欧拉角（通常是XYZ轴的顺序）。转换公式可能会因选择的欧拉角顺序而变化，常见的有三种：

- **ZYX顺序（Tait-Bryan Convention）**:

     pitch = math.asin(2 * (q.w * q.z + q.x * q.y))
     yaw = math.atan2(2 * (q.w * q.y - q.x * q.z), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)
     roll = math.atan2(2 * (q.w * q.x + q.y * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z)

- **XYZ顺序** 或 **ZYX Euler angles（绕Z轴旋转向量法）**:

     pitch = math.asin(2 * (q.y * q.z))
     yaw = math.atan2(2 * (q.x * q.z - q.w * q.y), q.w * q.w + q.x * q.x - q.y * q.y)
     roll = math.atan2(2 * (q.x * q.y + q.w * q.z), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)

- **ZXZ顺序** 或 **YXZ Euler angles**:

     pitch = math.atan2(2 * (q.w * q.z), q.w * q.w + q.z * q.z - q.x * q.x - q.y * q.y)
     yaw = math.atan2(2 * (q.x * q.y), q.y * q.y - q.x * x)
     roll = math.atan2(2 * (q.w * q.y + q.x * q.z), q.w * q.w - q.y * q.y - q.z * q.z + q.x * q.x)

2. **创建旋转矩阵**：
欧拉角分别对应旋转矩阵的各个旋转。例如对于ZYX顺序的yaw、pitch、roll：

   Rx = [[1, 0, 0], [0, math.cos(pitch), -math.sin(pitch)], [0, math.sin(pitch), math.cos(pitch)]]
   Ry = [[math.cos(yaw), 0, math.sin(yaw)], [0, 1, 0], [-math.sin(yaw), 0, math.cos(yaw)]]
   Rz = [[math.cos(roll), -math.sin(roll), 0], [math.sin(roll), math.cos(roll), 0], [0, 0, 1]]
   R = Rx @ Ry @ Rz

注意：不同的欧拉角顺序会得到不同的旋转效果，所以选择合适的顺序很重要。