四元数、旋转向量、欧拉角、旋转矩阵的相互转换

最新推荐文章于 2024-07-30 10:14:42 发布

原创

最新推荐文章于 2024-07-30 10:14:42 发布 · 1k 阅读

5 ·

CC 4.0 BY-SA版权

文章标签：

#矩阵 #线性代数 #python #机器人

该代码示例展示了如何使用Python的transform3d库在四元数、欧拉角和旋转矩阵之间进行转换。首先，从四元数转换为欧拉角，然后反向转换回四元数和旋转矩阵。接着，将旋转矩阵转换为旋转向量，再由旋转向量恢复旋转矩阵。注意，欧拉角的顺序为ZYX。

代码

1.transform3d实现

import numpy as np
from transforms3d.euler import quat2euler
from transforms3d.euler import mat2euler
from transforms3d.euler import euler2mat
from transforms3d.euler import euler2quat
from transforms3d.euler import axangle2mat
from transforms3d.euler import quat2axangle
import transforms3d.quaternions as tf#这个包需要单独下载

euler=quat2euler([ 0.828, 0.100,0.542,0.101])#四元数wxyz顺序，四元数求欧拉角
#欧拉角是按ZYX方向进行旋转的,我验证了很多次
#在线网站也好定义也好，这个地方他就就是ZYX，但是它的默认参数就是sxyz我也不知道为什么
quat=euler2quat(euler[0],euler[1],euler[2])#欧拉角转四元数

matrix=tf.quat2mat([ 0.828, 0.100,0.542,0.101])#四元数求旋转矩阵，也是wxyz顺序
quat2=tf.mat2quat(matrix)#旋转矩阵转四元数
# print('quat2',quat2)

euler2 = mat2euler(matrix)#旋转矩阵转欧拉角。
matrix2=euler2mat(euler2[0],euler2[1],euler2[2])#欧拉角转旋转矩阵

#旋转矩阵转旋转向量
quat2=tf.mat2quat(matrix)
axis,angle=quat2axangle(quat2)
vector=axis*angle
#旋转向量转旋转矩阵
angle = np.linalg.norm(vector)  # 计算旋转向量的模作为旋转角度
axis = vector / angle  # 计算旋转向量的单位向量作为旋转轴

最低0.47元/天解锁文章