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度,再向前移动20cm等这样的各种十分精确的数据,尽管如此,机器人依然没法做到像人类一样自如的进行各种行为操作.那么在这个过程中,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之间的变换关系: