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

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

drivenzyw

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

阅读量604

点赞数 1

文章标签：矩阵线性代数 python 机器人

本文链接：https://blog.csdn.net/drivenzyw/article/details/130723235

版权

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

摘要由CSDN通过智能技术生成

代码

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  # 计算旋转向量的单位向量作为旋转轴
matrix3 = axangle2mat(axis, angle)  # 使用axangle2mat函数转换为旋转矩阵

2.spicy实现

spicy的xyz参数顺序和上面的一样有问题，这里就不继续写了。