从头手搓一台ros2复合机器人（带机械臂）

一.前言

        上一小节我们学习了坐标描述的理论知识，但是光说不练假把式，这次我们就来实际动手体验一下坐标之间的转换，并且上节有一个课后练习题我们使用理论数学方法手算的，这节课我们可以学会如何使用程序代码计算。话不多说，马上开始！

二.ros2学习

        前五小节内容可以订阅往期博客。

6.空间坐标描述实战

6.1 numpy表示位姿

使用numpy中的矩阵表示位置矢量和旋转矩阵

6.1.1 位置表示

我们在numpy中可以使用这样一个矩阵表示

使用3*1的矩阵表示位置，我们新建一个沿着x、y、z各平移1个单位的位置矢量。

6.1.2 姿态表示

姿态可以使用3*3的旋转矩阵表示,3*3的单位矩阵代表没有姿态变换，注意没有姿态变换不是零矩阵，而是单位矩阵。

我们新建一个旋转矩阵，用该旋转矩阵表示绕着z轴旋转45度，可以这样写：

注意：角度需要用弧度制表示

6.2 numpy坐标变换

使用numpy表示位置和姿态后，接着我们使用numpy来完成上一节的小练习

求：

{B}为参考坐标系，坐标系{P}的位置矢量和旋转矩阵

6.2.1 旋转矩阵求解

需要使用复合坐标变换了，根据坐标变换规则有：

因为{C}绕着{B}的x旋转了180度，根据上节的重要公式可知

所以对应的程序可以这样写

6.2.2 平移矩阵求解

根据复合变换规则可知：

已知：

所以可以写这样写程序：