python将四元数转换为等效旋转矢量 代码 用公式进行编写

将四元数转换为等效旋转矢量，可以使用以下公式：

q = qw + qx*i + qy*j + qz*k

r = acos(qw) * 2
x = qx / sqrt(1 - qw*qw)
y = qy / sqrt(1 - qw*qw)
z = qz / sqrt(1 - qw*qw)

rotate_vector = [r*x, r*y, r*z]

其中，q为四元数，qw、qx、qy、qz为四元数的实部和虚部，i、j、k为虚部对应的基向量，r为旋转角度，x、y、z为旋转轴的方向向量。

以下是代码实现：

import math

def quaternion_to_rotate_vector(q):
    qw, qx, qy, qz = q
    r = math.acos(qw) * 2
    x = qx / math.sqrt(1 - qw*qw)
    y = qy / math.sqrt(1 - qw*qw)
    z = qz / math.sqrt(1 - qw*qw)
    rotate_vector = [r*x, r*y, r*z]
    return rotate_vector

其中，q为输入的四元数，rotate_vector为等效旋转矢量。