完成numpy表示位姿和坐标变换

docs/chapt7/7.2.1空间坐标描述.md

@@ -316,17 +316,35 @@ $$
 
 ![右手坐标系](7.2.1空间坐标描述/imgs/image-20211104162846579.png)
 
-7.练习
+## 7.练习
 
 光说不练假把式，我们来做个手眼转换题
 
 ![手眼系统的坐标系关系](7.2.1空间坐标描述/imgs/image-20211104102656102.png)
 
-如图🔓示，已知：
+#### 如图🔓示，已知：
 
-1.相机坐标系{C}为参考坐标系，工具坐标系{P}在相机坐标系{C}`x,y,z`各轴投影为$2,1,1$
+1.相机坐标系{C}为参考坐标系，工具坐标系{P}的位置矢量在相机坐标系{C}`x,y,z`各轴投影为$2,1,2$，并且工具坐标系和相机坐标系姿态相同。
 
+2.机器人基坐标系{B}为参考坐标系，相机坐标系{C}在的位置矢量在{B}各轴的投影为$0,0,3$,坐标系{C}和绕着坐标系{B}的x轴转了180度
 
+可以参考下图看题目
+
+![坐标系关系图](7.2.1空间坐标描述/imgs/image-20211108213832470.png)
+
+#### 求：
+
+**{B}为参考坐标系，坐标系{P}的位置矢量和旋转矩阵**
+
+**答案：**
+
+位置矢量：$[2,-1,1]^T$
+
+旋转矩阵：$\begin{bmatrix} 
+1 & 0 & 0\\
+0 & -1 & 0\\
+0 &0 & -1\\
+\end{bmatrix} \tag{小鱼提示：重要方程1}$
 
 **参考文档**
 

docs/chapt7/7.2.2动手学空间姿态描述.md

@@ -9,13 +9,13 @@
 - 掌握tf2相关CLI工具进行坐标变换
 - 掌握python操作TF进行坐标变换
 
-1.numpy表示位姿
+## 1.numpy表示位姿
 
 在前几节中，小鱼带你安装使用了ROS2和MiniConda，并学习使用Numpy进行矩阵相关运算的验证。
 
 那我们如何使用numpy中的矩阵表示位置矢量和旋转矩阵呢？
 
-1.1 位置表示
+### 1.1 位置表示
 
 上一节中我们使用一个3行1列的矩阵表示了位置，那么我们在numpy中自然也可以使用这样一个矩阵表示。
 
@@ -33,7 +33,7 @@ import numpy as np
 
 ![image-20211105102418250](7.2.2动手学空间姿态描述/imgs/image-20211105102418250.png)
 
-1.1.1 位置矩阵
+#### 1.1.1 位置矩阵
 
 使用3*1的矩阵表示位置，我们新建一个沿着x、y、z各平移1个单位的位置矢量。
 
@@ -43,7 +43,7 @@ np.asarray([1.0,1.0,1.0]).reshape(3,1)
 
 ![image-20211105102746824](7.2.2动手学空间姿态描述/imgs/image-20211105102746824.png)
 
-1.2 姿态表示
+### 1.2 姿态表示
 
 姿态可以使用`3*3`的旋转矩阵表示,`3*3`的单位矩阵代表没有姿态变换，注意没有姿态变换不是零矩阵，而是单位矩阵。
 
@@ -64,16 +64,119 @@ print(R_AB)
 
 ![image-20211105110244694](7.2.2动手学空间姿态描述/imgs/image-20211105110244694.png)
 
-2.numpy坐标变换
+## 2.numpy坐标变换
 
-掌握了使用numpy表示位置和姿态后，接着我们来使用numpy来完成上一节的两个小练习，来动手体验一下：
+掌握了使用numpy表示位置和姿态后，接着我们来使用numpy来完成上一节的小练习
 
+### 2.1 题目
 
+#### 如图🔓示，已知：
 
-3.tf2介绍
-4.tf2相关工具进行坐标变换
+![手眼系统的坐标系关系](7.2.2动手学空间姿态描述/imgs/image-20211104102656102.png)
 
-5.python操作tf2坐标变换
+1.相机坐标系{C}为参考坐标系，工具坐标系{P}的位置矢量在相机坐标系{C}`x,y,z`各轴投影为$2,1,2$，并且工具坐标系和相机坐标系姿态相同。
+
+2.机器人基坐标系{B}为参考坐标系，相机坐标系{C}在的位置矢量在{B}各轴的投影为$0,0,3$,坐标系{C}和绕着坐标系{B}的x轴转了180度
+
+可以参考下图看题目
+
+![坐标系关系图](7.2.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)
+```
+
+![完整程序及结果](7.2.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)
+```
+
+运行下，可以得到结果
+
+![完整结果](7.2.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}$
+
+
+
+
+
+## 3.tf2介绍
+
+
+
+
+
+
+
+## 4.tf2相关工具进行坐标变换
+
+
+
+
+
+
+
+## 5.python操作tf2坐标变换