[feat]:完成原有第七章迁移

docs/_sidebar.md

@@ -17,6 +17,7 @@
     -  [Launch](humble/codebook/pages/launch.md) 
     -  [网络通讯](humble/codebook/pages/networking.md) 
     -  [功能包](humble/codebook/pages/packages.md) 
+  
 - （一）​ROS2入门篇
   - 第 1 章 ROS2介绍与安装
     - [章节导读](humble/chapt1/章节导读.md) 
@@ -77,7 +78,7 @@
       -  [2.通信质量Qos配置指南](humble/chapt3/advanced/2.通信质量Qos配置指南.md) 
       -  [3.DDS进阶之Fast-DDS环境搭建](humble/chapt3/advanced/3.DDS进阶之Fast-DDS环境搭建.md) 
       -  [4.使用DDS进行订阅发布](humble/chapt3/advanced/4.使用DDS进行订阅发布.md) 
-      -  [5.ROS2消息序列化思考与实践 ](humble/chapt3/advanced/5.ROS2消息序列化思考与实践 .md) 
+      -  [5.ROS2消息序列化思考与实践](humble/chapt3/advanced/5.ROS2消息序列化思考与实践 .md) 
       -  [6.执行器与回调组](humble/chapt3/advanced/6.执行器与回调组.md) 
   - 第 4 章 ROS2通信之参数与动作
     -  [章节导读](humble/chapt4/章节导读.md) 
@@ -115,35 +116,39 @@
       - 3.兼容仿真工具WeBots
       - 4.兼容仿真工具UnityForROS2
       - 5.ROS2内存管理工具
+  
 - （二）机器人学篇(更新中)
   - 第 6 章 运动学基础
     - 章节导读
     - 基础篇-数学基础
-      - 1.矩阵与矩阵运算
-      - 2.MiniConda与Jupyter介绍安装
-      - 3.矩阵运算实战
+      -  [1.矩阵与矩阵运算](humble/chapt6/basic/1.矩阵与矩阵运算.md) 
+      -  [2.MiniConda与Jupyter介绍安装](humble/chapt6/basic/2.MiniConda与Jupyter介绍安装.md) 
+      -  [3.矩阵运算实战](humble/chapt6/basic/3.矩阵运算实战.md) 
+      
     - 入门篇-机器人运动学
-      - 1.空间坐标描述
-      - 2.空间坐标描述实战
-      - 3.姿态的多种表示
-      - 4.姿态转换实战
-      - 5.齐次坐标变换
-      - 6.齐次坐标变换实战
-      - 7.机器人运行学介绍
+      -  [1.空间坐标描述](humble/chapt6/get_started/1.空间坐标描述.md) 
+      -  [2.空间坐标描述实战](humble/chapt6/get_started/2.空间坐标描述实战.md) 
+      -  [3.姿态的多种表示](humble/chapt6/get_started/3.姿态的多种表示.md) 
+      -  [4.姿态转换实战](humble/chapt6/get_started/4.姿态转换实战.md) 
+      -  [5.齐次坐标变换](humble/chapt6/get_started/5.齐次坐标变换.md) 
+      -  [6.齐次坐标变换实战](humble/chapt6/get_started/6.齐次坐标变换实战.md) 
+      -  [7.机器人运行学介绍](humble/chapt6/get_started/7.机器人运行学介绍.md) 
       <!-- - 两轮差速运动学 -->
       <!-- - 机械臂运动学 -->
+      
     - 进阶篇-运动学进阶
-      - 动力学基础-力与速度
-
+      - 待定
+      <!-- - 动力学基础-力与速度 -->
+    
   - 第 7 章 ROS2运动学
     - 章节导读
     - 基础篇-常用工具
-      - MiniConda与Jupyter介绍安装
+      - 待定
     - 入门篇-机器人运动学
-      - 1.TF2介绍
-      - 2.坐标变换发布监听Python实现
-      - 3.坐标变换发布监听C++实现
-      - 4.坐标变换可视化
+      -  [1.TF2介绍](humble/chapt7/get_started/1.TF2介绍.md) 
+      -  [2.学会使用RVIZ2-TF组件](humble/chapt7/get_started/2.学会使用RVIZ2-TF组件.md) 
+      -  [3.坐标变换发布监听Python实现](humble/chapt7/get_started/3.坐标变换发布监听Python实现.md) 
+      -  [4.坐标变换发布监听C++实现](humble/chapt7/get_started/4.坐标变换发布监听C++实现.md) 
     - 进阶篇-TF时间机制
       - 1.ROS2-TF时间机制
 

docs/foxy/chapt7/7.1.1数学基础.md

@@ -116,7 +116,6 @@ $$
 标量乘法即一个矩阵和一个数相乘。运算法则：将矩阵的每一个元素都乘上这个数即可
 
 **栗子：**
-
 $$
 A = \begin{bmatrix}{1}&{2}\\{3}&{4}\\\end{bmatrix}\\
 $$
@@ -136,12 +135,17 @@ $$
 $$
 A = \begin{bmatrix}{1}&{2}\\{3}&{4}\\\end{bmatrix}, B = \begin{bmatrix}{1}&{0}\\{0}&{3}\\\end{bmatrix} \\
 $$
+
 则
 $$
 A \times B = C = \begin{bmatrix}{c_{11}}&{c_{12}}\\{c_{21}}&{c_{22}}\\\end{bmatrix} 
 $$
+
+
 $$
-c_{11} =1*1+2*0 = 1 $$
+c_{11} =1*1+2*0 = 1 
+$$
+
 $$
 c_{12} =1*0+2*3 = 6 
 $$
@@ -157,11 +161,13 @@ $$
 $$
 C = \begin{bmatrix}{1}&{6}\\{3}&{12}\\\end{bmatrix}
 $$
+
 > 乘积之和其实就是点乘运算，比如栗子：
 > $$
 > a = [1,2,3],b = [0,1,2]\\
 > a\cdot b =1*0+2*1+3*2 = 8
 > $$
+> 
 > 放一张摘抄的很形象的图片：
 >
 > ![矩阵乘法点积](7.1.1数学基础/imgs/matrix-multiply-a.svg)
@@ -179,12 +185,11 @@ $$
 $$
   (A\times B)\times C = A \times(B\times C)
 $$
-  
+
 - 分配律
 $$
   A\times(B+C) = A\times B + A\times C
 $$
