ROS2中如何从欧拉角转换成四元素
ROS1中使用from tf.transformations import quaternion_from_euler导入quaternion_from_euler()即可调用。而ROS2中默认没有安装，需要单独安装一下ros-galactic-tf-transformations(我使用的ROS2是galactic，根据版本名不同做相应修改即可)：
sudo apt-get update
sudo apt-get install ros-galactic-tf-transformations然后导入包，不过包名改成了tf_transformations
import tf_transformations
# 将欧拉角转换为四元数（roll, pitch, yaw）
q = tf_transformations.quaternion_from_euler(roll, pitch, yaw)
# 将四元素转换成欧拉角
euler = tf_transformations.euler_from_quaternion([x, y, z, w])不安装tf_transformations的话，还可以安装其它工具包来替代，例如python的第三方quaternions包:
pip install quaternions -i http://mirrors.aliyun.com/pypi/simple/ --trusted-host mirrors.aliyun.comfrom quaternions import Quaternion as Quaternion
euler = [0.98012,0.5232,0.2105] # ROLL PITCH HEADING
q = Quaternion.from_euler(euler, axes = ['y', 'y', 'x']) # a= (w, x, y, z)
e = Quaternion.get_euler(q) # e = (Heading, Pitch, Roll)需要注意的是ROS在gemetry_msgs包里已定义有Quaternion类，另外ROS1里tf2里也定义有Quaternion，同时使用时注意用别名或包名区分。
另外，也可以安装trimesh工具包后使用里面的quaternion_from_euler
pip3.8 install trimesh -i http://mirrors.aliyun.com/pypi/simple/ --trusted-host mirrors.aliyun.comfrom trimesh.transformations import quaternion_from_euler
from geometry_msgs.msg import Quaternion as GeoQuaternion
def createQuaternionFromYaw(yaw):
q = quaternion_from_euler(0, 0, yaw)
return GeoQuaternion(x=q[0],y=q[1],z=q[2],w=q[3])看一下trimesh里/usr/local/lib/python3.8/dist-packages/trimesh/transformations.py里的quaternion_from_euler()的源码和/opt/ros/galactic/lib/python3.8/site-packages/tf_transformations/__init__.py里的quaternion_from_euler()代码，发现功能实现基本一致：
# axis sequences for Euler angles
_NEXT_AXIS = [1, 2, 0, 1]
# map axes strings to/from tuples of inner axis, parity, repetition, frame
_AXES2TUPLE = {
'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
...
def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
"""Return quaternion from Euler angles and axis sequence.
ai, aj, ak : Euler's roll, pitch and yaw angles
axes : One of 24 axis sequences as string or encoded tuple
>>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
>>> np.allclose(q, [0.435953, 0.310622, -0.718287, 0.444435])
True
"""
try:
firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
except (AttributeError, KeyError):
_TUPLE2AXES[axes] # validation
firstaxis, parity, repetition, frame = axes
i = firstaxis + 1
j = _NEXT_AXIS[i + parity - 1] + 1
k = _NEXT_AXIS[i - parity] + 1
if frame:
ai, ak = ak, ai
if parity:
aj = -aj
ai /= 2.0
aj /= 2.0
ak /= 2.0
ci = math.cos(ai)
si = math.sin(ai)
cj = math.cos(aj)
sj = math.sin(aj)
ck = math.cos(ak)
sk = math.sin(ak)
cc = ci * ck
cs = ci * sk
sc = si * ck
ss = si * sk
q = np.empty((4, ))
if repetition:
q[0] = cj * (cc - ss)
q[i] = cj * (cs + sc)
q[j] = sj * (cc + ss)
q[k] = sj * (cs - sc)
else:
q[0] = cj * cc + sj * ss
q[i] = cj * sc - sj * cs
q[j] = cj * ss + sj * cc
q[k] = cj * cs - sj * sc
if parity:
q[j] *= -1.0
return q
def euler_from_quaternion(quaternion, axes='sxyz'):
"""Return Euler angles from quaternion for specified axis sequence.
>>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0])
>>> np.allclose(angles, [0.123, 0, 0])
True
"""
return euler_from_matrix(quaternion_matrix(quaternion), axes)def quaternion_from_euler(ai, aj, ak, axes='sxyz'):
"""
Return quaternion from Euler angles and axis sequence.
ai, aj, ak : Euler's roll, pitch and yaw angles
axes : One of 24 axis sequences as string or encoded tuple
>>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
>>> numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953])
True
"""
return _reorder_output_quaternion(
transforms3d.euler.euler2quat(ai, aj, ak, axes=axes)
)
...
def euler_from_quaternion(quaternion, axes='sxyz'):
"""
Return Euler angles from quaternion for specified axis sequence.
>>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947])
>>> numpy.allclose(angles, [0.123, 0, 0])
True
"""
return euler_from_matrix(quaternion_matrix(quaternion), axes)另外，ros2默认安装的spawn_entity代码/opt/ros/galactic/lib/python3.8/site-packages/scripts/spawn_entity.py里的提供了一个简单实现：
def quaternion_from_euler(roll, pitch, yaw):
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
q = [0] * 4
q[0] = cy * cp * cr + sy * sp * sr
q[1] = cy * cp * sr - sy * sp * cr
q[2] = sy * cp * sr + cy * sp * cr
q[3] = sy * cp * cr - cy * sp * sr
return q可以看到这个实现没有提供坐标轴顺序参数axes，按默认的sxyz来的。
那么axes参数值都有哪些呢？意思是什么，查了一下资料这里汇总一下：
根据三次基本转动选取的坐标轴的不同，欧拉角共有12种组合，再考虑到可选取原始坐标系的坐标轴，也可选取“新”坐标系的坐标轴，则共有24种欧拉角表示。一般规定原始坐标系为静坐标系(static)，每个基本转动后形成的新坐标系为动坐标系(rotating)。
24 种欧拉角表示列举如下：
- 静轴(即转轴选静坐标系的坐标轴):
sXYZ,sXZY,sXYX,sXYZ,sXZY,sXYX,sXYZ,sXZY,sXYX,
sXZX,sYXZ,sYZX,sXZX,sYXZ,sYZX,sXZX,sYXZ,sYZX,
sYXY,sYZY,sZXY,sYXY,sYZY,sZXY,sYXY,sYZY,sZXY,
sZYX,sZXZ,sZYZsZYX,sZXZ,sZYZsZYX,sZXZ,sZYZ
- 动轴(即转轴选动坐标系的坐标轴):
rZYX,rYZX,rXYX,rZYX,rYZX,rXYX,rZYX,rYZX,rXYX,
rXZX,rZXY,rXZY,rXZX,rZXY,rXZY,rXZX,rZXY,rXZY,
rYXY,rYZY,rYXZ,rYXY,rYZY,rYXZ,rYXY,rYZY,rYXZ,
rXYZ,rZXZ,rZYZrXYZ,rZXZ,rZYZrXYZ,rZXZ,rZYZ
静轴欧拉角和动轴欧拉角有如下规律:
绕静轴 XYZXYZXYZ 分别 转 α,β,γα,β,γα,β,γ 角度的转动与绕动轴 ZYXZYXZYX分别转 γ,β,αγ,β,αγ,β,α 角度的转动等价，其他形式的欧拉角亦有此类似规律。
axes的值和firstaxis, parity, repetition, frame的含义解释：
