四元数变换为旋转矩阵 解决万向节死锁

目录

不错的讲解：

scipy把四元数转旋转矩阵

autolab_core库实现

c++实现：

---

不错的讲解：

欧拉角(横滚角、俯仰角、偏航角)、旋转矩阵、四元数的转换与解决万向节死锁_站心坐标系下的欧拉角-CSDN博客

scipy把四元数转旋转矩阵

conda install scipy

原文：python中将四元数转换为旋转矩阵_python 四元数转旋转矩阵-CSDN博客

from scipy.spatial.transform import Rotation as R
import numpy as np
print('test')
# use [:, np.newaxis] to transform from row vector to col vector
position = np.array([0.6453529828252734, -0.26022684372145516, 1.179122068068349])[:, np.newaxis]
share_vector = np.array([0,0,0,1], dtype=float)[np.newaxis, :]
print('share_vector:\n', share_vector)
print('position:\n',position)
r = R.from_quat([-0.716556549511624,-0.6971278819736084, -0.010016582945017661,  0.02142651612120239])
r.as_matrix()
print('rotation:\n',r.as_matrix())
rotation_matrix = r.as_matrix()
print(rotation_matrix)
 
#combine three matrix or vector together
m34 = np.concatenate((rotation_matrix, position), axis = 1)
print(m34)
m44 = np.concatenate((m34, share_vector), axis=0)
# m44 = np.hstack((m34, share_vector))
 
print(m44)
 
rot_vec = r.as_rotvec()
print('rot_vec:\n', rot_vec)
rot_euler = r.as_euler('zyx', degrees = False)
print('rot_euler:\n',rot_euler)
 
r = R.from_matrix(rotation_matrix)
print('as_quat():\n',r.as_quat())
print('as_rotvec():\n', r.as_rotvec())
print('as_euler():\n', r.as_euler('zyx', degrees=True))

 

autolab_core库实现

import numpy as np
from autolab_core import RigidTransform

# 写上用四元数表示的orientation和xyz表示的position
orientation = {'y': -0.6971278819736084, 'x': -0.716556549511624, 'z': -0.010016582945017661, 'w': 0.02142651612120239}
position = {'y': -0.26022684372145516, 'x': 0.6453529828252734, 'z': 1.179122068068349}

rotation_quaternion = np.asarray([orientation['w'], orientation['x'], orientation['y'], orientation['z']])
translation = np.asarray([position['x'], position['y'], position['z']])
# 这里用的是UC Berkeley的autolab_core，比较方便吧，当然可以自己写一个fuction来计算，计算公式在https://www.cnblogs.com/flyinggod/p/8144100.html
T_qua2rota = RigidTransform(rotation_quaternion, translation)

print(T_qua2rota)
 
# 以下是打印的结果
Tra: [ 0.64535298 -0.26022684  1.17912207]
     Rot: [[ 0.02782477  0.99949234 -0.01551915]
     [ 0.99863386 -0.02710724  0.0446723 ]
     [ 0.04422894 -0.01674094 -0.99888114]]
     Qtn: [-0.02142652  0.71655655  0.69712788  0.01001658]
     from unassigned to world

c++实现：

有tf版本，Eigen库

欧拉角，四元数，旋转矩阵相互转化（c++, python) - 知乎