-
 ### 2.3转置运算
 
 转置运算定义非常简单，将矩阵的对应行列元素互换(右上角加${T}$表示）

docs/humble/chapt6/basic/1.矩阵与矩阵运算.md

@@ -0,0 +1,248 @@
+# 1.矩阵与矩阵运算
+
+大家好，我是小鱼。本节我们来学习一下线性代数的基础中的矩阵部分，矩阵作为我们学习机器人学中最常用的基础知识，后面学习过程中我们会经常遇到，比如：表示旋转的旋转矩阵、坐标变换中的齐次矩阵、关节速度映射雅可比矩阵、仿真中的惯性矩阵等等。所以很有必要在正式学习之前，了解一下矩阵的概念及常用的矩阵定义。
+
+## 1.矩阵介绍
+
+### 1.1 矩阵定义
+
+由$m*n$个数$a_{ij}(i=1,2,..,m;j=1,2...,n)$排成的m行n列的矩阵表格
+$$
+\begin{bmatrix}
+{a_{11}}&{a_{12}}&{\cdots}&{a_{1n}}\\
+{a_{21}}&{a_{22}}&{\cdots}&{a_{2n}}\\
+{\vdots}&{\vdots}&{\ddots}&{\vdots}\\
+{a_{m1}}&{a_{m2}}&{\cdots}&{a_{mn}}\\
+\end{bmatrix}
+$$
+称为一个$m*n$的矩阵,记为为$A$或$(a_{ij})_{m*n}(i=1,2,..,m;j=1,2...,n)$，当$m=n$时称$A$为$n$阶方阵.
+
+> 矩阵就是一堆可能存在着某种联系数的组合，编号规则也很简单，第一行第一列的数编号为$a_{11}$,第二行第一列叫做$a_{21}$，以此类推
+
+如果两个矩阵都是m*n个数组成，则称两个矩阵为同型矩阵。
+
+### 1.2 零矩阵
+
+所谓零矩阵，就是矩阵中每个数都是$0$,比如一个$3*3$的$0$矩阵（零矩阵常用$O$来表示）
+$$
+O_{3*3} =\begin{bmatrix}{0}&{0}&{0}\\{0}&{0}&{0}\\{0}&{0}&{0}\\\end{bmatrix}
+$$
+
+零矩阵是不是和自然数零一样神奇呢？
+
+根据矩阵的运算法则，零矩阵有以下性质，下一节我们会来动手验证。
+
+- 任何矩阵（前提符合运算法则）与零矩阵相加、减结果都是其本身
+  $$
+  A-O=A \\
+  \\
+  A+O=A 
+  $$
+
+- 零矩阵与任何矩阵的相乘结果都是零矩阵（注意，矩阵型号可能会变）
+
+### 1.3 单位矩阵
+
+主对角线上的元素都为$1$，其余元素全为$0$的$n$阶矩阵称为$n$阶单位矩阵,常用符号$I$表示，如$I3$
+$$
+I_{3} =\begin{bmatrix}{1}&{0}&{0}\\{0}&{1}&{0}\\{0}&{0}&{1}\\\end{bmatrix}
+$$
+
+单位矩阵的性质与自然数1相似
+
+根据矩阵的运算法则，单位矩阵有以下性质：
+
+任何矩阵与单位矩阵的乘积结果为其本身
+$$
+AI_n = A\\\\
+I_nB = B
+$$
+
+## 2.矩阵的运算
+
+### 2.1加减法运算
+
+**两个矩阵相加减，即其对应元素相加减。**
+
+设矩阵
+$$
+A=\begin{bmatrix}
+{a_{11}}&{a_{12}}&{\cdots}&{a_{1n}}\\
+{a_{21}}&{a_{22}}&{\cdots}&{a_{2n}}\\
+{\vdots}&{\vdots}&{\ddots}&{\vdots}\\
+{a_{m1}}&{a_{m2}}&{\cdots}&{a_{mn}}\\
+\end{bmatrix},B=\begin{bmatrix}
+{b_{11}}&{b_{12}}&{\cdots}&{b_{1n}}\\
+{b_{21}}&{b_{22}}&{\cdots}&{b_{2n}}\\
+{\vdots}&{\vdots}&{\ddots}&{\vdots}\\
+{b_{m1}}&{b_{m2}}&{\cdots}&{b_{mn}}\\
+\end{bmatrix}
+$$
+有
+
+$$
+A\pm\;B=\begin{bmatrix}
+{a_{11}\pm\;b_{11}}&{a_{12}\pm\;b_{12}}&{\cdots}&{a_{1n}\pm\;b_{1n}}\\
+{a_{21}\pm\;b_{21}}&{a_{22}\pm\;b_{22}}&{\cdots}&{a_{2n}\pm\;b_{2n}}\\
+{\vdots}&{\vdots}&{\ddots}&{\vdots}\\
+{a_{m1}\pm\;b_{m1}}&{a_{m2}\pm\;b_{m2}}&{\cdots}&{a_{mn}\pm\;b_{mn}}\\
+\end{bmatrix}
+$$
+> **只有两个矩阵为同型矩阵时才能进行加减运算。**
+
+**运算性质**
+
+- 交换律：$A+B=B+A$
+
+- 结合律：$(A+B)+C=A+(B+C)$
+
+- 乘法结合律：$(AB)C=A(BC)$
+
+**栗子：**
+
+$$
+A=\begin{bmatrix}{1}&{0}&{1}\\{1}&{0}&{0}\\{0}&{0}&{1}\\\end{bmatrix},B=\begin{bmatrix}{0}&{0}&{1}\\{1}&{0}&{0}\\{0}&{0}&{1}\\\end{bmatrix}
+$$
+$$
+A+B= B+A =\begin{bmatrix}{1}&{0}&{2}\\{2}&{0}&{0}\\{0}&{0}&{2}\\\end{bmatrix}
+$$
+
+### 2.2乘法运算
+
+乘法运算分为两种，一种是标量乘法，一种是矩阵乘法。
+
+#### 2.2.1 标量乘法
+
+标量乘法即一个矩阵和一个数相乘。运算法则：将矩阵的每一个元素都乘上这个数即可
+
+**栗子：**
+$$
+A = \begin{bmatrix}{1}&{2}\\{3}&{4}\\\end{bmatrix}\\
+$$
+$$
+2\times A= 2\times \begin{bmatrix}{1}&{2}\\{3}&{4}\\\end{bmatrix} = \begin{bmatrix}{2 \times 1}&{2 \times 2}\\{2 \times 3}&{2 \times 4}\\\end{bmatrix} =\begin{bmatrix}{2}&{4}\\{6}&{8}\\\end{bmatrix}
+$$
+
+#### 2.2.2 矩阵运算
+
+##### 运算法则
+
+设矩阵$A\times B = C = (c_{ij})_{m*n}$，则$C$的第$i$行第$j$列的元素$c_{ij}$的值等于矩阵A的第$i$行元素和矩阵B的第$j$列元素两两乘积之和。
+
+**栗子：**
+
+设：
+$$
+A = \begin{bmatrix}{1}&{2}\\{3}&{4}\\\end{bmatrix}, B = \begin{bmatrix}{1}&{0}\\{0}&{3}\\\end{bmatrix} \\
+$$
+
+则
+$$
+A \times B = C = \begin{bmatrix}{c_{11}}&{c_{12}}\\{c_{21}}&{c_{22}}\\\end{bmatrix} 
+$$
+
+
+$$
+c_{11} =1*1+2*0 = 1 
+$$
+
+$$
+c_{12} =1*0+2*3 = 6 
+$$
+
+$$
+c_{21} = 3*1+4*0 = 3 
+$$
+
+$$
+c_{22} = 3+0 + 4*3 = 12 
+$$
+
+$$
+C = \begin{bmatrix}{1}&{6}\\{3}&{12}\\\end{bmatrix}
+$$
+
+> 乘积之和其实就是点乘运算，比如栗子：
+> $$
+> a = [1,2,3],b = [0,1,2]\\
+> a\cdot b =1*0+2*1+3*2 = 8
+> $$
+>
+> 放一张摘抄的很形象的图片：
+>
+> ![矩阵乘法点积](7.1.1数学基础/imgs/matrix-multiply-a.svg)
+> 矩阵的乘法的意义是非常有意思的，这里放一个链接，欢迎大家阅读：[ 矩阵乘法的本质是什么？](https://www.zhihu.com/question/21351965)
+
+##### 运算性质
+
+尝试计算下上面栗子中的$B\times A$的值，得到的结果依然是上面栗子中的$C$吗?
+
+答案：并不是，一般情况下，矩阵的乘法并不满足交换律
+
+矩阵的运算规律：
+
+- 结合律
+$$
+  (A\times B)\times C = A \times(B\times C)
+$$
+
+- 分配律
+$$
+  A\times(B+C) = A\times B + A\times C
+$$
+### 2.3转置运算
+
+转置运算定义非常简单，将矩阵的对应行列元素互换(右上角加${T}$表示）
+$$
+C = \begin{bmatrix}{c_{11}}&{c_{12}}\\{c_{21}}&{c_{22}}\\\end{bmatrix} ,C^{T} =  \begin{bmatrix}{c_{11}}&{c_{21}}\\{c_{12}}&{c_{22}}\\\end{bmatrix} \\
+$$
+**栗子：**
+$$
+A = \begin{bmatrix}{1}&{2}\\{3}&{4}\\\end{bmatrix},A^T = \begin{bmatrix}{1}&{3}\\{2}&{4}\\\end{bmatrix}
+$$
+**运算规律**
+$$
+(A^T)^T = A 
+$$
+
+$$
+(A+B)^T = A^T + B^T 
+$$
+
+$$
+(AB)^T = B^TA^T
+$$
+## 3.重要定义
+
+### 3.1 矩阵的逆
+
+#### 3.1.1 定义
+$A,B$是n阶方阵，E是n阶单位矩阵，若$AB=BA=E$,则称$A$是可逆矩阵，并称B是A的逆，且逆矩阵是唯一的，记作$A^{-1}$
+
+这个我们本节课就不举栗子了，下节课我们使用代码来直接求。
+
+> 注意：不一定所有的矩阵都是可逆的
+
+#### 3.1.2 运算规律
+$$
+(A^{-1})^{-1} = A 
+$$
+
+$$
+AB可逆，(AB)^{-1}=B^{-1}A^{-1}
+$$
+
+## 4.总结
+
+本节主要介绍了下矩阵的定义的一些性质，还有很多需要补充的，由于时间原因先发一下文章，小鱼要去录课了
+
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划

docs/humble/chapt6/basic/2.MiniConda与Jupyter介绍安装.md

@@ -0,0 +1,184 @@
+# 2.MiniConda与Jupyter介绍安装
+
+大家好，我是小鱼，上一节我们介绍了机器人学的基础——矩阵相关知识，秉持着学以至用的原则，小鱼准备带大家先用代码来实现下矩阵的创建以及矩阵的运算。
+
+工欲善其事，必先利其器，如果是编写机器人相关算法，小鱼最为推荐的就是使用jupyter来和ros进行通信了。
+
+所以本节课小鱼就带你一起安装好MiniConda，并在MiniConda中安装好jupyter并配置好ros2环境。
+
+
+
+## 1.MiniConda与Jupyter介绍
+
+### 1.1 MiniConda 介绍
+
+Miniconda 是 Conda 的免费最小安装程序。
+
+![miniconda](2.MiniConda与Jupyter介绍安装/imgs/68fd19a405984074bb66f252b1768b1a.png)
+
+**Conda是什么呢？**
+Conda是在Windows、macOS和Linux上运行的开源软件包管理系统和环境管理系统。Conda可以快速安装、运行和更新软件包及其依赖项。
+
+**所以相比于conda的大体积miniconda更加的小和易用。**
+
+
+### 1.2 Jupyter介绍
+
+可以把jupyter理解为一个**可以在网页运行python**语言的工具。
+
+![jupyter](2.MiniConda与Jupyter介绍安装/imgs/6100b4f6bff0418faead3cf324f79d96.png)
+
+
+jupyter提出了文学化编程的概念，让我们可以在单元格(cell)中，**像在笔记本上写文章一样，随时写，随时运行，随时根据运行的结果来修改我们的代码**，这样的交互式编程模式可以极大的提升我们编写代码的效率。
+
+
+## 2.安装miniconda与jupyer
+
+### 2.1 miniconda下载
+
+- minconda下载地址：https://docs.conda.io/en/latest/miniconda.html
+
+这里小鱼推荐选择`Python3.8 Miniconda3 Linux 64-bit`
+
+![miniconda下载版本](2.MiniConda与Jupyter介绍安装/imgs/cc7b32eb5290421998ffbb195bccd32d.png)
+
+下载完成后你可以得到一个.sh的安装文件
+
+![安装文件](2.MiniConda与Jupyter介绍安装/imgs/8280c3a01583481dbe5a8248391d5448.png)
+
+
+### 2.2 miniconda安装
+
+在安装目录文件夹空白处右击，在终端中打开,输入下面指令即可安装
+
+```bash
+bash Miniconda3-py38_4.10.3-Linux-x86_64.sh 
+```
+
+![安装miniconda](2.MiniConda与Jupyter介绍安装/imgs/d0712dc35c0b4ef7b06e24409f962c3a.png)
+
+一路点enter，然后输入yes，回车即可完成安装，最后一步会提醒你是否要自动初始化环境，这里我们先选择yes。
+
+没有选没事，跟着小鱼来一起初始化一次conda，打开终端输入下面的命令即可
+
+```bash
+cd ~/miniconda3/bin
+./conda init
+```
+
+重启终端，你会发现你的终端前多了一个`base`,这代表你已经装好了conda。
+
+![base](2.MiniConda与Jupyter介绍安装/imgs/2b7b5d46297f417d8193c15ade9703aa.png)
+
+如果想退出conda环境可以输入
+
+```bash
+conda deactivate
+```
+
+### 2.3 jupyter安装
+
+方便起见，我们就在base环境下安装jupyter，使用的是清华大学源。
+
+```bash
+pip3 install jupyter -i https://pypi.tuna.tsinghua.edu.cn/simple
+```
+
+安装完成后，我们就可以使用下面的指令启动jupyter了，不出意外你的浏览器将要跳出来，然后自动打开conda页面。
+
+```bash
+jupyter-notebook
+```
+
+![启动jupyter](2.MiniConda与Jupyter介绍安装/imgs/a37fb5886320420c8a008b41f5570326.png)
+
+## 3.使用jupyter编写节点李四
+
+接着小鱼带你一起使用jupyter来写ros2的代码。
+
+## 3.1 新建Python3并编写第一行代码
+
+点击新建Python3
+
+![](2.MiniConda与Jupyter介绍安装/imgs/975648e46abd4fd0932c25668aa01521.png)
+
+然后你可以看到一行行的单元格。
+
+![jupyter代码编写界面](2.MiniConda与Jupyter介绍安装/imgs/f4f899e96b3c4fd1bb525aff1cc1a81e.png)
+
+接着我们就可以在单元格里输入python代码，然后使用`Shift+Enter`来运行这行代码，比如我们可以试一试打印一句话
+
+```bash
+print("hello jupyter")
+```
+
+接着我们使用`Shift+Enter`来运行这行代码，可以看到这行代码的结果，随之展示在了该句打印的下方，我们可以修改打印的内容，然后重新使用`Shift+Enter`来运行，下面的数值也会随之改变。
+
+![在这里插入图片描述](2.MiniConda与Jupyter介绍安装/imgs/156096c7220641e193eb4120bf6529f2.png)
+
+## 3.2 编写ROS2代码
+
+使用ros2无非是使用其rclpy客户端库，jupyter完美的支持了该库，一起和小鱼用import来导入吧。
+
+第一行输入下面的指令，点击Shift+Enter可以看到没有任何报错，即导入成功了。
+
+```python
+import rclpy
+from rclpy.node import Node 
+```
+
+接着在和小鱼一起初始化客户端库，新建节点李四和spin节点吧，完整的代码如下：
+
+```python
+import rclpy
+from rclpy.node import Node 
+rclpy.init() # 初始化rclpy
+node = Node("li4")  # 新建一个节点
+node.get_logger().info("大家好，我是作家li4.")
+rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
+```
+
+分步输入运行
+
+![分布输入运行](2.MiniConda与Jupyter介绍安装/imgs/a7a988ae701b425589bdcd127b472731.png)
+
+注意这里我们没有调用shutdown,因为我们还不希望rclpy关掉。
+
+## 3.3 测试
+
+我们接下来用，ros2的命令行工具查看一下li4节点是否在线。
+
+在jupyter中运行命令行和打开终端也是非常方便的，为了学习jupyter，我们使用jupyter自带的终端来运行ros2的指令。
+
+我们新建一个终端，回到文件夹目录视图，点击新建终端
+
+![新建终端](2.MiniConda与Jupyter介绍安装/imgs/fa57ec6d47b64b938d5e6aee7e880353.png)
+
+打开后的样子如下
+
+![jupyter的终端](2.MiniConda与Jupyter介绍安装/imgs/395a98223c2a4f879a7775e222b306c8.png)
+
+在终端中输入
+
+```python
+ros2 node list
+```
+
+可以看到李四已经出现了
+
+![结果](2.MiniConda与Jupyter介绍安装/imgs/79fc43950de34915ad38d209f71af3cb.png)
+
+## 4.总结
+
+小鱼不能说吹爆jupyter，但用python和ros写算法调试程序，jupyter真的是非常的好用，还有更强大的画图功能等等，小鱼后面在机器人学学习课程中都会一一讲到，并且带着大家一起来熟悉jupyter与ros2的更多知识。
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划
+

docs/humble/chapt6/basic/3.矩阵运算实战.md

@@ -0,0 +1,205 @@
+# 3.矩阵运算实战
+
+上一节我们安装好了MiniConda和Jupyter，本节课我们继续回到学习机器人学的路上来。首先我们来学习使用一个矩阵库Numpy，并且通过Numpy将我们第一节学习到的矩阵相关计算一一实现。
+
+## 1.NumPy是什么
+
+`NumPy`是一个功能强大的`Python`库，主要用于对多维数组执行计算。NumPy这个词来源于两个单词-- `Numerical`和`Python`。NumPy提供了大量的库函数和操作，可以帮助程序员轻松地进行数值计算。
+
+通过这个库我们可以非常简单的完成矩阵的创建、矩阵的计算，不管是二维还是多维都非常的`easy`.
+
+NumPy可以使用在以下场景：
+
+- **机器学习模型：** 在编写机器学习算法时，需要对矩阵进行各种数值计算。例如矩阵乘法、换位、加法等。NumPy提供了一个非常好的库，用于简单(在编写代码方面)和快速(在速度方面)计算。NumPy数组可用于存储训练数据和机器学习模型的参数。
+
+- **图像处理和计算机图形学：** 计算机中的图像表示为多维数字数组。NumPy成为同样情况下最自然的选择。实际上，NumPy提供了一些优秀的库函数来快速处理图像。例如，镜像图像、按特定角度旋转图像等。
+
+- **数学任务：** NumPy对于执行各种数学任务非常有用，如数值积分、微分、内插、外推等。因此，当涉及到数学任务时，它形成了一种基于Python的对MATLAB的快速替代。
+
+## 2.Numpy安装
+
+我们尝试在单元格内导入numpy，如果报错说明你没有安装
+
+```bash
+import numpy as np
+```
+
+![导入numpy](3.矩阵运算实战/imgs/c979e2884b7f40d28b49dfd195adbcd3.png)
+在conda环境中安装非常的简单，也是一句话的事，在单元格内输入下面的代码，然后`Shift+Enter`执行即可。
+
+> 注意指令前面有一个!，感叹号代表我们输入的是一句命令行
+
+```bash
+!pip install numpy  -i https://pypi.tuna.tsinghua.edu.cn/simple
+```
+
+![安装numpy](3.矩阵运算实战/imgs/c20c804d3c594267a7cd359afe321221.png)
+安装完成后，再次导入就不会报错了。
+
+## 3.创建矩阵
+
+导入完成之后我们就可以通过numpy来创建矩阵。
+
+> numpy中有矩阵和数组两个概念，数组相比矩阵更加灵活
+
+### 3.1 创建单位矩阵
+
+创建一个3*3的零矩阵。
+
+```bash
+np.identity(3)
+```
+
+![创建单位矩阵](3.矩阵运算实战/imgs/fee1809105154a88a37229de406bc017.png)
+
+### 3.2 创建零矩阵
+
+创建一个3*3的零矩阵。
+
+```python
+np.zeros([3,3])
+```
+
+![创建零矩阵](3.矩阵运算实战/imgs/9164b1c685724ffb8047859f5b0973c7.png)
+
+### 3.3 创建随机矩阵
+
+创建一个3*3的随机矩阵。
+
+```python
+np.random.rand(3,3)
+```
+
+![创建一个随机矩阵](3.矩阵运算实战/imgs/4fcc0b302fcf4ab2ac8539a4699da92d.png)
+
+### 3.4 从已有数组创建矩阵
+
+假设我们已经有了数据，我们想创建一个矩阵怎么办呢？
+
+比如我们创建一个2*2的矩阵，矩阵的数据分别是[1,2,3,4]。
+
+> 我们可以通过reshape改变矩阵的形状，这里我们把矩阵变成了2*2的样子。
+
+```python
+np.asarray([1,2,3,4]).reshape(2,2)
+```
+
+![从已有数据创建矩阵](3.矩阵运算实战/imgs/913e42cdf6de44fa9d0a80db02d581d8.png)
+
+### 3.5 判断两个矩阵是否相等
+
+numpy的allclose方法，比较两个array是不是每一元素都相等，默认在1e-05的误差范围内。
+
+我们做一个测试：
+
+```python
+print("零矩阵和单位矩阵",np.allclose(np.zeros([3,3]),np.identity(3)))
+print("单位矩阵和单位矩阵",np.allclose(np.identity(3),np.identity(3)))
+```
+
+![矩阵相等](3.矩阵运算实战/imgs/9a640b126bcd48bfa9f7d8d1ff7ac866.png)
+
+## 4.计算矩阵
+
+计算矩阵我们主要对第一节中的矩阵算法进行验证。
+
+### 4.1 矩阵加法/减法
+
+加法使用`np.add`,减法`np.subtract`
+
+
+```python
+A = np.asarray([1,2,3,4]).reshape(2,2)
+B = np.zeros(2)
+print(np.add(A,B))
+```
+
+![任意矩阵加上零矩阵还是其本身](3.矩阵运算实战/imgs/b3f97d2505804be8a9420969ca382826.png)
+
+### 4.2 矩阵乘法
+
+乘法使用`np.dot`
+
+**（前提同形）任何矩阵乘上零矩阵等于零矩阵，任何矩阵乘上单位矩阵等于其本身**
+
+```python
+A = np.asarray([1,2,3,4]).reshape(2,2)
+B = np.zeros([2,2])
+C = np.identity(2)
+print(np.dot(A,B))
+print(np.dot(A,C))
+```
+
+![矩阵乘法](3.矩阵运算实战/imgs/0253b1c32a744498afc749c6409ff400.png)
+
+### 4.3 矩阵求逆
+
+矩阵求逆使用`np.linalg.inv`
+
+**矩阵的逆与矩阵的乘积为单位矩阵**
+
+```python
+A = np.asarray([1,2,3,4]).reshape(2,2)
+A_INV = np.linalg.inv(A)
+print(A_INV)
+print(np.dot(A,A_INV))
+```
+
+![矩阵的逆](3.矩阵运算实战/imgs/9bcbc212d6af415cafe490fd3dd95b44.png)
+
+**需要注意的是，并不是所有的矩阵都有逆，比如零矩阵就没有逆，如果尝试用`numpy`来求逆则会出现错误**
+
+```python
+O = np.zeros(2)
+O_INV = np.linalg.inv(O)
+```
+
+![零矩阵求逆](3.矩阵运算实战/imgs/2a011b6a2b6a43ae80d445e79e912e4b.png)
+
+**另外如果使用的是`conda`提供的`numpy`包，对于某些不存在逆的矩阵，并不会抛出错误。**
+**但是对于通过`pip`安装的相同版本甚至更高版本的`numpy`，则不会出现类似问题。**
+
+```python
+""" 
+numpy packages isntalled from conda:
+numpy                     1.21.2           py38h20f2e39_0
+numpy-base                1.21.2           py38h79a1101_0
+"""
+c=np.asarray([
+    1,2,3,
+    4,5,6,
+    7,8,9,
+]).reshape((3,3))
+
+print("矩阵的行列式：",np.linalg.det(c))
+print("矩阵的秩：",np.linalg.matrix_rank(c))
+c_inv=np.linalg.inv(c)
+print("矩阵c和c_inv点乘的结果：","\n",np.dot(c,c_inv))
+```
+
+![使用conda安装的numpy求逆可能出现的问题](3.矩阵运算实战/imgs/image-20220216115928.PNG)
+
+### 4.4 矩阵转置
+
+矩阵转置在矩阵后使用`.T`即可
+
+```python
+A = np.asarray([1,2,3,4]).reshape(2,2)
+A.T
+```
+
+![转置运算](3.矩阵运算实战/imgs/b7f383304214422cb9320680ac35d6ef.png)
+
+## 5.总结
+
+今天一起学习了使用numpy操作矩阵，还有更多的资料可以在公众号后台回复numpy获取，我们下节见～
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划

docs/humble/chapt6/get_started/1.空间坐标描述.md

@@ -0,0 +1,374 @@
+# 1.空间坐标描述
+
+## 1.何为位姿
+
+### 1.1 引言
+
+在书籍《天才在左，疯子在右》中有这样一个故事，有一位十七岁的少年，他对量子力学有着独特的见解和远超同龄人的知识储备，他说四维生物突破了时空的界限，在它的观察中，将人类一生的活动看成一条连续的线，就像一条长长的虫子，它可以看到人的过去，也可以看到人的未来。
+
+我们对四维空间不了解，但对三维立体和二维的平面每天都有接触。一张铺平的中国地图是二维平面的，拔地而起的高楼大厦是三维立体的。
+
+那我们如何用数字表示高楼在地图中的位姿呢？又如何用数字表示某个高楼中你的位姿呢？
+
+### 1.2 我们先来看地图中高楼的位姿
+
+在地图上，我们可以利用经纬度坐标系来描述位姿，在经纬度坐标系中，我们可以使用经度值和纬度值表示地图上的某一个大楼的坐标，比如天安门的经纬度`经度：116.38 ，纬度：39.90`
+
+经纬度坐标系是以经纬度原点（几内亚湾）建立的二维平面直角坐标系，因此我们可以使用x,y来描述该坐标系中的任意一个点的位置。
+
+> 经度的起点就是指0度经线，位于英国格林尼治天文台旧址。 纬度的起点就是指0度纬线，就是指赤道。而经度0和纬度0的交点，大致位于非洲西部的几内亚湾，位于海上。
+>
+> ![img](1.空间坐标描述/imgs/format,f_auto.png)
+
+通过经纬度可以定位到地图中的建筑物的位置，但我们依然无法得知这个建筑物的朝向（东南西北），所以除了描述位置的经纬度外，我们还需要增加一个theta($\theta$),表示朝向。这样我们就得到了一个物体在二维平面中用数学描述的三个值，经度（x）、维度（y） 和 朝向（theta）。
+
+### 1.3 高楼中你的位姿
+
+现实世界是三维的，除了经纬度还有海拔高度。所以采用经度（x）、维度（y）并不能完整的描述出空间中物体的位置，例如只有经纬度无法表示你所处的楼层的海拔高度，这样就没办法确定你的位置。
+
+我们同样可以在经纬度原点建立一个三维空间直角坐标系，采用经度（x）、维度（y）和海拔高度（z）三个值来描述三维空间中任意一点的位置。
+
+和二维空间中类似，知道了你在某一个大楼中的位置还不够，并不能判断出你的姿态（躺平的、站着的还是侧卧的），那我们该如何描述三维空间中的姿态呢? 
+
+答案是旋转矩阵，旋转矩阵是什么？我们接着往下看。
+
+### 1.4 参考坐标系
+
+无论是在二维空间还是在三维空间，我们想要描述一个物体的位置和姿态第一步就是确定一个参考坐标系，物体的位置和姿态描述我们都是以这个坐标系作为参考的。
+
+> 参考坐标系，这一点很重要，因为没有绝对的坐标，只有相对的坐标。
+
+
+## 2.位置的表示
+
+从第一节背景中可以得知，二维平面中的位置可以用$x,y$表示，三维空间中的位置可以用$x,y,z$来表示。
+
+无论是在三维空间还是在二维平面，我们都可以使用$x,y,z$来表示位置，只不过对于二维空间来说，z的值是默认的一个固定不变的值，比如$0$
+
+假如我们确定了一个空间直角坐标系A,我们就可以使用$x,y,z$来确定A坐标系中任意一点P的位置，可以记作${^A}P$
+
+$$
+{^A}P=
+\begin{bmatrix}
+x \\
+y \\ 
+z  \\
+\end{bmatrix}
+$$
+
+### 2.1 位置矢量
+
+直角坐标系{A}其实可以看作是由三个互相正交（两两垂直）的单位矢量组成的。 那么在坐标系{A}中的一点P也可以写作矢量形式，其矢量形式由其在三个单位矢量上的分量组成。
+
+
+
+![image-20211102151409351](1.空间坐标描述/imgs/image-20211102151409351.png)
+
+
+
+所以参考坐标系{A}中一点P也可以写作
+$$
+{^A}P=
+\begin{bmatrix}
+{^x}p \\
+{^y}p \\ 
+{^z}p  \\
+\end{bmatrix} = [{^x}p，{^y}p , {^z}p  ]^T
+\tag{位置矢量}
+$$
+
+
+如果说位置矢量不太好理解，可以简单的认为就是坐标轴的$x,y,z$
+
+$$
+\begin{bmatrix}{x}\\{y}\\{z}\\\end{bmatrix}
+$$
+
+## 3.姿态的表示
+
+在背景中小鱼提到除了位置，坐标描述还有另外一个非常重要的组成部分——姿态。
+
+接着上面的说，我们已经知道坐标系A中的一个点P的位置，我们如何描述P点在{A}坐标系下的姿态呢？
+
+我们可以以P点为原点再建立一个坐标系(也可以认为该坐标系固定在物体P点上)，这样我们就可以通过描述**新坐标系{P}**和**参考坐标系{A}**之间的姿态关系来表示${^A}P$点的姿态。
+
+
+
+![rviz的位姿显示](1.空间坐标描述/imgs/image-20211102153515882.png)
+
+
+
+> 上图是使用RVIZ2绘制的，红色代表x轴，绿色代表y轴，蓝色代表z轴。
+
+从图中可以看出，坐标系{P}的每一个轴和参考坐标系的每一个轴之间都有一个角度，比如Px轴和Ax,Ay,Az三个轴之间存在三个角度，通过这三个角度我们就可以确定Px轴和参考坐标系{A}之间的关系，以此类推，我们也可以确定Py轴和Pz轴分别与{A}之间的关系。
+
+每个轴的相对姿态关系确定了，坐标系之间的姿态也就确定了，${^A}P$点的姿态也就确定了。
+
+### 3.1 旋转矩阵
+
+我们将上述坐标系{P}的三个轴相对于参考坐标系{A}三个轴的共九个角度的余弦值，组成一个3*3的矩阵，该矩阵就是旋转矩阵，因该矩阵是{P}相对于{A}的姿态关系的表示，故记作${^A_P}R$
+$$
+{^A_P}R=[{^A}x_{P} \ {^A}y_{P} \ {^A}z_{P}] = \begin{bmatrix}{r_{11}}&{r_{12}}&{r_{13}}\\{r_{21}}&{r_{22}}&{r_{23}}\\{r_{31}}&{r_{32}}&{r_{33}}\\\end{bmatrix} \tag{旋转矩阵}
+$$
+
+两个向量的点乘为两个向量的长度与它们夹角余弦的积,所以$r11$可以表示为单位向量$P_{x}$与$A_{x}$的点积，旋转矩阵就可以写为下面的形式
+
+$$
+{^A_P}R = \begin{bmatrix} 
+{P_{x}\cdot A_x} & {P_{y}\cdot A_x} & {P_{z}\cdot A_x}\\
+{P_{x}\cdot A_y} & {P_{y}\cdot A_y} & {P_{z}\cdot A_y}\\
+{P_{x}\cdot A_z} & {P_{y}\cdot A_z} & {P_{z}\cdot A_z}\\
+\end{bmatrix}
+$$
+
+
+将${^A_P}R$进行转置可得${^A_P}R^T$
+
+$$
+{^A_P}R^T = \begin{bmatrix} 
+{P_{x}\cdot A_x} & {P_{x}\cdot A_y} & {P_{x}\cdot A_z}\\
+{P_{y}\cdot A_x} & {P_{y}\cdot A_y} & {P_{y}\cdot A_z}\\
+{P_{z}\cdot A_x} & {P_{z}\cdot A_y} & {P_{z}\cdot A_z}\\
+\end{bmatrix}
+$$
+
+可以看出${^A_P}R^T$其实表示`坐标系{P}`作为参考坐标系下`坐标系{A}`的姿态，即
+$$
+{^A_P}R^T = {^P_A}R = {^A_P}R^{-1}
+$$
+
+需要注意的是：
+
+- 当两个坐标系之间姿态没有变化，即坐标系间$x,y,z$轴方向对应重合，则旋转矩阵为单位矩阵，这个很好求得，有兴趣的同学可以算一下
+- 从矩阵的角度看，矩阵的逆等于矩阵的转置，则该矩阵为正交矩阵，显而易见，旋转矩阵是正交矩阵
+
+## 4.位置+姿态
+
+通过位置矢量我们可以描述一个点在特定参考坐标系下的位置，通过旋转矩阵可以描述一个点在特定参考坐标系下的姿态。
+
+在机器人当中位置和姿态一般会一起出现，所以我们将其组合就叫做`位姿`
+
+#### 4.1 位姿描述的多个含义
+
+- 含义1：表示特定参考坐标系下某个物体（点）的位置和姿态，比如我们描述参考坐标系{A}中物体（点）P的位置和姿态
+
+- 含义2：表示两个坐标系之间的位姿关系，比如位置可以表示坐标系{A}和坐标系{B}原点位置关系，姿态可以表示两个坐标系坐标轴的朝向关系
+
+  坐标系之间关系，$^AP_{Bo}$表示两坐标系原点之间位置矢量
+  $$
+  {B} = \{ ^A_BR, {^A}P_{Bo} \}
+  $$
+
+- 含义3：两个物体之间的关系，我们通常把坐标系固定在物体上，这样就可以表示两个物体之间的位姿关系，比如自行车前轮和后轮的关系
+
+**小鱼说：学会了位置和姿态描述，三维空间坐标关系描述相信已经难不倒你了**
+
+## 5.坐标变换
+
+位姿是相对的，同一个物体在不同的参考坐标系下的位姿数据肯定是不同的。在后续的学习和使用当中，我们会经常需要求同一个点在不同坐标系的位姿表示，这就要求我们掌握坐标变换的方法了。
+
+比如在手眼系统中，我们可以通过视觉算法获取到`工件坐标系P`鱼`相机坐标系C`之间的关系，我们想要控制`机械臂的末端`运动到`工件坐标系P`进行夹取，那么我们就要知道`工件坐标系P`在`机器人基坐标系B`下的位姿，如何获取呢？这就需要进行坐标变换了。
+
+![手眼系统的坐标系关系](1.空间坐标描述/imgs/image-20211104102656102.png)
+
+### 5.1 平移坐标变换
+
+![RVIZ平移坐标变换](1.空间坐标描述/imgs/image-20211104110930303.png)
+
+如图，坐标系{A}、{B}、{C}的姿态是相同的，其之间的姿态对应的旋转矩阵都是单位矩阵。在我们已知：
+
+- {A}为参考坐标系，{B}的位置矢量${^A_B}P=[1,1,1]^T$
+- {B}为参考坐标系，{C}的位置矢量${^B_C}P=[3,0,1]^T$
+
+求：
+
+- {A}为参考坐标系，{C}的位置矢量${^A_C}P$
+
+大家可以利用简单的几何知识手算一下，再继续往下看
+
+正确的答案应该是：
+$$
+{^A_C}P=[4,1,2]^T
+$$
+算法也很简单，直接将${^A_B}P$鱼${^B_C}P$相加即可
+$$
+{^A_C}P ={^A_B}P+{^B_C}P=[1,1,1]^T+[3,0,1]^T=[4,1,2]^T
+$$
+由此可知，我们通过平移坐标变换可以求出同一个点（{P}）在相同姿态不同位置坐标系({A}鱼{B})下的不同表示，也可以得到坐标的平移方程
+$$
+{^A_C}P ={^A_B}P+{^B_C}P \tag{坐标平移方程}
+$$
+小鱼要提醒大家注意：在上述例子中，{A},{B},{C}三个坐标系的姿态都是相同的，所以${^A_C}R={^A_B}R={^B_C}R$，即都是单位旋转矩阵
+
+### 5.2 旋转坐标变换
+
+如果坐标系之间姿态不同，同一个点的位姿在不同参考姿态下的位置和姿态也会不一样。
+
+![RVIZ旋转坐标变换](1.空间坐标描述/imgs/image-20211104115451563.png)
+
+上图中，坐标系{A}鱼坐标系{B}原点重合（连名字都连一块了），我们已知：
+
+- {B}坐标系绕{A}的z轴即$A_z$旋转了$45^\circ$
+- {B}鱼{C}姿态相同，{B}为参考坐标系，{C}的位置矢量为${^B_C}P=[3,0,1]^T$
+
+求
+
+- {A}为参考坐标系，{C}的位置矢量${^A_C}P$
+- {A}为参考坐标系，{C}的旋转矩阵${^A_C}R$
+
+这次相对来说困难一些，不过我们依然可以手推出来,跟着小鱼的思路一起往下走
+
+#### 5.2.1 求${^A_C}R$
+
+因为{B}鱼{C}姿态相同，所以{A}参考坐标系下{C}的姿态和{A}参考坐标系下{B}的姿态是相同的，即：
+$$
+{^A_B}R={^A_C}R
+$$
+接着只需要求${^A_B}R$即可，因为{B}坐标系绕{A}的z轴即$A_z$旋转了$45^\circ$,我们可以一一计算旋转矩阵各个元素的值
+$$
+{^A_B}R ={^A_C}R = \begin{bmatrix} 
+{B_{x}\cdot A_x} & {B_{y}\cdot A_x} & {B_{z}\cdot A_x}\\
+{B_{x}\cdot A_y} & {B_{y}\cdot A_y} & {B_{z}\cdot A_y}\\
+{B_{x}\cdot A_z} & {B_{y}\cdot A_z} & {B_{z}\cdot A_z}\\
+\end{bmatrix}
+= \begin{bmatrix} 
+{cos(45)} & cos(90+45) & cos(90)\\
+{cos(90-45)} & cos(45) & cos(90)\\
+{cos(90)} & cos(90) & cos(0)\\
+\end{bmatrix}
+= \begin{bmatrix} 
+{cos(45)} & cos(90+45) & 0\\
+{cos(90-45)} & cos(45) & 0\\
+{0} &0 &1\\
+\end{bmatrix}
+$$
+将结果算出来，再利用[工具](https://www.andre-gaschler.com/rotationconverter/)来验证一下，看看最终结果对不对
+
+![绕z轴旋转45度](1.空间坐标描述/imgs/image-20211105104937239.png)
+
+到此我们求出了${^A_C}P$鱼${^A_C}R$，但聪明的鱼粉肯定不会就此罢休，我们接着来举一反三
+
+{B}绕着{A}的z轴旋转了45度我们计算出来了，那如果{B}绕着{A}旋转$\theta$角度该如何呢?相信聪明的鱼粉心中已经有了答案
+$$
+R(z,\theta)= \begin{bmatrix} 
+{cos\theta} & -sin\theta & 0\\
+{sin\theta} & cos\theta & 0\\
+{0} &0 &1\\
+\end{bmatrix} \tag{小鱼提示：重要方程1}
+$$
+那如果不是绕着z轴旋转，而是绕着x轴呢？
+$$
+R(x,\theta)= \begin{bmatrix} 
+1 & 0           &          0\\
+0 & {cos\theta} & -sin\theta \\
+0&{sin\theta} & cos\theta \\
+
+\end{bmatrix} \tag{小鱼提示：重要方程2}
+$$
+绕着y轴呢？
+$$
+R(y,\theta)= \begin{bmatrix} 
+{cos\theta} & 0 & {sin\theta}\\
+0 &1 &0\\
+{-sin\theta} & 0 &cos\theta \\
+
+\end{bmatrix} \tag{小鱼提示：重要方程3}
+$$
+
+#### 5.2.2 求$^A_CP$
+
+现在我们已知$^B_CP$和$^A_BR$,那如何求$^A_CP$呢，我们可以使用下面这个方程
+$$
+^A_CP = {^A_BR}{^B_CP}\tag{坐标旋转方程}
+$$
+关于这个方程，其实是将$^B_CP$在{B}上各轴的分量变换到了{A}的各轴上
+
+${^A_BR}$是一个$3*3$的矩阵，${^B_CP}$是$3*1$的矩阵，其相乘结果还是一个$3*1$的矩阵，利用前几节学习的矩阵乘法手算或者使用numpy进行计算，得到最终结果为：
+$$
+^A_CP = {^A_BR}{^B_CP}\ = [2.12,2.12,1]^T
+$$
+
+> 小思考:{C}的参考坐标系从{B}变成了{A},{C}在空间中的位置发生了变化了吗？
+
+### 5.3平移旋转复合变换
+
+在我们平时搞机（器人）时，一般情况下两个坐标系原点不重合姿态也不相同。我们将坐标变换拆分成先绕参考坐标系旋转，再绕参考坐标系平移两步，这样我们就得到了坐标的复合变换方程
+$$
+^A_CP = {^A_BR}{^B_CP}+^A_BP
+$$
+这个公式其实也很好理解，${^A_BR}{^B_CP}$就是将{C}在{B}上的各轴分量转换到{A}上，再和原来在{A}上各轴的分量再相加。不理解的同学也没事，后面我们使用最多的还是齐次变换矩阵，根据方程直接撸代码就行了。
+
+![复合变换](1.空间坐标描述/imgs/image-20211104160047629.png)
+
+## 6.左手还是右手
+
+最后小鱼要说的是左手坐标系和右手坐标系，一般情况下我们都是使用右手坐标系搞机。
+
+用右手确定的坐标系就是右手坐标系了，跟着小鱼一起做：
+
+拿起你的右手，先给自己竖个大拇指，然后打开手掌，将大拇指的方向朝向下图中的蓝色z轴，让剩下的四根手指朝向红色的x轴，此时朝向手心外的就是绿绿的y轴的方向了。
+
+![右手坐标系](1.空间坐标描述/imgs/image-20211104160841318.png)
+
+如果你换左手，做上述动作，你会发现y轴是朝手心里的。
+
+再放张图给肢体不协调的同学：
+
+![右手坐标系](1.空间坐标描述/imgs/image-20211104162846579.png)
+
+## 7.练习
+
+光说不练假把式，我们来做个手眼转换题
+
+![手眼系统的坐标系关系](1.空间坐标描述/imgs/image-20211104102656102.png)
+
+#### 如图🔓示，已知：
+
+1.相机坐标系{C}为参考坐标系，工具坐标系{P}的位置矢量在相机坐标系{C}的`x,y,z`各轴投影为$2,1,2$，并且工具坐标系和相机坐标系姿态相同。
+
+2.机器人基坐标系{B}为参考坐标系，相机坐标系{C}的位置矢量在{B}各轴的投影为$0,0,3$，坐标系{C}绕着坐标系{B}的x轴转了180度
+
+可以参考下图看题目
+
+![坐标系关系图](1.空间坐标描述/imgs/image-20211108213832470.png)
+
+#### 求：
+
+**{B}为参考坐标系，坐标系{P}的位置矢量和旋转矩阵**
+
+**答案：**
+
+位置矢量：
+$$
+[2,-1,1]^T
+$$
+
+
+旋转矩阵：
+$$
+\begin{bmatrix} 
+1 & 0 & 0\\
+0 & -1 & 0\\
+0 &0 & -1\\
+\end{bmatrix}
+$$
+
+
+
+
+**参考文档**
+
+- 机器人学导论
+- 机器人学基础
+
+
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划
+

docs/humble/chapt6/get_started/2.空间坐标描述实战.md

@@ -0,0 +1,164 @@
+# 2.空间坐标描述实战
+
+大家好，我是小鱼，上一节我们学习了坐标描述和坐标变换的理论知识，本节课我们把重点放到动手实现上，通过numpy实现坐标的描述和变换，同时结合ROS2鱼RVIZ进行坐标关系可视化与求解。
+
+通过本节你将掌握：
+
+- 使用numpy表示位置矢量和旋转矩阵
+- 使用numpy进行平移与旋转坐标变换
+- 了解ROS2中TF2的概念
+- 使用tf2相关CLI工具进行坐标变换
+- 使用python操作TF进行坐标变换
+
+## 1.numpy表示位姿
+
+在前几节中，小鱼带你安装使用了ROS2和MiniConda，并学习使用Numpy进行矩阵相关运算的验证。
+
+那我们如何使用numpy中的矩阵表示位置矢量和旋转矩阵呢？
+
+### 1.1 位置表示
+
+上一节中我们使用一个3行1列的矩阵表示了位置，那么我们在numpy中自然也可以使用这样一个矩阵表示。
+
+打开终端，输入下面指令打开jupyter
+
+```shell
+jupyter-notebook
+```
+
+新建一个python3的代码并重命名，接着在单元格中导入numpy
+
+```
+import numpy as np
+```
+
+![image-20211105102418250](2.空间坐标描述实战/imgs/image-20211105102418250.png)
+
+#### 1.1.1 位置矩阵
+
+使用3*1的矩阵表示位置，我们新建一个沿着x、y、z各平移1个单位的位置矢量。
+
+```
+np.asarray([1.0,1.0,1.0]).reshape(3,1)
+```
+
+![image-20211105102746824](2.空间坐标描述实战/imgs/image-20211105102746824.png)
+
+### 1.2 姿态表示
+
+姿态可以使用`3*3`的旋转矩阵表示,`3*3`的单位矩阵代表没有姿态变换，注意没有姿态变换不是零矩阵，而是单位矩阵。
+
+我们新建一个旋转矩阵，用该旋转矩阵表示绕着z轴旋转45度，可以这样写：
+
+```
+import math
+import numpy as np
+theta = math.radians(45)
+R_AB = np.asarray([math.cos(theta),-math.sin(theta),0,
+                   math.sin(theta),math.cos(theta),0,
+                   0,0,1
+                  ]).reshape(3,3)
+print(R_AB)
+```
+
+运行后得到的旋转矩阵是不是和上节的一样
+
+![image-20211105110244694](2.空间坐标描述实战/imgs/image-20211105110244694.png)
+
+## 2.numpy坐标变换
+
+掌握了使用numpy表示位置和姿态后，接着我们使用numpy来完成上一节的小练习
+
+### 2.1 题目
+
+#### 如图🔓示，已知：
+
+![手眼系统的坐标系关系](2.空间坐标描述实战/imgs/image-20211104102656102.png)
+
+1.相机坐标系{C}为参考坐标系，工具坐标系{P}的位置矢量在相机坐标系{C}的`x,y,z`各轴投影为$2,1,2$，并且工具坐标系和相机坐标系姿态相同。
+
+2.机器人基坐标系{B}为参考坐标系，相机坐标系{C}的位置矢量在{B}各轴的投影为$0,0,3$，坐标系{C}绕着坐标系{B}的x轴转了180度
+
+可以参考下图看题目
+
+![坐标系关系图](2.空间坐标描述实战/imgs/image-20211108213832470.png)
+
+#### 求：
+
+**{B}为参考坐标系，坐标系{P}的位置矢量和旋转矩阵**
+
+### 2.2 使用numpy求解
+
+#### 2.2.1 旋转矩阵求解
+
+这里我们就需要使用复合坐标变换了，根据坐标变换规则有：
+$$
+^B_PR=^B_CR^C_PR
+$$
+{C}和{P}姿态相同，所以$^C_PR$是一个单位矩阵。又因为{C}绕着{B}的x旋转了180度，根据上节的重要公式2可知
+$$
+R(x,\theta)= \begin{bmatrix} 
+1 & 0           &          0\\
+0 & {cos\theta} & -sin\theta \\
+0&{sin\theta} & cos\theta \\
+
+\end{bmatrix} \tag{小鱼提示：重要方程2}
+$$
+所以$^B_CR$对应的程序可以这样写
+
+```python
+import math
+import numpy as np
+theta = math.radians(180)
+R_BC = np.asarray([1,0,0,
+                   0,math.cos(theta),-math.sin(theta),
+                   0,math.sin(theta),math.cos(theta)]).reshape(3,3)
+```
+
+所以$^B_PR$可以用程序求得：
+
+```
+R_BP = R_BC*np.identity(3)
+print("旋转矩阵R_BP：\n",R_BP)
+```
+
+![完整程序及结果](2.空间坐标描述实战/imgs/image-20211108224851838.png)
+
+#### 2.2.2 平移矩阵求解
+
+根据复合变换规则可知：
+$$
+^B_PP=^B_CR^C_PP+^B_CP
+$$
+根据描述有：$^B_CP=[0,0,3]^T$、$^C_PP=[2,1,2]^T$
+
+所以可以写这样写程序：
+
+```python
+P_BC = np.asarray([0,0,3]).reshape(3,1)
+P_CP = np.asarray([2,1,2]).reshape(3,1)
+P_BP = np.add(np.dot(R_BC,P_CP),P_BC)
+print("位置矢量P_BP:\n",P_BP.T)
+```
+
+运行下，可以得到结果
+
+![完整结果](2.空间坐标描述实战/imgs/image-20211108231029275.png)
+
+### 2.3 结果对比
+
+与上节课答案一致
+
+位置矢量：$[2,-1,1]^T$
+
+旋转矩阵：
+$$\begin{bmatrix} 
+1 & 0 & 0\\
+0 & -1 & 0\\
+0 &0 & -1\\
+\end{bmatrix} \tag{小鱼提示：重要方程1}$$
+
+
+
+以上大家跟着小鱼一起学习了使用numpy表示位置和姿态，并使用了numpy来进行坐标变换，接下来我们直接使用ROS2中的TF工具不写一行代码进行坐标发布，同时我们还可以使用RVIZ2直观的查看坐标关系。
+

docs/humble/chapt6/get_started/3.姿态的多种表示.md

@@ -0,0 +1,337 @@
+# 3.姿态的不同表示
+
+大家好，我是小鱼，本节课，我们来学习姿态的多种表示方式。在前面的课程中，我们一共接触了三种姿态的表示方式：
+
+1. 旋转矩阵-在位姿描述一节中
+2. 坐标轴旋转-绕xyz轴旋转不同的角度(欧拉角)
+3. 四元数-ROS2的TF2中的姿态描述
+
+本节小鱼将对以上的三种姿态描述进行归类与介绍，并对他们之间的转换方法进行讲解，下一节小鱼带你一起通过代码直观的观察和操作姿态变换。
+
+小鱼将常用的坐标描述分为三类，共五种。这五种也是小鱼在平时工作中所接触到的几乎所有姿态描述方法，三类共五种方法如下：
+
+- 旋转矩阵-`旋转矩阵`
+- 坐标轴旋转-`固定轴欧拉角`,`非固定轴欧拉角`
+- 任意轴旋转-`等效轴角`,`四元数`
+
+常用的坐标转换包括：
+
+- `固定角`与`四元数`互转
+- `固定角`与`旋转矩阵`互转
+- `四元数`与`旋转矩阵`互转
+
+
+## 1.旋转矩阵
+
+关于旋转矩阵我们在前几节教程中已经介绍了，旋转矩阵采用的是`旋转后的坐标系`三个轴分别与`原坐标系`三个轴的夹角余弦值共九个数字组成的3*3矩阵。
+
+旋转矩阵一般记作$R$ 
+
+> 若两个坐标系姿态相同，其旋转矩阵为单位矩阵。
+
+### 1.1 旋转矩阵的描述
+
+![坐标系和坐标系P关系](3.姿态的多种表示/imgs/watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBA6bG86aaZUk9T,size_20,color_FFFFFF,t_70,g_se,x_16.png)
+
+
+如图，描述`坐标系{P}`和`参考坐标系{A}`之间的姿态关系的旋转矩阵用符号${^A_P}R$来表示。
+
+
+$$
+{^A_P}R=[{^A}x_{P} \ {^A}y_{P} \ {^A}z_{P}] = \begin{bmatrix}{r_{11}}&{r_{12}}&{r_{13}}\\{r_{21}}&{r_{22}}&{r_{23}}\\{r_{31}}&{r_{32}}&{r_{33}}\\\end{bmatrix} \tag{旋转矩阵}
+$$
+
+两个向量的点乘=两个向量的长度(1)与它们夹角余弦的积，所以$r11$可以表示为向量$P_{x}$与$A_{x}$的点积，旋转矩阵就可以写为下面的形式
+
+$$
+{^A_P}R = \begin{bmatrix} 
+{P_{x}\cdot A_x} & {P_{y}\cdot A_x} & {P_{z}\cdot A_x}\\
+{P_{x}\cdot A_y} & {P_{y}\cdot A_y} & {P_{z}\cdot A_y}\\
+{P_{x}\cdot A_z} & {P_{y}\cdot A_z} & {P_{z}\cdot A_z}\\
+\end{bmatrix}
+$$
+
+
+### 1.2 绕某一轴旋转$\theta$角的旋转矩阵
+
+`新的坐标系`绕`原坐标系`某一坐标轴旋转任意角度得到的旋转矩阵有如下等式。
+
+#### 绕x轴旋转$\theta$后姿态矩阵
+
+$$
+R(x,\theta)= \begin{bmatrix} 
+1 & 0           &          0\\
+0 & {cos\theta} & -sin\theta \\
+0&{sin\theta} & cos\theta \\
+\end{bmatrix}
+$$
+
+#### 绕y轴旋转$\theta$后姿态矩阵
+
+$$
+R(y,\theta)= \begin{bmatrix} 
+{cos\theta} & 0 & {sin\theta}\\
+0 &1 &0\\
+{-sin\theta} & 0 &cos\theta \\
+\end{bmatrix}
+$$
+
+#### 绕z轴旋转$\theta$后姿态矩阵
+
+$$
+R(z,\theta)= \begin{bmatrix} 
+{cos\theta} & -sin\theta & 0\\
+{sin\theta} & cos\theta & 0\\
+{0} &0 &1\\
+\end{bmatrix} 
+$$
+
+
+
+## 2.欧拉角-绕坐标轴的旋转
+
+### 2.1 12种旋转顺序
+
+旋转矩阵是一个冗余的（九个值之间存在约束关系），可以只需要三个参数来表示的矩阵。聪明的鱼粉肯定会想到，假如知道坐标系绕分别绕X、Y、Z轴的旋转角度，不就同样可以表示旋转了吗？
+
+这个猜想是对的，结合1.2中绕三个轴旋转的三个$\theta$，按照特定的顺序将对应的旋转矩阵乘起来就可以确定一个旋转矩阵。
+
+但需要注意的是，矩阵的乘法不具备交换性，所以旋转顺序不同会造成不同的结果。
+
+比方说若是先绕自身$x$轴旋转$\alpha$，再绕自身$y$轴旋转$\beta$：
+
+$$
+R(x,\alpha)R(y,\beta)=
+\begin{bmatrix} 
+1 & 0           &          0\\
+0 & {cos\alpha} & -sin\alpha \\
+0&{sin\alpha} & cos\alpha \\
+\end{bmatrix}
+\begin{bmatrix} 
+{cos\beta} & 0 & {sin\beta}\\
+0 &1 &0\\
+{-sin\beta} & 0 &cos\beta 
+\end{bmatrix}=
+\begin{bmatrix} 
+{cos\beta} & 0 & {sin\beta}\\
+sin\alpha sin\beta &cos\alpha & -sin\alpha cos\beta\\
+{-cos\alpha sin\beta} & sin\alpha &cos\alpha cos\beta \\
+\end{bmatrix}
+$$
+
+
+比方说若是，先绕自身$y$轴旋转$\beta$，绕自身$x$轴旋转$\alpha$：
+
+$$
+R(y,\beta)R(x,\alpha)=
+\begin{bmatrix} 
+{cos\beta} & 0 & {sin\beta}\\
+0 &1 &0\\
+{-sin\beta} & 0 &cos\beta \\
+\end{bmatrix}
+\begin{bmatrix} 
+1 & 0           &          0\\
+0 & {cos\alpha} & -sin\alpha \\
+0&{sin\alpha} & cos\alpha \\
+\end{bmatrix}=
+\begin{bmatrix} 
+cos\beta   & sin\alpha sin\beta  & {sin\beta}\\
+0 & {cos\alpha} & -sin\alpha \\
+{-sin\beta} & sin\alpha &cos\alpha sin\beta \\
+\end{bmatrix}
+$$
+
+
+所以我们对旋转顺序做排列组合，可以得到12种旋转顺序:
+
+- `xyz`,`xyx`, `xzy`
+- `xzx`,`yzx`, `yzy`
+- `yxz`, `yxy` , `zxy`
+- `zxz`, `zyx`, `zyz`
+
+### 2.2 两种参考坐标系
+
+**除了要考虑旋转时所绕轴的顺序，还要考虑参考坐标系（坐标轴）的不同。**
+
+#### 2.2.1 参考固定的坐标系
+
+![固定轴旋转](3.姿态的多种表示/imgs/808db9fe269b4684bc0537e3154f2a32.gif)
+
+假设坐标系B与坐标系A初始姿态相同
+
+- 坐标系{B}绕坐标系A的x轴`Ax`旋转$\alpha$
+- 接着坐标系{B}绕着A的y轴`Ay`旋转$\beta$
+- 接着绕`Az`旋转$\gamma$
+
+上述三次旋转，都是以A坐标系的xyz轴为参考坐标系进行旋转，该旋转方式为固定旋转轴的旋转，通常称之为固定角欧拉角或固定轴旋转。
+
+#### 2.2.2 参考自身坐标系
+
+![参考自身坐标系](3.姿态的多种表示/imgs/fa42e7371bf84f848eaf55c4406699b5.gif)
+
+我们也可以不沿着坐标系A的各轴旋转，而是绕旋转之后B的某一轴再次旋转，我们称之为非固定旋转轴的欧拉角。
+
+> 小鱼说：无论是参考自身坐标系还是参考固定的坐标系，都有12种旋转方式，所以欧拉角有12*2=24种旋转方式，后面的计算中我们也将直观的感受到24种旋转方式的不同。
+
+### 2.3 固定转轴欧拉角 转 旋转矩阵
+
+首先我们来考虑绕固定的坐标系旋转如何转换成旋转矩阵
+
+我们以XYZ的旋转顺序来举例说明，其他旋转顺序类似
+
+现在假设A、B两个坐标系重合，B坐标系绕A坐标系的X轴旋转45度，绕A的Z轴旋转90度.
+
+求旋转之后A为参考坐标系，B坐标系的姿态$^A_BR_{XYZ(45,0,90)}$
+
+小鱼先告诉你最终的结果：
+$$
+^A_BR_{XYZ(45,0,90)}=R_{Z(90)}R_{Y(0)}R_{X(45)}=R_{Z(90)}R_{X(45)}=\begin{bmatrix} 
+0.&-0.70710678 & 0.70710678\\
+-1&0&0\\ 
+0&0.70710678&0.70710678\\
+\end{bmatrix}
+$$
+
+为什么结果是将绕Z轴的旋转矩阵乘绕X轴的旋转矩阵呢？
+
+这里引用林沛群老师的解释：
+
+我们可以假设一个向量v固定在B坐标系上，那我们让B坐标系绕着A坐标系的三个轴做旋转，就可以认为是让向量v绕着坐标系A的三个轴做旋转，那先转的肯定先乘，所以我们先让向量v乘上Rx(45)，再让其乘上Rz(90)，即：
+$$
+v' = R_{Z(90)}(R_{X(45)}v)
+$$
+根据矩阵乘法的结合律，括号可以去掉：
+$$
+v' = R_{Z(90)}R_{X(45)}v
+$$
+
+所以我们可以得到，绕固定轴XYZ旋转的欧拉角转旋转矩阵方法:
+$$
+R_{XYZ(\gamma,\beta,\alpha)}=R_{Z(\alpha)}R_{Y(\beta)}R_{X(\gamma)}
+$$
+最终结果：
+![固定轴欧拉角转旋转矩阵](3.姿态的多种表示/imgs/b36d9d33b5ae47bfb3759556f10fd70f.png)
+
+根据旋转顺序不同，固定角有12种旋转方式，这里我们给出了绕固定轴以XYZ顺序旋转欧拉角的转旋转矩阵的等式，其他旋转顺序对应的旋转矩阵可以尝试自行推导。
+
+### 2.2 非固定旋转轴的欧拉角
+
+非固定旋转轴，即每次旋转是绕着自身的坐标轴进行旋转，其旋转动图如2.2.2节所示。
+
+非固定旋转的欧拉角转旋转矩阵推导也很简单，我们以旋转顺序ZYX为例子分析
+
+#### 2.2.1 ZYX
+
+因为每次旋转都是绕着自身进行的，我们可以将每次的旋转进行拆解
+
+$$
+^A_BR={^A_{B'}R}{^{B'}_{B''}R}{^{B''}_{B}R}
+$$
+
+等式右边的三次旋转按照顺序Z-Y-X进行的，所以最终B坐标系在A坐标系下的姿态为：
+
+$$
+^A_BR_{Z'Y'X'}=R_{Z(\alpha)}R_{Y(\beta)}R_{X(\gamma)}
+$$
+
+最终结果太难敲，小鱼直接截图啦
+![绕自身旋转欧拉角转旋转矩阵](3.姿态的多种表示/imgs/1d478c77e59d4e93a42ae944ea6bd3a9.png)
+
+> 旋转矩阵转欧拉角的方法需要使用双参变量的反正切函数，我们后面在程序当中直接调用对应函数即可实现，这里对原理就不再进行推导了
+
+
+
+## 3.轴角
+
+在介绍四元数之前，我们先来说说等效角度轴线，这种表示姿态的方式。
+
+上一节欧拉角中无论是绕着自身的某个轴旋转，还是绕着固定的坐标系的某个轴进行旋转，旋转时参考的轴都是坐标系的主轴
+
+假如我们参考的轴不是主轴，那么任何姿态都可以通过选择适当的轴和角度得到，换句话说，两个坐标系之间的任何姿态都可以通过绕某一个特定的轴(矢量)旋转特定的角度得到。
+
+说到这里相信你已经理解了轴角的意义，接着我们给出轴角和旋转矩阵之间的转换关系
+
+### 轴角转旋转矩阵
+
+假设坐标系B和参考坐标系A重合，将B绕着A坐标系下的矢量$^AK$按右手定则旋转$\theta$角度，旋转之后B坐标系在A坐标系下的姿态可以用
+$$
+^A_BR(K,\theta)
+$$
+表示，注意矢量K为单位矢量(模长为1)，K为一个3*1的矢量
+$$
+^AK=[k_x,k_y,k_z]^T
+$$
+
+在已知矢量K和$\theta$的情况下，我们如何得到旋转矩阵呢?
+
+有等式：
+![轴角转旋转矩阵](3.姿态的多种表示/imgs/a8faaa8c30654a39a62a4b05fb276b60.png)
+其中
+
+$$
+c\theta=cos\theta \\
+s\theta=sin\theta \\
+v\theta=1-cos\theta
+$$
+
+$\theta$的符号由右手定则确定，右手大拇指指向矢量K的方向.
+
+
+
+旋转矩阵转轴角需要根据情况讨论，该部分转换我们直接调用相应函数实现，这里对其原理不再叙述，感兴趣的同学可以参考：https://en.wikipedia.org/wiki/Rotation_matrix#Axis_of_a_rotation
+
+
+
+## 4.四元数
+
+除了轴角可以使用一个数字表示角度，三个数字表示旋转轴，一共四个数字表示旋转外。还有另外一种四个数字表示表示旋转的方式——四元数。
+
+四元数的四个数字由一个实部和三个虚部组成，是一个超复数形式
+
+$$
+q = w + x*i+ y*j + z*k
+$$
+
+
+关于四元数的由来有个小故事，小鱼分享一下：
+
+> 1843年10月16日的傍晚，英国数学家哈密顿和他的妻子一起步行去都柏林，途中经过布鲁哈姆桥时，他的脚步突然放慢了。妻子以为他要尽情欣赏周围的景色，于是也放慢了脚步。其实哈密顿此时正在思考他久久不能解决的问题。早在1828年，他就想发明一种新的代数，用来描述绕空间一定轴转动并同时进行伸缩的向量的运动。他设想这种新代数应包含四个分量：两个来固定转动轴，一个来规定转动角度，第四个来规定向量的伸缩。但是在构造新代数的过程中，由于他受传统观念的影响，不肯放弃乘法交换律，故屡受挫折。哈密顿盲目地相信，普通代数最重要的规律必定继续存在于他寻找的代数中。然而此刻，他的脑际突然产生了一个闪念：在所寻找的代数中，能否让交换律不成立呢？比方说，A×B不等于B×A而是等于负的B×A。这个想法太大胆了，他感到非常激动。哈密顿马上掏出笔记本，把他的思想火花记录下来。这一火花就是I，J，K之间的基本方程，即四元数乘法基本公式。哈密顿因此把1843年10月16日称为四元数的生日。此后，哈密顿一生的最后22年几乎完全致力于四元数的研究，成果发表在他去世后出版的《四元数基础》一书中。四元数的出现，推倒了传统代数的关卡，故有数学史上里程碑的美誉。后人为了纪念这一发明，特意在当年哈密顿刻划过的石头上镶嵌了一块水泥板，上面清楚地记载着1843年曾经发生的故事。
+
+
+
+四元数在机器人中使用的非常多，甚至在量子力学中都有使用，关于四元数旋转的本质，小鱼也学习了很久才搞清楚，B站上3B1B的视频非常经典，大家自行食用。
+
+
+
+> 在机器人学当中用到的四元数都是单位四元数（四维单位超球体在三维空间的投影）,下文中提到的四元数默认指单位四元数
+
+
+
+<iframe height="400" width="600" src="//player.bilibili.com/player.html?aid=33385105&bvid=BV1SW411y7W1&cid=58437850&page=1" scrolling="no" border="0" frameborder="no" framespacing="0" allowfullscreen="true"> </iframe>
+
+接着我们来说说四元数常用的转换
+
+四元数转旋转矩阵
+
+![image-20211229184439675](3.姿态的多种表示/imgs/image-20211229184439675.png)
+
+旋转矩阵转四元数
+
+![image-20211229184453523](3.姿态的多种表示/imgs/image-20211229184453523.png)
+
+四元数转欧拉角
+
+![image-20211229184338075](3.姿态的多种表示/imgs/image-20211229184338075.png)
+
+轴角转四元数
+
+轴:$^AK=[k_x,k_y,k_z]^T$ 角:$\theta$
+$$
+x= k_x\sin(\theta/2)\\
+y= k_y\sin(\theta/2) \\
+z= k_z\sin(\theta/2)\\
+w = \cos(\theta/2)
+$$
+
+>  小计算: $w^2+x^2+y^2+z^2=1$
+

docs/humble/chapt6/get_started/4.姿态转换实战.md

@@ -0,0 +1,142 @@
+# 4.姿态转换实战
+
+上一节小鱼带你一起学习了四种姿态表示方式，这节课我们就利用相关的开源库，来完成姿态的不同表示方式之间的转换。
+
+
+包含12中转换形式，其中一些转换的计算方式，前面几节课中小鱼有给出相应的公式。
+
+四元数模块:quaternions
+
+- 四元数 转 旋转矩阵
+- 旋转矩阵 转 四元数
+- 四元数 转 轴角
+- 轴角 转 四元数
+
+欧拉模块:euler
+
+- 欧拉角 转 四元数
+- 四元数 转 欧拉角
+- 欧拉角 转 旋转矩阵
+- 旋转矩阵 转 欧拉角
+- 欧拉角 转 轴角
+- 轴角 转欧拉角
+
+轴角模块:axangles
+
+- 轴角 转 旋转矩阵
+- 旋转矩阵 转 轴角
+
+
+本节主要推荐的是Python的相关实现，用的是小鱼常用的transforms3d库,该库的api设计非常的巴适，小鱼是非常的爱~
+
+## 1.安装
+
+安装使用pip即可
+
+```
+pip install transforms3d -i https://pypi.tuna.tsinghua.edu.cn/simple
+```
+
+## 2.四元数相关转换
+
+四元数模块在transforms3d.quaternions里，直接导入即可使用
+
+### 2.1 四元数与旋转矩阵互转
+
+```
+import transforms3d as tfs
+import numpy as np 
+# 四元数转旋转矩阵
+tfs.quaternions.quat2mat([1,0,0,0])
+# 旋转矩阵转四元数
+tfs.quaternions.mat2quat(np.asarray([[1., 0., 0.],[0., 1., 0.],[0., 0., 1.]]))
+```
+
+### 2.2 四元数与轴角互转
+
+
+```
+import transforms3d as tfs
+import numpy as np 
+# 四元数转旋轴角
+tfs.quaternions.quat2axangle([1,0,0,0])
+# 轴角转四元数
+tfs.quaternions.axangle2quat([1,0,0],0.5)
+```
+
+## 3.欧拉角相关转换
+
+四元数模块在transforms3d.euler里，直接导入即可使用
+
+### 3.1 欧拉角与四元数互转
+
+```
+import transforms3d as tfs
+import numpy as np 
+# 固定轴欧拉角转四元数
+tfs.euler.euler2quat(0,0,0,"sxyz")
+# 四元数转固定轴欧拉角
+tfs.euler.quat2euler([1,0,0,0],"sxyz")
+```
+
+### 3.2 欧拉角与旋转矩阵互转
+
+```
+import transforms3d as tfs
+import numpy as np 
+# 固定轴欧拉角转旋转矩阵
+tfs.euler.euler2mat(0,0,0,"sxyz")
+# 旋转矩阵转固定轴欧拉角
+tfs.euler.mat2euler(np.asarray([[1., 0., 0.],[0., 1., 0.],[0., 0., 1.]]),"sxyz")
+```
+
+
+### 3.3 欧拉角与轴角互转
+
+```
+import transforms3d as tfs
+import numpy as np 
+# 固定轴欧拉角轴角
+tfs.euler.euler2axangle(0,0,0,"sxyz")
+# 轴角转固定轴欧拉角
+tfs.euler.axangle2euler([1,0,0],0.5,"sxyz")
+```
+
+## 4.轴角相关转换
+
+四元数模块在transforms3d.axangle里，直接导入即可使用
+
+4.1 轴角与旋转矩阵互转
+
+```
+import transforms3d as tfs
+import numpy as np 
+# 轴角转旋转矩阵
+tfs.axangles.axangle2mat([1,0,0],0.5)
+# 旋转矩阵转轴角
+tfs.axangles.mat2axangle(np.asarray([[1., 0., 0.],[0., 1., 0.],[0., 0., 1.]]))
+```
+
+## 5.C++实现Eigen
+
+> 开源地址,欢迎一起加入完善: https://gitee.com/ohhuo/transforms3d_cpp
+
+![image-20211230112734251](4.姿态转换实战/imgs/image-20211230112734251.png)
+
+
+
+参考文档：
+
+- http://matthew-brett.github.io/transforms3d/
+
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划
+

docs/humble/chapt6/get_started/5.齐次坐标变换.md

@@ -0,0 +1,98 @@
+# 5.齐次坐标变换
+
+前面几节中，小鱼带你一起学习了使用TF进行坐标的变换，也带你通过旋转和平移求解了坐标的变换关系，但计算的过程中旋转和平移是分开计算的，那有没有一种方法，可以让旋转矩阵和平移向量合并到同一个矩阵里呢？
+
+答案是有的，我们可以将$3*3$的旋转矩阵和$3*1$的平移矩阵进行组合，并添加一行(0,0,0,1)使其变成一个$4*4$的方阵，其组合方式如下：
+
+有旋转矩阵
+$$
+R = \begin{bmatrix}{r_{11}}&{r_{12}}&{r_{13}}\\{r_{21}}&{r_{22}}&{r_{23}}\\{r_{31}}&{r_{32}}&{r_{33}}\\\end{bmatrix} \tag{旋转矩阵}
+$$
+平移矩阵
+$$
+P= \begin{bmatrix}{x}\\{y}\\{z}\\\end{bmatrix}  \tag{平移矩阵}
+$$
+
+
+合并成齐次变换矩阵
+$$
+T = \begin{bmatrix}{r_{11}}&{r_{12}}&{r_{13}}&{x}
+\\{r_{21}}&{r_{22}}&{r_{23}}&{y}
+\\{r_{31}}&{r_{32}}&{r_{33}}&{z}
+\\0&0&0&1
+\\\end{bmatrix} 
+\tag{齐次矩阵}
+$$
+
+
+为什么要这样写，我们可以简单的推导一下，矩阵是支持分块运算的，我们将上面的矩阵进行分块
+$$
+T = \begin{bmatrix}{R}&{P}
+\\0&1\\\end{bmatrix} \tag{齐次矩阵}
+$$
+假设$^A_BT$表示B坐标系到A坐标系的齐次变换，B坐标系下的点C坐标为$^B_CP$，求C在A坐标系下的坐标$^A_CP$
+
+我们将$^A_BT$乘$^B_CP$上，可得
+$$
+^A_CP= 
+\begin{bmatrix}{^A_BR}&{^A_BP}\\0&1\\\end{bmatrix}
+\begin{bmatrix}{^B_CP}\\1\\\end{bmatrix} 
+= {^A_BR}{^B_CP}+^A_BP
+$$
+根据前面学习的平移+旋转复合坐标变换公式，正确的结果如下
+$$
+^A_CP = {^A_BR}{^B_CP}+^A_BP
+$$
+你会发现，两者最终结果完全相同，也就是说，我们的平移加旋转复合变换，可以直接用齐次变换矩阵代替。
+
+
+
+## 1.齐次变换矩阵特性
+
+接着我们来探索一下齐次变换矩阵的一些特性
+
+### 2.1.齐次变换矩阵的符号表示
+
+一般使用H或者T来表示齐次变换矩阵，矩阵的左上角标明参考坐标系，矩阵左下角标明目标坐标系，比如$^A_BT$表示B坐标系到A坐标系的变换关系(平移+旋转)
+
+### 2.2.齐次变换矩阵的逆的几何含义
+
+就像矩阵的逆一样，齐次变换矩阵也有逆，其逆也有对应的几何含义，比如
+
+比如$^A_BT$表示B坐标系到A坐标系的变换关系
+
+那么
+
+$^A_BT$的逆$^A_BT^{-1}=^B_AT$表示A坐标系到B坐标系的变换关系
+
+### 2.3.齐次变换矩阵的乘法的几何含义
+
+#### 3.3.1齐次矩阵与平移向量相乘
+
+齐次矩阵与平移向量相乘，即可求出某个向量在另一坐标系下的表示，上面例子中即是如此。
+
+#### 3.3.2齐次矩阵与齐次矩阵相乘
+
+齐次矩阵与齐次矩阵相乘，可以转换不同坐标系之间的关系，比如：
+$$
+^A_BT^B_CT=^A_CT
+$$
+比如当我们有一个六自由度的机械臂，知道两两相邻关节之间的关系，那么就可以通过其次矩阵相乘的方法求出，关节6在关节0下的位置和姿态：
+$$
+^0_1T^1_2T^2_3T^3_4T^4_5T^5_6T=^0_6T
+$$
+
+## 3.练习
+
+练习小鱼放到了下一节了，毕竟不希望大家用手来算~
+
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划

docs/humble/chapt6/get_started/6.齐次坐标变换实战.md

@@ -0,0 +1,241 @@
+# 6.齐次坐标变换实战
+
+上一节我们对齐次矩阵的组成和齐次矩阵的求逆和乘法两个运算的几何意义进行了介绍。
+
+本节课我们就通过对应的函数和库实现齐次矩阵的生成，齐次矩阵的乘法和求逆。
+
+## 1.齐次矩阵的合成与分解
+
+齐次矩阵的的生成可以一个姿态和一个平移向量组成，因为姿态可以用四元数、欧拉角、轴角、旋转矩阵四种方式来表示
+
+所以我们考虑先将对应的姿态转成旋转矩阵，然后使用numpy讲旋转矩阵和平移向量填写到齐次矩阵对应的位置即可
+
+### 1.1旋转矩阵+平移向量
+
+```
+#导入库
+import numpy as np
+import transforms3d as tfs
+```
+
+```
+# 定义旋转矩阵R和平移向量T
+R = np.asarray([[1., 0., 0.],[0., 1., 0.],[0., 0., 1.]])
+T = np.asarray([1,0,1])
+R,T
+```
+
+![image-20220104124708934](6.齐次坐标变换实战/imgs/image-20220104124708934.png)
+
+#### 1.1.1 使用numpy方法合成齐次变换矩阵
+
+```
+temp = np.hstack((R,T.reshape(3,1)))
+np.vstack((temp,[0,0,0,1]))
+```
+
+![image-20220104124655166](6.齐次坐标变换实战/imgs/image-20220104124655166.png)
+
+#### 1.1.2 使用tfs中的函数合成齐次变换矩阵
+
+```
+tfs.affines.compose(T,R,[1,1,1])
+```
+
+![image-20220104124645988](6.齐次坐标变换实战/imgs/image-20220104124645988.png)
+
+### 1.2四元数+平移向量
+
+思路:先将四元数转换成旋转矩阵，然后再利用1.1合成齐次矩阵
+
+```
+R = tfs.quaternions.quat2mat([1,0,0,0])
+tfs.affines.compose(T,R,[1,1,1])
+```
+
+![image-20220104124632233](6.齐次坐标变换实战/imgs/image-20220104124632233.png)
+
+### 1.3 练习
+
+#### 1.3.1 练习1
+
+已知相机坐标系{C}为参考坐标系，工具坐标系{P}的位置矢量在相机坐标系{C}`x,y,z`各轴投影为$2,1,2$，并且工具坐标系和相机坐标系姿态相同，求$^C_PT$
+
+#### 1.3.2 练习2
+
+已知机器人基坐标系{B}为参考坐标系，相机坐标系{C}在的位置矢量在{B}各轴的投影为$0,0,3$,坐标系{C}和绕着坐标系{B}的x轴转了180度，求$^B_CT$
+
+
+
+## 2.齐次矩阵的分解
+
+齐次矩阵的分解指的是已有齐次矩阵的情况下，将其分解为姿态和平移两部分
+
+### 2.1 将qcjz分解为固定轴欧拉角和平移向量
+
+```
+tfs.euler.mat2euler(T[0:3,0:3]),T[:3,3:4]
+```
+
+![image-20220108161652509](6.齐次坐标变换实战/imgs/image-20220108161652509.png)
+
+
+
+### 2.3 将qcjz分解为四元数和平移向量
+
+```
+tfs.quaternions.mat2quat(T[0:3,0:3]),T[:3,3:4]
+```
+
+![image-20220108161712715](6.齐次坐标变换实战/imgs/image-20220108161712715.png)
+
+
+
+## 3.齐次矩阵的乘法
+
+对应numpy中矩阵的乘法`np.dot`讲两个矩阵相乘即可，我们以一道例题来讲解这个问题。
+
+### 3.1 练习-眼在手外
+
+如图🔓示，已知：
+
+![手眼系统的坐标系关系](6.齐次坐标变换实战/imgs/image-20211104102656102.png)
+
+1.相机坐标系{C}为参考坐标系，工具坐标系{P}的位置矢量在相机坐标系{C}`x,y,z`各轴投影为$2,1,2$，并且工具坐标系和相机坐标系姿态相同。
+
+2.机器人基坐标系{B}为参考坐标系，相机坐标系{C}在的位置矢量在{B}各轴的投影为$0,0,3$,坐标系{C}和绕着坐标系{B}的x轴转了180度
+
+可以参考下图看题目
+
+![坐标系关系图](6.齐次坐标变换实战/imgs/image-20211108213832470.png)
+
+求：
+
+{B}为参考坐标系，坐标系{P}的位置矢量和旋转矩阵
+
+解体思路也很简单，我们只要得出
+
+- B到C的齐次变换矩阵$^B_CT$
+- C到P的齐次变换矩阵$^C_PT$
+
+得到之后将两者相乘即可得出：
+$$
+^B_PT=^B_CT^C_PT
+$$
+求出$^B_PT$我们再将其分解成位置矢量和旋转矩阵即可
+
+动手写代码：
+
+**先求T_BC**
+
+```
+import math
+T_BC = tfs.affines.compose([0,0,3],tfs.euler.euler2mat(math.pi,0,0),[1,1,1])
+T_BC
+```
+
+![image-20220108162310322](6.齐次坐标变换实战/imgs/image-20220108162310322.png)
+
+**再求T_CP**
+
+```
+T_CP =  T = tfs.affines.compose([2,1,2],np.identity(3),[1,1,1])
+T_CP
+```
+
+![image-20220108162328993](6.齐次坐标变换实战/imgs/image-20220108162328993.png)
+
+**求T_BP**
+
+```
+T_BP = np.dot(T_BC,T_CP)
+T_BP
+```
+
+![image-20220108162303105](6.齐次坐标变换实战/imgs/image-20220108162303105.png)
+
+**分解成欧拉角对比结果**
+
+```
+tfs.euler.mat2euler(T_BP[0:3,0:3]),T_BP[:3,3:4]
+```
+
+![image-20220108162339874](6.齐次坐标变换实战/imgs/image-20220108162339874.png)
+
+到这里我们就利用做了齐次矩阵的乘法完成了坐标的变换
+
+## 3.齐次矩阵求逆
+
+### 3.1练习-眼在手上
+
+![image-20220108163802318](6.齐次坐标变换实战/imgs/image-20220108163802318.png)
+
+如图机器人基座坐标系为B、末端坐标系为E、相机坐标系为C、物品坐标系为O、其中相机固定在机械臂的末端。
+
+已知
+$$
+^B_ET={ xyz:[0.5,0.6,0.8] ,qwqxqyqz:[1,0,0,0]} \\
+^C_ET={ xyz:[0.00,0.05,0.05] ,qwqxqyqz:[0.707, 0.706, 0,0]} \\
+^C_OT={ xyz:[0.00,0.02,0.85] ,qwqxqyqz:[0.877,0.479,0,0]} \\
+$$
+求：$^B_OT$
+$$
+^B_OT=^B_ET^E_CT^C_OT \\
+^E_CT=^C_ET^{-1}
+$$
+写代码：
+
+```
+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])
+```
+
+```
+T_EC = np.linalg.inv(T_CE)
+```
+
+```
+np.dot(np.dot(T_BE,T_EC),T_CO)
+```
+
+![image-20220108174934212](6.齐次坐标变换实战/imgs/image-20220108174934212.png)
+
+## 4.练习
+
+### 4.1 map坐标系转换
+
+在移动机器人导航中，存在这样一个坐标系关系.
+
+地图坐标系(Map)->里程计坐标系(Odom)->机器人坐标系(BaseLink)
+
+其中里程计到机器人坐标系关系一般是由底盘轮子编码器给出，而地图坐标系和里程计坐标系之间的关系是通过定位模块估算出来的。
+
+**所以请听题目：**
+
+现在通过地图匹配获取到机器人在地图中的位置为[1.5,2.3,0],姿态(固定轴欧拉角)为[0,0,3.14]
+
+查看里程计上报的机器人坐标为：位置[1.0,3.2,0] 姿态(固定轴欧拉角)[0,0,1.0]
+
+求地图坐标系和里程计坐标系之间的关系
+
+### 4.2 机械臂运动学正解
+
+已知一个3自由的机械臂，已知:
+
+- 关节1和关节2坐标关系为:[0,0,0.2] 固定轴欧拉角:[0,0,1.57]
+- 关节2和关节3坐标关系为:[0.5,0,0.0] 固定轴欧拉角:[0,0,1.0]
+
+求关节1和关节3之间的关系？
+
+
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划

docs/humble/chapt6/get_started/7.机器人运动学介绍.md

@@ -0,0 +1,119 @@
+# 7. 机器人运动学介绍
+
+机器人学是一个非常庞大的学科，凡是和机器人设计应用相关的都可以划分到机器人学中，主要有运动学和动力学、系统结构、感知传感技术、运动规划技术、决策技术等。
+
+为了方便后面的机器人建模和仿真，本节我们对机器人的运动学进行简单的介绍和学习。
+
+
+
+## 1.机器人运动学
+
+机器人运动学研究机器人的位姿关系，主要包含正向运动学和逆向运动学两类。
+
+正向运动学即给定机器人各关节变量，计算机器人末端的位置姿态；比如上节课我们已知机器人关节和关节之间的关系，求关节1和关节3之间的关系
+
+逆向运动学即已知机器人末端的位置姿态，反求机器人的关节变量；比如当我们已知机器人关节1和关节3之间的关系，求关节关节1和关节2，关节2和关节3之间关系。
+
+
+
+## 2.机械臂运动学介绍
+
+![image-20220108223220972](7.机器人运动学介绍/imgs/image-20220108223220972.png)
+
+正运动学：已知每个关节的角度，求末端的位姿
+
+逆运动学：已知末端姿态，求每一个关节的角度
+
+
+
+## 3.两轮差速底盘运动学介绍
+
+两轮差速模型指机器人底盘由两个驱动轮和若干支撑轮构成的底盘模型，像turtlebot和开源机器人fishbot都是两轮差速模型。
+
+![image-20220308230020572](7.机器人运动学介绍/imgs/image-20220308230020572.png)
+
+两轮差速模型通过两个驱动轮可以通过不同转速和转向，使得机器人的达到某个特定的角速度和线速度。
+
+![image-20220108223512852](7.机器人运动学介绍/imgs/image-20220108223512852.png)
+
+两轮的平衡车大家都见过吧，靠着两个轮子即可实现前后移动（线速度），左转右转（角速度）。
+
+### 3.1 正逆解
+
+了解了两轮差速模型，那正逆解又是怎么回事？
+
+正运动学：已知两个轮子的速度，求整车的角速度（弧度/秒）和线速度（米/秒）
+
+
+```mermaid
+graph LR;
+A[左轮当前速度]-->B[正运动学]-->C[机器人当前角速度]
+D[右轮当前速度]-->B
+B-->E[机器人当前线速度]
+```
+
+逆运动学：已知目标角速度和线速度，求两个轮子的转速
+
+```mermaid
+   graph LR;
+   A[机器人目标线速度]-->C[运动学逆解]
+   B[机器人目标角速度]-->C
+   C-->D[左轮目标速度]
+   C-->E[右轮目标速度]
+```
+
+
+
+### 3.2轮式里程计
+
+```mermaid
+graph LR;
+A[左右轮当前速度/位置]-->B[里程计推算]
+B-->C[里程计-odom]
+```
+
+
+当我们知道了两个轮子之间的相对位置，同时知道了每一时刻机器人的角速度和线速度，那我们如何获取机器人的当前角度和位置呢？
+
+#### 3.2.1 角度
+
+影响机器人当前角度的因素只有一个，就是角速度。
+
+某一时刻机器人转动的角度 = 这一时刻机器人的角速度*这一时刻时长
+
+假如我们认定初始时刻机器人的角度为0,通过对机器人转动角度角度进行累加，即可获得机器人的当前角度。
+
+**上述过程其实就是对角速度进行积分得到角度。**
+
+
+
+
+
+#### 3.2.2 位置
+
+通过对角速度积分，我们得到了角度。
+
+机器人某一时刻自身方向上的前进速度可以分解为里程计坐标系中x轴和y轴方向上的速度。
+
+![速度分解](7.机器人运动学介绍/imgs/OIP-C.kPisImqMhOUrt0gstMeUMAHaEv)
+
+从图中可以看出：
+$$
+v_y = v*cos(\theta) \\
+v_y = v*sin(\theta)
+$$
+得到了x和y方向上的速度，乘上该速度对应的某一时刻经过的时间，即可得到这一时刻在x轴和y轴方向上的位移，对位移进行累加即可得到里程计中的x和y。
+
+
+
+
+--------------
+
+技术交流&&问题求助：
+
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
+
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划
+

docs/humble/chapt7/get_started/1.TF2介绍.md

@@ -0,0 +1,176 @@
+# 1.tf2介绍
+
+TF即`变换`的英文单词`TransForm`的缩写。所以`ROS`和`ROS2`中的`TF`就是指和坐标变换相关的工具。
+
+> 在搞机器人当中，坐标变换经常用到，所以`ROS2`帮我们做了一个强大易用的TF工具
+
+![../../_images/simple_robot.png](1.TF2介绍/imgs/simple_robot.png)
+
+## 1.发布坐标关系
+
+我们先使用TF2的相关工具，解决上一节的手眼坐标转换问题，直观的感受一下TF2的强大。
+
+要想让TF帮我们完成坐标变换，我们就需要告诉它坐标和坐标之间的关系。
+
+![手眼系统](1.TF2介绍/imgs/820a681b891a4719867e0c84a492d85f.png)
+
+拿上面的手眼系统来说，我们要想获取到相机的基坐标系{B}和工具{P}之间的关系，只需要将机械臂和相机、相机和工具之间的关系告诉TF即可。
+
+## 2.我们如何告诉TF？
+
+可以使用tf的坐标广播工具进行广播坐标关系，广播时需要三个数据：
+
+- 父坐标系名称（字符串）
+- 子坐标系名称（字符串）
+- 父子之间的变换关系（平移关系和旋转关系）
+
+在终端中输入：
+
+```
+ros2 run tf2_ros static_transform_publisher 
+```
+
+按`enter`键，可以看到
+
+```
+A command line utility for manually sending a transform.
+Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id 
+OR 
+Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id 
+```
+
+这是该CLI所提供的使用提示，可以看出
+
+使用TF发布位置和姿态时，位置的描述使用的是xyz三个参数，而姿态的描述则分两种
+
+- 第一种是四元数形式（qx qy qz qw）
+- 第二种是欧拉角形式（yaw偏航角-rz pitch俯仰角-ry roll滚转角-rx），我们这里采用的是欧拉形式，绕x轴旋转采用欧拉角中的滚转角roll来描述，注意角度单位采用弧度制。
+
+![image-20220125112120457](1.TF2介绍/imgs/image-20220125112120457.png)
+
+>  关于欧拉角和四元数的区别我们放到了姿态的多种表示章节来讲。
+
+
+
+
+
+### 2.1 发布B到C的位姿
+
+比如针对上面的手眼转换，广播机械臂坐标系{B}和相机坐标系{C}之间的关系。
+
+父坐标系的名字就是B，子坐标系的名字是C，父子之间的平移关系是`0 0 3`,旋转关系是绕x轴旋转180度。
+
+在ROS2中可以使用下面的指令发布变换，打开终端，输入下面的指令：
+
+```
+ros2 run tf2_ros static_transform_publisher 0 0 3 0 0 3.14 B C
+```
+
+如果在终端中看到下面的提示则代表发布成功
+
+![发布位姿关系](1.TF2介绍/imgs/6b0d8be89ab04fbd8d82e9cea3d882c8.png)
+
+
+### 2.2 发布C到P的位姿
+
+接着我们发布坐标系{C}到坐标系{T}的位姿
+
+再打开一个新的终端，输入下面的命令：
+
+```
+ros2 run tf2_ros static_transform_publisher 2 1 2 0 0 0 C P
+```
+
+## 3. 监听/获取TF关系
+
+发布也发布了，接着我们就把坐标系之间的关系打印出来，只要坐标系之间是有连接的，我们就可以使用TF求出来，使用下面的指令就可以得到机械臂基坐标系{B}和工具坐标系{P}之间的关系。
+
+打开终端,输入命令：
+
+```bash
+ros2 run tf2_ros tf2_echo B P
+```
+
+![输出两个坐标之间关系](1.TF2介绍/imgs/6c85c97787474091acda23a39a66f9d1.png)
+
+可以看到终端中不断输出B和C之间的平移和旋转，平移采用的是xyz，基本正确，y和z的微小差异是因为我们发布变换时旋转输入的是3.14并不精确。
+
+至于旋转部分采用的是四元数表示，关于这部分姿态的表示，小鱼在下一节会讲，大家不必纠结。
+
+除了使用TF获取关系外，ros2还提供很多工具来查看坐标之间的关系，大家可以在终端中输入下面的命令自行尝试。
+
+## 4.TF常用工具
+
+### 4.1 rqt_tf_tree
+
+> 2022-04-26更新的，这个工具的二进制安装版本作者3月底才发布，之前没提的原因是需要源码装太麻烦了
+
+这个工具需要我们手动安装下
+
+```
+sudo apt install ros-foxy-rqt-tf-tree
+```
+
+安装完成后，再次打开rqt工具，`Plugins`->`Visualization`->`TF Tree`
+
+
+
+接着你就可以看到这个强大的，几乎可以实时看到系统tf更新信息的工具，这个工具对于后面我们进行导航和机械臂的调试非常有帮助。
+
+> 长的不一样没关系，这是后面补充的图。
+
+![image-20220426181252686](1.TF2介绍/imgs/image-20220426181252686.png)
+
+
+
+### 4.2 tf2_monitor
+
+查看所有的发布者和频率。
+
+```
+ros2 run tf2_ros tf2_monitor 
+Gathering data on all frames for 10 seconds...
+
+RESULTS: for all Frames
+
+Frames:
+Frame: C, published by <no authority available>, Average Delay: 3001.98, Max Delay: 3001.98
+Frame: P, published by <no authority available>, Average Delay: 741.497, Max Delay: 741.497
+All Broadcasters:
+Node: <no authority available> 5029.14 Hz, Average Delay: 1871.74 Max Delay: 3001.98
+
+
+RESULTS: for all Frames
+Frames:
+Frame: C, published by <no authority available>, Average Delay: 3001.98, Max Delay: 3001.98
+Frame: P, published by <no authority available>, Average Delay: 741.497, Max Delay: 741.497
+All Broadcasters:
+Node: <no authority available> 5029.14 Hz, Average Delay: 1871.74 Max Delay: 3001.98
+
+
+```
+
+##### view_frames.py
+
+可以生成TF的pdf，目前也有在线的实时查看工具。
+
+```
+ros2 run tf2_tools view_frames.py 
+[INFO] [1636558316.667894410] [view_frames]: Listening to tf data during 5 seconds...
+[INFO] [1636558321.702280144] [view_frames]: Generating graph in frames.pdf file...
+[INFO] [1636558321.709904442] [view_frames]: Result:tf2_msgs.srv.FrameGraph_Response(frame_yaml="C: \n  parent: 'B'\n  broadcaster: 'default_authority'\n  rate: 10000.000\n  most_recent_transform: 0.000000\n  oldest_transform: 0.000000\n  buffer_length: 0.000\nP: \n  parent: 'C'\n  broadcaster: 'default_authority'\n  rate: 10000.000\n  most_recent_transform: 0.000000\n  oldest_transform: 0.000000\n  buffer_length: 0.000\n")
+/opt/ros/foxy/lib/tf2_tools/view_frames.py:75: YAMLLoadWarning: calling yaml.load() without Loader=... is deprecated, as the default Loader is unsafe. Please read https://msg.pyyaml.org/load for full details.
+  data = yaml.load(result.frame_yaml)
+
+```
+
+![坐标树](1.TF2介绍/imgs/82291e00c53c4711b6328cc4b3a9ce90.png)
+
+
+
+
+
+除了使用命令行进行坐标关系的广播和监听，我们还可以使用代码来广播和监听，接下来小鱼就带你一起用程序来发布TF广播和获取坐标关系。
+
+
+