[update]:完成动手学坐标变换

docs/chapt7/7.3.2动手学坐标变换.md

@@ -4,12 +4,13 @@
 
 本节课我们就通过对应的函数和库实现齐次矩阵的生成，齐次矩阵的乘法和求逆。
 
-1.齐次矩阵的合成
-齐次矩阵的的生成可以一个姿态和一个平移向量组成，因为姿态可以用四元数、欧拉角、轴角、旋转矩阵四种方式来表示。
+## 1.齐次矩阵的合成与分解
 
-所以我们考虑先将对应的姿态转成旋转矩阵，然后使用numpy讲旋转矩阵和平移向量填写到齐次矩阵对应的位置即可。
+齐次矩阵的的生成可以一个姿态和一个平移向量组成，因为姿态可以用四元数、欧拉角、轴角、旋转矩阵四种方式来表示
 
-1.1旋转矩阵+平移向量
+所以我们考虑先将对应的姿态转成旋转矩阵，然后使用numpy讲旋转矩阵和平移向量填写到齐次矩阵对应的位置即可
+
+### 1.1旋转矩阵+平移向量
 
 ```
 #导入库
@@ -26,7 +27,7 @@ R,T
 
 ![image-20220104124708934](7.3.2动手学坐标变换/imgs/image-20220104124708934.png)
 
-1.1.1 使用numpy方法合成齐次变换矩阵
+#### 1.1.1 使用numpy方法合成齐次变换矩阵
 
 ```
 temp = np.hstack((R,T.reshape(3,1)))
@@ -35,7 +36,7 @@ np.vstack((temp,[0,0,0,1]))
 
 ![image-20220104124655166](7.3.2动手学坐标变换/imgs/image-20220104124655166.png)
 
-1.1.2 使用tfs中的函数合成齐次变换矩阵
+#### 1.1.2 使用tfs中的函数合成齐次变换矩阵
 
 ```
 tfs.affines.compose(T,R,[1,1,1])
@@ -43,7 +44,7 @@ tfs.affines.compose(T,R,[1,1,1])
 
 ![image-20220104124645988](7.3.2动手学坐标变换/imgs/image-20220104124645988.png)
 
-1.2四元数+平移向量
+### 1.2四元数+平移向量
 
 思路:先将四元数转换成旋转矩阵，然后再利用1.1合成齐次矩阵
 
@@ -54,10 +55,47 @@ tfs.affines.compose(T,R,[1,1,1])
 
 ![image-20220104124632233](7.3.2动手学坐标变换/imgs/image-20220104124632233.png)
 
-2.齐次矩阵的乘法
+### 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](7.3.2动手学坐标变换/imgs/image-20220108161652509.png)
+
+
+
+### 2.3 将qcjz分解为四元数和平移向量
+
+```
+tfs.quaternions.mat2quat(T[0:3,0:3]),T[:3,3:4]
+```
+
+![image-20220108161712715](7.3.2动手学坐标变换/imgs/image-20220108161712715.png)
+
+
+
+## 3.齐次矩阵的乘法
+
 对应numpy中矩阵的乘法`np.dot`讲两个矩阵相乘即可，我们以一道例题来讲解这个问题。
 
-2.1 练习-眼在手外
+### 3.1 练习-眼在手外
 
 如图🔓示，已知：
 
@@ -88,17 +126,109 @@ $$
 
 动手写代码：
 
+**先求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](7.3.2动手学坐标变换/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](7.3.2动手学坐标变换/imgs/image-20220108162328993.png)
+
+**求T_BP**
+
+```
+T_BP = np.dot(T_BC,T_CP)
+T_BP
+```
+
+![image-20220108162303105](7.3.2动手学坐标变换/imgs/image-20220108162303105.png)
+
+**分解成欧拉角对比结果**
+
+```
+tfs.euler.mat2euler(T_BP[0:3,0:3]),T_BP[:3,3:4]
+```
+
+![image-20220108162339874](7.3.2动手学坐标变换/imgs/image-20220108162339874.png)
+
+到这里我们就利用做了齐次矩阵的乘法完成了坐标的变换
+
+## 3.齐次矩阵求逆
+
+### 3.1练习-眼在手上
+
+![image-20220108163802318](7.3.2动手学坐标变换/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](7.3.2动手学坐标变换/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]
 
-3.齐次矩阵求逆
-3.1练习-眼在手上
+求关节1和关节3之间的关系？
 
 
-4.练习
-4.1 map坐标系转换
-4.2 机械臂运动学正解
 
 --------------