ROS2-运动学
1. 位姿
要在三维空间中描述一个物体，我们可以用位姿来表示。
位姿即位置和姿态。位置通常用一个三维位置矢量（xyz坐标）表示，姿态即物体的朝向，可以用欧拉角、四元数或者旋转矩阵来表示。
位置容易理解，例如某点的位置矢量为[𝑥, 𝑦, 𝑧]，为方便计算，取其转置矩阵[𝑥, 𝑦, 𝑧]ᵀ。
2. 姿态
旋转矩阵
若我们已经知道坐标系A中的一个点P的位置，我们如何描述P点在{A}坐标系下的姿态？
我们可以以P点为原点，建立一个新的坐标系{P}，这样P点的姿态即可描述为{P}坐标系相对于{A}坐标系的姿态关系。
坐标系{P}的每一个坐标轴都和{A}坐标系的三个轴之间存在一定的角度，通过这三个角度我们就可以确定{P}的每个轴分别相对于{A}的姿态关系，由此确定{P}坐标系相对于{A}坐标系的姿态。
将上述坐标系{P}的三个轴相对于参考坐标系{A}三个轴的共九个角度的余弦值，组成一个3*3的矩阵，该矩阵就是旋转矩阵, 可用坐标轴方向的单位向量的点积表示：
欧拉角
旋转矩阵的描述是冗余的，因为它包含了9个元素，但实际上只需要3个角度就可以描述一个物体的姿态关系：绕x轴旋转角度𝜑，绕y轴旋转角度𝜃，绕z轴旋转角度𝜓。
假设{A}为参考坐标系，{B}绕{A}的x轴旋转𝜑，绕{A}的y轴旋转𝜃，绕{A}的z轴旋转𝜓，则{B}坐标系相对于{A}坐标系的旋转矩阵为：
R = Rz(𝜓) * Ry(𝜃) * Rx(𝜑)（即三个旋转矩阵的乘积）
三个角度分别称为俯仰角、滚转角和偏航角，不同的旋转顺序会得到不同的旋转矩阵。
轴角
欧拉角旋转时参考的轴都是坐标系的主轴，而轴角旋转时参考的轴可以是任意轴。可以使用一个单位矢量来代表旋转轴，一个角度来代表右手定则旋转的角度，轴角旋转的旋转矩阵可以通过罗德里格斯公式计算得到。
四元数
四元数使用四维空间中的一个点来表示三维空间中的旋转，四元数由一个实部和三个虚部组成，通常表示为q = [w, x, y, z]，其中w是实部，x、y、z是虚部。四元数可以避免欧拉角不同旋转顺序旋转矩阵不同的问题
四元数学习：https://www.bilibili.com/video/BV14t421h7M4
3. 坐标变换
平移坐标变换
{A}为参考坐标系，{B}的位置矢量为[𝑥, 𝑦, 𝑧]ᵀ
{B}为参考坐标系，{C}的位置矢量为[𝑥', 𝑦', 𝑧']ᵀ
则{A}坐标系下{C}的位置矢量为：
[𝑥, 𝑦, 𝑧]ᵀ + [𝑥', 𝑦', 𝑧']ᵀ = [x + x', y + y', z + z']ᵀ
旋转坐标变换
{A}为参考坐标系，{B}的旋转矩阵为R
{B}为参考坐标系，{C}的旋转矩阵为R'
则{A}坐标系下{C}的旋转矩阵为：
R * R'
平移旋转复合变换
{A}为参考坐标系，{B}的位置矢量为[0, 0, 0]ᵀ（即未移动）旋转矩阵为R
{B}为参考坐标系，{C}的位置矢量为[𝑥', 𝑦', 𝑧']ᵀ
（坐标旋转方程）如果已知{B}坐标系下{C}的位置矢量为[𝑥', 𝑦', 𝑧']ᵀ，{A}坐标系下{B}的旋转矩阵为R，那么{A}坐标系下{C}的位置矢量为：R·[𝑥', 𝑦', 𝑧']ᵀ（即将[𝑥', 𝑦', 𝑧']ᵀ在{B}各轴上的分量变换到{A}各轴上）
若{A}坐标系下{B}的位置矢量为[𝑥, 𝑦, 𝑧]ᵀ，那么{A}坐标系下{C}的位置矢量为：
R * [𝑥', 𝑦', 𝑧']ᵀ + [𝑥, 𝑦, 𝑧]ᵀ（即先旋转后移动）
齐次坐标变换
为了方便计算，我们可以将平移和旋转复合变换表示为一个4*4的矩阵，称为齐次变换矩阵。
假设{A}为参考坐标系，{B}的位置矢量P为[𝑥, 𝑦, 𝑧]ᵀ，旋转矩阵为R，则{A}坐标系下{B}的齐次变换矩阵T为：
| R | [𝑥, 𝑦, 𝑧]ᵀ |
|---|---|
| 0 | 1 |
展开为：
| R11 | R12 | R13 | 𝑥 |
|---|---|---|---|
| R21 | R22 | R23 | 𝑦 |
| R31 | R32 | R33 | 𝑧 |
| 0 | 0 | 0 | 1 |
假设{B}为参考坐标系，{C}的位置矢量P'为[𝑥', 𝑦', 𝑧']ᵀ，则{C}在{A}坐标系下的位置矢量为：
T * [𝑥', 𝑦', 𝑧']ᵀ = R * [𝑥', 𝑦', 𝑧']ᵀ + [𝑥, 𝑦, 𝑧]ᵀ
与前面平移旋转复合变换的结果一致。
其次矩阵相乘可转换不同坐标系下的位姿关系，例如{A}坐标系下{B}的齐次变换矩阵为T1，{B}坐标系下{C}的齐次变换矩阵为T2，则{A}坐标系下{C}的齐次变换矩阵为：
T1 * T2
其次矩阵的逆可交换坐标系下的位姿关系，例如{A}坐标系下{B}的齐次变换矩阵为T1，则{B}坐标系下{A}的齐次变换矩阵为：
T1的逆矩阵
4. 练习变换
我们使用Python库transforms3d和numpy来进行坐标变换的练习。创建一个Python虚拟环境并安装这两个库
眼在手外
场景描述：我们可以通过视觉算法获取到工件坐标系P和相机坐标系C之间的关系，我们想要控制机械臂的末端坐标系E运动到工件坐标系P进行夹取，那么我们就要知道工件坐标系P在机器人基坐标系B下的位姿
已知条件：
-
工具坐标系{P}的位置矢量在相机坐标系{C}x,y,z各轴投影为2,1,2，并且工具坐标系{P}和相机坐标系{C}姿态相同。 -
相机坐标系{C}在基坐标系{B}的位置矢量在各轴的投影为0,0,3，相机坐标系{C}绕着基坐标系{B}的x轴转了180度
求：工具坐标系{P}在基坐标系{B}下的位置矢量和旋转矩阵
解：
可将B到C的齐次变换矩阵和C到P的齐次变换矩阵相乘，得到B到P的齐次变换矩阵，从中提取出位置矢量和旋转矩阵。
import numpy as np
import transforms3d as tfs
import math
# T_BC
T_BC = tfs.affines.compose([0,0,3],tfs.euler.euler2mat(math.pi,0,0),[1,1,1])
# tfs.affines.compose(translation, rotation, scale) 按“先缩放、再旋转、后平移”的顺序组合成 4×4 变换矩阵
# 平移：[0, 0, 3] 表示 C 的原点在 B 中的位置为 (0, 0, 3)（即沿 B 的 Z 轴正方向平移 3 个单位）
# 旋转：tfs.euler.euler2mat(math.pi, 0, 0) 将欧拉角 (π, 0, 0) 转换为旋转矩阵，表示 C 绕 B 的 X 轴旋转 180 度
# 缩放：[1, 1, 1] 表示没有缩放
# T_CP
T_CP = T = tfs.affines.compose([2,1,2],np.identity(3),[1,1,1])
# np.identity(3) 表示 3x3 的单位矩阵，表示 P 和 C 的姿态相同
# T_BP
T_BP = np.dot(T_BC, T_CP)
# np.dot(A, B) 表示矩阵 A 和 B 的点积，即矩阵乘法
# 提取位置矢量和平移矩阵
print(tfs.euler.mat2euler(T_BP[0:3,0:3]))
print(T_BP[:3,3:4])
# T_BP[0:3,0:3] 提取旋转矩阵部分，T_BP[:3,3:4] 提取位置矢量部分
求得结果：
眼在手上
如图机器人基座坐标系为B、末端坐标系为E、相机坐标系为C、物品坐标系为O、其中相机固定在机械臂的末端。
已知条件：
1. 末端坐标系{E}在基坐标系{B}的位置矢量在各轴的投影为0.5，0.6，0.8，四元数为[1,0,0,0]
2. 末端坐标系{E}在相机坐标系{C}的位置矢量在各轴的投影为0，0，0.05，四元数为[0.707,0.706,0,0]
3. 物品坐标系{O}在相机坐标系{C}的位置矢量在各轴的投影为0，0.02，0.85，四元数为[0.877,0.479,0,0]
求：物品坐标系{O}在基坐标系{B}下的位置矢量和旋转矩阵
解：
同样将B到E、E到C和C到O的齐次变换矩阵相乘，得到B到O的齐次变换矩阵，从中提取出位置矢量和旋转矩阵。
其中E到C的齐次变换矩阵需要求C到E的逆矩阵
import numpy as np
import transforms3d as tfs
import math
T_BE = tfs.affines.compose([0.5,0.6,0.8],tfs.quaternions.quat2mat([1,0,0,0]),[1,1,1])
T_CE = tfs.affines.compose([0.00,0.05,0.05],tfs.quaternions.quat2mat([0.707,0.706,0,0]),[1,1,1])
T_CO = tfs.affines.compose([0.00,0.02,0.85],tfs.quaternions.quat2mat([0.877,0.479,0,0]),[1,1,1])
# tfs.quaternions.quat2mat([qw, qx, qy, qz]) 将四元数 q 转换为旋转矩阵
T_EC = np.linalg.inv(T_CE)
# np.linalg.inv(A) 表示矩阵 A 的逆矩阵
T_BO = np.dot(T_BE, np.dot(T_EC, T_CO))
求得结果：
5. TF2
ROS2提供了强大易用的坐标变换工具TF2，即Transform
拿上面的手眼系统来说，我们要想获取到相机的基坐标系{B}和工具{P}之间的关系，只需要将机械臂和相机、相机和工具之间的关系告诉TF即可。
可以使用tf的坐标广播工具进行广播坐标关系：
ros2 run tf2_ros static_transform_publisher
出现以下提示：
usage: static_transform_publisher [--x X] [--y Y] [--z Z] [--qx QX] [--qy QY] [--qz QZ] [--qw QW] [--roll ROLL] [--pitch PITCH] [--yaw YAW] --frame-id FRAME_ID --child-frame-id CHILD_FRAME_ID
其中：
- --x, --y, --z：表示位置的x、y、z分量
- --qx, --qy, --qz, --qw：表示四元数形式的姿态
- --roll, --pitch, --yaw：表示欧拉角形式的姿态（yaw偏航角-rz、pitch俯仰角-ry、roll滚转角-rx，弧度制）
- frame-id：表示父坐标系的名称
- child-frame-id：表示子坐标系的名称
针对上述眼在手外系统，打开三个终端：
终端1监听B到P的坐标关系：
ros2 run tf2_ros tf2_echo B P
终端2发布B到C的坐标关系：
ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 3 --roll 3.14159 --pitch 0 --yaw 0 --frame-id B --child-frame-id C
终端3发布C到P的坐标关系：
ros2 run tf2_ros static_transform_publisher --x 2 --y 1 --z 2 --roll 0 --pitch 0 --yaw 0 --frame-id C --child-frame-id P
最终终端1输出B到P的坐标关系：
常用工具
- rqt_tf_tree：可视化TF树，显示坐标系之间的关系
#安装rqt插件
sudo apt install ros-kilted-rqt-tf-tree
#运行rqt
rqt --force-discover # 强制重新加载插件
在菜单栏选择Plugins -> Visualization -> TF Tree，即可看到TF树的可视化界面
- tf2_monitor：监视TF变换，查看发布者、频率等信息
ros2 run tf2_ros tf2_monitor
6. RVIZ2-TF
保持上一节的TF发布不变，打开RVIZ2：
rviz2
- 在左侧的Displays面板中，Global Options下的Fixed Frame设置为B，表示以B坐标系为参考坐标系进行可视化
- 点击左下角的Add按钮，添加TF插件，rviz2会自动显示TF树中的坐标系关系
- 在左侧的Displays面板中，TF下，勾选Show Names显示坐标系名称，Marker Scale设置为5，放大坐标轴的显示
在RVIZ2中可以看到B、C、P三个坐标系的关系：
7. TF2编程
坐标变换广播
广播出去的一组数据可以称做一个TF帧，TF帧包含了父坐标系、子坐标系、位置和姿态等信息。
我们根据两个坐标系之间的关系是否会随着时间变化分成以下两种情况：
- 坐标系之间的关系不随时间推移而改变，称为静态坐标变换，需要使用静态广播发布器（StaticTransformBroadcaster）发布。比如：机器人的两个轮子之间关系。
- 坐标系之间的关系随时间的推移而改变，称为（动态）坐标变换，使用广播发布器（TransformBroadcaster）发布坐标关系。比如机器人在世界坐标系中的位置。
仍以手眼系统坐标变换为例。
# 发布B到C的静态坐标关系
import rclpy
from rclpy.node import Node
# 导入TF帧消息接口
# 查看接口： ros2 interface show geometry_msgs/msg/TransformStamped
from geometry_msgs.msg import TransformStamped
# 导入静态坐标变换广播器
from tf2_ros import StaticTransformBroadcaster
# 初始化节点
rclpy.init()
node = Node("transform_node")
# 构造静态变换广播器
tf_pub = StaticTransformBroadcaster(node)
# 构造TF帧
t = TransformStamped()
t.header.stamp = node.get_clock().now().to_msg() # 时间戳
t.header.frame_id = 'B' # 父坐标系/基坐标系
t.child_frame_id = 'C' # 子坐标系
# 位置关系-坐标变换
t.transform.translation.x = 0.0
t.transform.translation.y = 0.0
t.transform.translation.z = 3.0
# 旋转关系需要使用四元数，欧拉角四元数在线转换：https://quaternions.online/
t.transform.rotation.x = 1.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 0.0
t.transform.rotation.w = 0.0
# 发布坐标关系
def send_transform():
t.header.stamp = node.get_clock().now().to_msg()
tf_pub.sendTransform(t)
node.create_timer(0.1, send_transform)
rclpy.spin(node)
运行程序后在另一个终端监听B到C的坐标关系：
ros2 run tf2_ros tf2_echo B C
这里输出的At time 0.0 代表任意时刻
相对于静态广播发布器，广播发布器用法相同，但坐标关系与时间有关系。
# 发布C到P的动态坐标关系
import rclpy
from rclpy.node import Node
# 导入TF帧消息接口
# 查看接口： ros2 interface show geometry_msgs/msg/TransformStamped
from geometry_msgs.msg import TransformStamped
# 导入坐标变换广播器
from tf2_ros import TransformBroadcaster
# 初始化节点
rclpy.init()
node = Node("transform_node2")
# 构造广播器
tf_pub = TransformBroadcaster(node)
# 构造TF帧
t = TransformStamped()
t.header.stamp = node.get_clock().now().to_msg() # 时间戳
t.header.frame_id = 'C' # 父坐标系/基坐标系
t.child_frame_id = 'P' # 子坐标系
# 位置关系-坐标变换
t.transform.translation.x = 2.0
t.transform.translation.y = 1.0
t.transform.translation.z = 2.0
# 旋转关系需要使用四元数，欧拉角四元数在线转换：https://quaternions.online/
t.transform.rotation.x = 1.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 0.0
t.transform.rotation.w = 0.0
# 发布坐标关系
def send_transform():
t.header.stamp = node.get_clock().now().to_msg()
tf_pub.sendTransform(t)
node.create_timer(0.1, send_transform)
rclpy.spin(node)
运行程序后在另一个终端监听C到P的坐标关系：
ros2 run tf2_ros tf2_echo C P
这里的At time 17...... 表示Unix时间戳
坐标变换监听
监听器（TransformListener）用于监听TF树中坐标系之间的关系，可以获取任意两个坐标系之间的位姿关系。
import time
import rclpy
from rclpy.node import Node
# 导入坐标变换监听器
from tf2_ros.transform_listener import TransformListener
from tf2_ros.buffer import Buffer # 缓冲区
# 初始化节点
rclpy.init()
node = Node("transform_node3")
# 构建缓冲区和监听器
tf_buffer = Buffer()
tf_listener = TransformListener(tf_buffer, node) # 监听的消息会暂存至缓冲区
def transform_callback():
try:
now = rclpy.time.Time()
trans = tf_buffer.lookup_transform('B','P', now)
print("trans:", trans)
except Exception as e:
print("Couldn't transform:", e)
node.create_timer(1, transform_callback)
rclpy.spin(node)
运行两个发布器和一个监听器，获得B和P之间的坐标关系：
