type
status
date
slug
summary
category
tags
password
Created time
Jul 30, 2024 06:37 PM
Last edited time
Aug 18, 2024 04:26 PM
UUID
icon
🗒️发表的笔记
URL CK
ErrorCheck
ErrorCheck
Description
{% note primary %}
📖 机器人的坐标变换一直以来是机器人学的一个难点，我们人类在进行一个简单的动作时，从思考到实施行动再到完成动作可能仅仅需要几秒钟，但是机器人来讲就需要大量的计算和坐标转换。
{% endnote %}
1. 认识TF
1.1. 简介
TF是一个ROS世界里的一个基本的也是很重要的概念，所谓TF(TransForm)，就是坐标转换．在现实生活中，我们做出各种行为模式都可以在很短的时间里完成，比如拿起身边的物品，但是在机器人的世界里，则远远没有那么简单．观察下图，我们来分析机器人拿起身边的物品需要做到什么，而TF又起到什么样的作用．
观察这个机器人，我们直观上不认为拿起物品会又什么难度，站在人类的立场上，我们也许会想到手向前伸，抓住，手收回．就完成了这整个一系列的动作．但是如今的机器人远远没有这么智能，它能得到的只是各种传感器发送回来的数据，然后它再处理各种数据进行操作，比如手臂弯曲45度，再向前移动２０cm等这样的各种十分精确的数据，尽管如此，机器人依然没法做到像人类一样自如的进行各种行为操作．那么在这个过程中，TF又扮演着什么样的角色呢？还拿该图来说，当机器人的＂眼睛＂获取一组数据，关于物体的坐标方位，但是相对于机器人手臂来说，这个坐标只是相对于机器人头部的传感器，并不直接适用于机器人手臂执行，那么物体相对于头部和手臂之间的坐标转换，就是TF.
坐标变换包括了位置和姿态两个方面的变换，ROS中的tf是一个可以让用户随时记录多个坐标系的软件包。tf保持缓存的树形结构中的坐标系之间的关系，并且允许用户在任何期望的时间点在任何两个坐标系之间转换点，矢量等．
1.2. ROS中的TF
tf的定义不是那么的死板，它可以被当做是一种标准规范，这套标准定义了坐标转换的数据格式和数据结构．tf本质是树状的数据结构，所以我们通常称之为"tf tree",tf也可以看成是一个topic:
/tf，话题中的message保存的就是tf tree的数据结构格式．维护了整个机器人的甚至是地图的坐标转换关系．tf还可以看成是一个package,它当中包含了很多的工具．比如可视化，查看关节间的tf,debug tf等等．tf含有一部分的接口，就是我们前面章节介绍的roscpp和rospy里关于tf的API．所以可以看成是话题转换的标准，工具，接口．
观察上图，我们可以看到ROS数据结构的一个抽象图，ROS中机器人模型包含大量的部件，这些部件统称之为link,每一个link上面对应着一个frame, 即一个坐标系．link和frame概念是绑定在一起的．像上图pr2模型中我们可以看到又很多的frame,错综复杂的铺置在机器人的各个link上，维护各个坐标系之间的关系，就要靠着tf tree来处理，维护着各个坐标系之间的联通．如下图：
上图是我们常用的robot_sim_demo运行起来的tf tree结构，每一个圆圈代表一个frame,对应着机器人上的一个link，任意的两个frame之间都必须是联通的，如果出现某一环节的断裂，就会引发error系统报错．所以完整的tf tree不能有任何断层的地方，这样我们才能查清楚任意两个frame之间的关系．仔细观察上图，我们发现每两个frame之间都有一个broadcaster,这就是为了使得两个frame之间能够正确连通，中间都会有一个Node来发布消息来broadcaster.如果缺少Node来发布消息维护连通，那么这两个frame之间的连接就会断掉．broadcaster就是一个publisher,如果两个frame之间发生了相对运动，broadcaster就会发布相关消息．
2. TF消息
2.1. TransformStamped.msg
上一节在介绍ROS中的TF时候我们已经初步的认识了TF和TF树，了解了在每个frame之间都会有broadcaster来发布消息维系坐标转换．那么这个消息到底是什么样子的呢？这个消息TransformStampde.msg,它就是处理两个frame之间一小段tf的数据格式．
2.2. 格式规范
TransformStamped.msg的格式规范如下：
观察标准的格式规范，首先header定义了序号，时间以及frame的名称．接着还写了child_frame，这两个frame之间要做那种变换就是由geometry_msgs/Transform来定义．Vector3三维向量表示平移，Quaternion四元数表示旋转．像下图TF树中的两个frame之间的消息，就是由这种格式来定义的．odom就是frame_id,baselink_footprint就是child_frame_id.我们知道，一个topic上面可能会有很多个node向上面发送消息。如图所示，不仅有我们看到的frame发送坐标变换个tf，还有别的frame也在同样的向它发送消息。最终，许多的TransformStamped.msg发向tf，形成了TF树。
2.3. TF树的数据类型
上面我们讲了，TF tree是由很多的frame之间TF拼接而成。那么TF tree是什么类型呢？如下：
- tf/tfMessage.msg
- tf2_msgs/TFMessage.msg
这里TF的数据类型有两个，主要的原因是版本的迭代。自ROS Hydro以来，tf第一代已被“弃用”，转而支持tf2。tf2相比tf更加简单高效。此外也添加了一些新的功能。
由于tf2是一个重大的变化，tf API一直保持现有的形式。由于tf2具有tf特性的超集和一部分依赖关系，所以tf实现已经被移除，并被引用到tf2下。这意味着所有用户都将与tf2兼容。官网建议新工作直接使用tf2，因为它有一个更清洁的界面，和更好的使用体验。
如何查看自己使用的TF是哪一个版本，使用命令
rostopic info /tf即可。2.4. 格式定义
tf/tfMessage.msg或tf2_msgs/TFMessage标准格式规范如下:
如上，一个TransformStamped数组就是一个TF tree。
3. tf in c++
3.1. 简介
前面内容我们介绍了TF的基本的概念和TF树消息的格式类型，我们知道，TF不仅仅是一个标准、话题，它还是一个接口。本节课我们就介绍c++中TF的一些函数和写法。
3.2. 数据类型
C++中给我们提供了很多TF的数据类型，如下表：
名称 | 数据类型 |
向量 | tf::Vector3 |
点 | tf::Point |
四元数 | tf::Quaternion |
3*3矩阵（旋转矩阵） | tf::Matrix3x3 |
位姿 | tf::pose |
变换 | tf::Transform |
带时间戳的以上类型 | tf::Stamped |
带时间戳的变换 | tf::StampedTransform |
易混注意：虽然此表的最后带时间戳的变换数据类型为tf::StampedTransform,和上节我们所讲的geometry_msgs/TransformStamped.msg看起来很相似，但是其实数据类型完全不一样，tf::StampedTransform只能用在C++里，只是C++的一个类，一种数据格式，并不是一个消息。而geometry_msgs/TransformStamped.msg是一个message,它依赖于ROS，与语言无关，也即是无论何种语言，C++、Python、Java等等，都可以发送该消息。
3.3. 数据转换
在TF里有可能会遇到各种各样数据的转换，例如常见的四元数、旋转矩阵、欧拉角这三种数据之间的转换。tf in roscpp给了我们解决该问题的函数。详细源码在我们教学课程的代码包中。 首先在tf中与数据转化的数据都类型都包含在
#include头文件中，我们将与数据转换相关API都存在tf_demo中的coordinate_transformation.cpp当中，其中列表如下：3.4. 第1部分定义空间点和空间向量
编号 | 函数名称 | 函数功能 |
1.1 | tfScalar::tfDot(const Vector3 &v1, const Vector3 &v2) | 计算两个向量的点积 |
1.2 | tfScalar length() | 计算向量的模 |
1.3 | Vector3 &normalize() | 求与已知向量同方向的单位向量 |
1.4 | tfScalar::tfAngle(const Vector3 &v1, const Vector3 &v2) | 计算两个向量的夹角 |
1.5 | tfScale::tfDistance(const Vector3 &v1, const Vector3 &v2) | 计算两个向量的距离 |
1.6 | tfScale::tfCross(const Vector3 &v1,const Vector3 &v2) | 计算两个向量的乘积 |
示例代码：
3.5. 第2部分定义四元数
编号 | 函数名称 | 函数功能 |
2.1 | setRPY(const tfScalar& yaw, const stScalar &pitch, const tfScalar &roll) | 由欧拉角计算四元数 |
2.2 | Vector3 getAxis() | 由四元数得到旋转轴 |
2.3 | setRotation(const Vector3 &axis, const tfScalar& angle) | 已知旋转轴和旋转角估计四元数 |
示例代码：
3.6. 第3部分定义旋转矩阵
编号 | 函数名称 | 函数功能 |
3.1 | setRotaion(const Quaternion &q) | 通过四元数得到旋转矩阵 |
3.2 | getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll ) | 由旋转矩阵求欧拉角 |
示例代码：
此外，在tf_demo的教学包中，我们还提供常见的欧拉角与四元数的互换，详见Euler2Quaternion.cpp与Quaternion2Euler.cpp Euler2Quaternion.cpp
Quaternion2Euler.cpp
3.7. TF类
3.8. tf::TransformBroadcaster类
这个类在前面讲TF树的时候提到过，这个broadcaster就是一个publisher,而sendTransform的作用是来封装publish的函数。在实际的使用中，我们需要在某个Node中构建tf::TransformBroadcaster类，然后调用sendTransform(),将transform发布到
/tf的一段transform上。/tf里的transform为我们重载了多种不同的函数类型。在我们的tf_demo教学包当中提供了相关的示例代码tf.broadcaster.cpp，具体如下：3.9. tf::TransformListener类
上一个类是向
/tf上发的类，那么这一个就是从/tf上接收的类。首先看lookuptransform()函数，第一个参数是目标坐标系，第二个参数为源坐标系，也即是得到从源坐标系到目标坐标系之间的转换关系，第三个参数为查询时刻，第四个参数为存储转换关系的位置。值得注意，第三个参数通常用ros::Time（0）,这个表示为最新的坐标转换关系，而ros::time::now则会因为收发延迟的原因，而不能正确获取当前最新的坐标转换关系。canTransform()是用来判断两个transform之间是否连通，waitForTransform()const是用来等待某两个transform之间的连通，在我们的tf_demo教学包当中提供了相关的示例代码tf_listerner.cpp，具体如下：4. tf in python
4.1. 简介
我们知道tf中不仅有C++的接口，也有Python的接口。相比C++，tf在Python中的具体实现相对简单好用。
4.2. 数据类型
TF的相关数据类型，向量、点、四元数、矩阵都可以表示成类似数组形式，就是它们都可以用Tuple，List，Numpy, Array来表示。
例如：
第一个平移数据使用Tuple表示的，同时也可以用List表示成t=[1.0,1.5,0],也能用numpy.array(1.0,1.5,0)来表示都是可以的。这些数据类型没有特殊对应，全部是通用的，所以这里也就没有了各种数据类型的转换的麻烦。
4.3. 库
4.4. tf.transformations
基本数学运算函数
函数 | 注释 |
euler_matrix(ai,aj,ak,axes='sxyz') | 欧拉角到矩阵 |
eulaer_form_matrix(matrix,axes='sxyz') | 矩阵到欧拉角 |
eular_from_quaternion(quaternion,axes='sxyz') | 四元数到欧拉角 |
quaternion_form_euler(ai,aj,ak,axes='sxyz') | 欧拉角到四元数 |
quaternion_matrix(quaternion) | 四元数到矩阵 |
quaternion_form_matrix(matrix) | 矩阵到四元数 |
...... | ...... |
使用该函数库时候，首先
import tf，tf.transformations给我们提供了一些基本的数学运算函数如上，使用起来非常方便。在tf_demo中教学包当中，我们列举了一些tf.transformations常见的API和示例代码，具详见下表。4.4.1. 第1部分，定义空间点和空间向量
编号 | 函数名称 | 函数功能 |
1.1 | tf.transformations.random_quaternion(rand=None) | 返回均匀随机单位四元数 |
1.2 | tf.transformations.random_rotation_matrix(rand=None) | 返回均匀随机单位旋转矩阵 |
1.3 | tf.transformations.random_vector(size) | 返回均匀随机单位向量 |
1.4 | tf.transformations.translation_matrix(v) | 通过向量来求旋转矩阵 |
1.5 | tf.transformations.translation_from_matrix(m) | 通过旋转矩阵来求向量 |
4.4.2. 第2部分，定义四元数
编号 | 函数名称 | 函数功能 |
2.1 | tf.transformations.quaternion_about_axis(angle axis) | 通过旋转轴和旋转角返回四元数 |
2.2 | tf.transformations.quaternion_conjugate(quaternion) | 返回四元数的共轭 |
2.3 | tf.transformations.quaternion_from_euler(ai,aj,ak, axes'ryxz') | 从欧拉角和旋转轴，求四元数 |
2.4 | tf.transformations.quaternion_from_matrix(matrix) | 从旋转矩阵中，返回四元数 |
2.5 | tf.transformations.quaternion_multiply(quaternion1,quaternion2) | 两个四元数相乘 |
4.4.3. 第3部分，定义四元数
编号 | 函数名称 | 函数功能 |
3.1 | tf.transformations.euler_matrix(ai,aj,ak,axes='xyz') | 由欧拉角和旋转轴返回旋转矩阵 |
3.2 | tf.transformations.euler_from_matrix(matrix) | 由旋转矩阵和特定的旋转轴返回欧拉角 |
3.3 | tf.transformations.euler_from_quaternion(quaternion) | 由四元数和特定的轴得到欧拉角 |
示例代码：
4.5. TF类
4.5.1. tf.TransformListener类
方法 | 作用 |
canTransform(self,target_frame,source_frame,time) | frame是否相通 |
waitForTransform(self,target_frame,source_frame,time,timeout) | 阻塞直到frame相通 |
lookup Transform(self,target_frame,source_frame,time) | 查看相对的tf，返回（trans，quat） |
tf.TransformListener类中主要包含以上三种方法，它的构造函数不需要填值。注意这里的time参数，依然是使用
rospy.Time(0)而不是rospy.Time.now().具体原因上节已经介绍，这里不再赘述。除了上述三种重要的方法，这个类中还有一些辅助用的方法如下：方法 | 作用 |
chain(target_frame,target_time,source_frame,source_time,fixed_frame) | frame的连接关系 |
frameExists(self,frame_id) | frame是否存在 |
getFrameStrings(self) | 返回所有tf的名称 |
fromTranslationRotation(translation,rotation) | 根据平移和旋转返回4X4矩阵 |
transformPoint(target_frame,point_msg) | 将PointStamped消息转换到新frame下 |
transformPose(target_frame,pose_msg) | 将PoseStamped消息转换到新frame下 |
transformQuaternion(target_frame,quat_msg) | 将QuaternionStamped...返回相同类型 |
... | ... |
在
tf_demo教学包当中的scripts/py_tf_listerner.py给出了示例程序，详见如下。py_tf_listerner.py
4.5.2.
4.5.3. tf.TransformBroadcaster类
类似的，我们介绍的是发布方，tf.TransformBroadcaster类。该类的构造函数也是不需要填值，成员函数有两个如下：
- sendTransform(translation,rotation,time,child,parent)#向/tf发布消息
- sendTransformMessage(transform)#向/tf发布消息
第一个sendTransform()把transform的平移和旋转填好，打上时间戳，然后表示出从父到子的frame流，然后发向
/tf的topic。第二种是发送transform已经封装好的Message给/tf，这两种不同的发送方式，功能是一致的。在tf_demo教学包当中的scripts/py_tf_broadcaster.py和scripts/py_tf_broadcaster02.py给出了示例程序，详见如下。py_tf_broadcaster.py
py_tf_broadcaster02.py
4.6. TF相关工具命令
- 根据当前的tf树创建一个pdf图：
这个工具首先订阅
/tf，订阅5秒钟，根据这段时间接受到的tf信息，绘制成一张tf tree，然后创建成一个pdf图。- 查看当前的tf树：
该命令同样是查询tf tree的，但是与第一个命令的区别是该命令是动态的查询当前的tf tree,当前的任何变化都能当即看到，例如何时断开何时连接，捕捉到这些然后通过rqt插件显示出来。
- 查看两个frame之间的变换关系：
5. tf in python
5.1. 简介
我们知道tf中不仅有C++的接口，也有Python的接口。相比C++，tf在Python中的具体实现相对简单好用。
5.2. 数据类型
TF的相关数据类型，向量、点、四元数、矩阵都可以表示成类似数组形式，就是它们都可以用Tuple，List，Numpy Array来表示。
例如：
第一个平移数据使用Tuple表示的，同时也可以用List表示成t=[1.0,1.5,0],也能用numpy.array(1.0,1.5,0)来表示都是可以的。这些数据类型没有特殊对应，全部是通用的，所以这里也就没有了各种数据类型的转换的麻烦。
5.3. TF库
5.3.1. tf.transformations
基本数学运算函数
函数 | 注释 |
euler_matrix(ai,aj,ak,axes='sxyz') | 欧拉角到矩阵 |
eulaer_form_matrix(matrix,axes='sxyz') | 矩阵到欧拉角 |
eular_from_quaternion(quaternion,axes='sxyz') | 四元数到欧拉角 |
quaternion_form_euler(ai,aj,ak,axes='sxyz') | 欧拉角到四元数 |
quaternion_matrix(quaternion) | 四元数到矩阵 |
quaternion_form_matrix(matrix) | 矩阵到四元数 |
...... | ...... |
使用该函数库时候，首先
import tf，tf.transformations给我们提供了一些基本的数学运算函数如上，使用起来非常方便。在tf_demo中教学包当中，我们列举了一些tf.transformations常见的API和示例代码，具详见下表。5.3.2. 第1部分，定义空间点和空间向量
编号 | 函数名称 | 函数功能 |
1.1 | tf.transformations.random_quaternion(rand=None) | 返回均匀随机单位四元数 |
1.2 | tf.transformations.random_rotation_matrix(rand=None) | 返回均匀随机单位旋转矩阵 |
1.3 | tf.transformations.random_vector(size) | 返回均匀随机单位向量 |
1.4 | tf.transformations.translation_matrix(v) | 通过向量来求旋转矩阵 |
1.5 | tf.transformations.translation_from_matrix(m) | 通过旋转矩阵来求向量 |
5.3.3. 第2部分，定义四元数
编号 | 函数名称 | 函数功能 |
2.1 | tf.transformations.quaternion_about_axis(angle axis) | 通过旋转轴和旋转角返回四元数 |
2.2 | tf.transformations.quaternion_conjugate(quaternion) | 返回四元数的共轭 |
2.3 | tf.transformations.quaternion_from_euler(ai,aj,ak, axes'ryxz') | 从欧拉角和旋转轴，求四元数 |
2.4 | tf.transformations.quaternion_from_matrix(matrix) | 从旋转矩阵中，返回四元数 |
2.5 | tf.transformations.quaternion_multiply(quaternion1,quaternion2) | 两个四元数相乘 |
5.3.4. 第3部分，定义四元数
编号 | 函数名称 | 函数功能 |
3.1 | tf.transformations.euler_matrix(ai,aj,ak,axes='xyz') | 由欧拉角和旋转轴返回旋转矩阵 |
3.2 | tf.transformations.euler_from_matrix(matrix) | 由旋转矩阵和特定的旋转轴返回欧拉角 |
3.3 | tf.transformations.euler_from_quaternion(quaternion) | 由四元数和特定的轴得到欧拉角 |
示例代码：
5.4. TF类
5.4.1. tf.TransformListener类
方法 | 作用 |
canTransform(self,target_frame,source_frame,time) | frame是否相通 |
waitForTransform(self,target_frame,source_frame,time,timeout) | 阻塞直到frame相通 |
lookup Transform(self,target_frame,source_frame,time) | 查看相对的tf，返回（trans，quat） |
tf.TransformListener类中主要包含以上三种方法，它的构造函数不需要填值。注意这里的time参数，依然是使用
rospy.Time(0)而不是rospy.Time.now().具体原因上节已经介绍，这里不再赘述。除了上述三种重要的方法，这个类中还有一些辅助用的方法如下：方法 | 作用 |
chain(target_frame,target_time,source_frame,source_time,fixed_frame) | frame的连接关系 |
frameExists(self,frame_id) | frame是否存在 |
getFrameStrings(self) | 返回所有tf的名称 |
fromTranslationRotation(translation,rotation) | 根据平移和旋转返回4X4矩阵 |
transformPoint(target_frame,point_msg) | 将PointStamped消息转换到新frame下 |
transformPose(target_frame,pose_msg) | 将PoseStamped消息转换到新frame下 |
transformQuaternion(target_frame,quat_msg) | 将QuaternionStamped...返回相同类型 |
... | ... |
在
tf_demo教学包当中的scripts/py_tf_listerner.py给出了示例程序，详见如下。py_tf_listerner.py
5.4.2.
5.4.3. tf.TransformBroadcaster类
类似的，我们介绍的是发布方，tf.TransformBroadcaster类。该类的构造函数也是不需要填值，成员函数有两个如下：
- sendTransform(translation,rotation,time,child,parent)#向/tf发布消息
- sendTransformMessage(transform)#向/tf发布消息
第一个sendTransform()把transform的平移和旋转填好，打上时间戳，然后表示出从父到子的frame流，然后发向
/tf的topic。第二种是发送transform已经封装好的Message给/tf，这两种不同的发送方式，功能是一致的。在tf_demo教学包当中的scripts/py_tf_broadcaster.py和scripts/py_tf_broadcaster02.py给出了示例程序，详见如下。py_tf_broadcaster.py
py_tf_broadcaster02.py
5.5. TF相关工具命令
- 根据当前的tf树创建一个pdf图：
这个工具首先订阅
/tf，订阅5秒钟，根据这段时间接受到的tf信息，绘制成一张tf tree，然后创建成一个pdf图。- 查看当前的tf树：
该命令同样是查询tf tree的，但是与第一个命令的区别是该命令是动态的查询当前的tf tree,当前的任何变化都能当即看到，例如何时断开何时连接，捕捉到这些然后通过rqt插件显示出来。
- 查看两个frame之间的变换关系：
