第十八章 添加你自己的机械臂
在第16，17 章，我们学会了如何给一个新的移动机器人增加 ROS 支持，内容涵盖建模，仿真，乃至自动导航。本章中，我们会仿照这种模式，把一个机械臂（或其他操纵器）带入 ROS。之前（第11章中）已经讲解了关于操纵器的一些基本知识，以及如何使用一个 ROS 中已有完整支持的机械臂。现在我们会更进一步，从零开始制作一个全新的机械臂，并为其配置用于路径规划的Moveit。
猎豹机械臂
我们将要搭建的是一个新的操纵器。回顾历史，最早的工业机械臂为 Unimation 公司在20世纪60年代所生产。这一公司由George Devol 和 Jospeh Engelberger 创办，早年主要向通用汽车提供机械臂，由于技术先进，销售范围逐渐扩大，并彻底的改变了整个制造业生产的面貌。1966年，Engelberger 带着他的机器人，在Tonight Show 节目上向公众展示了机械人倒啤酒，指挥乐队和打高尔夫球等功能，引起一阵轰动。图18-1 是Unimation 公司后期PUMA（可编程通用装配机械，Programmable Universal Machine for Assembly，PUMA）系列的机械臂之一。
Figure 18-1 Unimation 公司 PUMA 系列机械臂一例（图源：Wikimedia Commons）
为了表达对这些机器人所做贡献的尊敬，我们将新机械臂命名为“猎豹”。制作猎豹机械臂的步骤与小龟机器人差不多，大致如下：
- 确定 ROS 消息接口
- 编写机器人电机的驱动
- 编写机器人物理结构的模型
- 为 3 中编写的模型增加物理特性，用于在 Gazebo 中进行仿真
- 使用tf 对外发布坐标变换数据，并使用 rviz 进行可视化
- 增加传感器，注意要带有驱动和仿真的支持
- 添加一些标准的机器人算法，如路径规划等
ROS 消息接口
第16章中的移动机器人使用 cmd-vel/odom 对作为标准的 ROS 接口，这样就可以同时发送速度指令，更新里程计数据。对机械臂而言，类似功能的消息接口如下：
control_msgs/FollowJointTrajectory (follow-joint-trajectory 动作)
指定机械臂的运动轨迹，并监视其运动过程
sensor_msgs/JointState(joint-state 话题)
发布机械臂上每个连接点的当前状态
通过 follow-joint-trajectory/joint-state 这一对 ROS 接口，我们可以用一种具有较好可移植性的方法，来灵活的控制和监视机械臂。先来看看可以向follow-joint-trajectory 发送哪些控制信息：
看起来相当多！大致来说，我们需要将位置，速度，加速度，力，时间参数和公差组合成一条轨迹，从而控制机械臂，这未免过于麻烦。好在后面我们会讲到一种方法，可以在不用任何其他参数的情况下，构造出一条简单的轨迹。
再看看joint-state：
这个相对直观一些，只包括所有连接点当前位置，速度和力的信息。我们稍后再来处理他们。
硬件驱动
为了将 follow-joint-trajectory 接口应用在实体机器人上，我们需要编写一个与机器人硬件通信的节点。具体的实现细节取决于硬件本身和采用的通信方式。与移动机器人类似，机械臂也会对外提供一些物理接口，一般是串行或网络接口，还会有配套的数据通信协议。运气好的话，或许能找到一个实现好的通讯程序库，减少不必要的工作量。
虽然在这里我们不能提供通用的机械臂驱动代码，但是 ROS 中已有许多样例，可供你编写时参考。为方便起见，我们将假定已经写好了一个支持 follow-joint-trajectory/joint-state 接口的驱动，然后继续讲解 ROS 集成的剩余步骤。接下来的任务，就是对机器人进行建模，并将模型用于仿真。
对机器人建模：使用 URDF
现在开始编写猎豹机械臂的URDF模型文件。这一模型可以被 rviz 用来对机器人的状态进行可视化，也可以用作 Gazebo 仿真的参考，还可能会被 Moveit! 拿来做动作规划。
先从模型的运动学部分开始。看图18-1，可知机械臂的一些关键特性如下：
- 机械臂底座（base）被刚性连接在工作平面上
- 底座之上的第一个连接点连接的是“hip”和“torso”两个连接段，它们之间可以相对旋转
- 接下来的三个连接点分别为“shoulder”，“elbow”，“wrist”。机械臂的“upper arm”，“lower arm”和“hand”分别可以相对这三个连接点上下摆动。
总的来说，我们的机械臂模型需要5个连接段（base，torso，upper arm，lower arm，hand），彼此之间通过4个连接点相连（hip，shoulder，elbow，wrist）。为简便起见，模型中的连接段一律使用圆柱体。不过，使用更精确的模型可以提高仿真准确度和视觉真实性。
建模从底盘开始。由于底盘固定在工作平面上，我们要额外创建一个名为 world 的特殊连接段，并将圆柱形的底盘用一个 fixed 类型的连接点与之连接。具体的 URDF 代码见 Example18-1。
Example 18-1 向猎豹机械臂添加base
注意，我们在<visual>标签中使用了<origin>元素，将基座的原点从圆柱体正中心移到了底部中心。这样有利于确定下一个连接点的位置。后续的连接段都会进行类似的处理。将上述代码保存至文件 cougarbot.urdf，并使用 roslaunch urdf_tutorial display.launch 来进行可视化。
rivz 会自动打开，我们将看到一个圆柱体，如图18-2 所示。可以发现，坐标轴原点被转移到了圆柱体的底部中心。
Figure 18-2 猎豹机械臂 base 的可视化图形
下面添加 torso 连接段和hip连接点。代码见 Example 18-2：
Example 18-2 向猎豹机械臂添加torso 和 hip
torso 是一个细而长的圆柱体，通过 hip 连接在基座上。hip 则是一个 continuous 类型的连接点，torso 可以相对hip 绕 z 轴任意旋转。将上述代码加入 cougarbot.urdf，然后使用 rviz 检查机械臂可视化图形。注意使能 joint control。
滑动 joint control 窗口中的滑块，torso 就会绕 z 轴旋转，说明添加正确。接下来是 upper arm 和 lower arm。模型上可以使用比 torso 稍短的细长圆柱体。连接上，upper arm 通过 shoulder 连在 torso 的右侧（外侧），lower arm 通过 elbow 连在 upper arm 的左侧（内侧）。这几个组件的 URDF 代码见Example18-3 和 Example 18-4：
Example 18-3 向猎豹机械臂添加 upper arm 和 shoulder
Example 18-4 向猎豹机械臂添加 lower arm 和 elbow
把上述代码插入到 cougarbot.urdf 中。最后一个运动学元素是hand，hand 通过 wrist 连接在 lower arm 的末端。我们使用盒子来代表它，代码见 Example 18-5：
Example 18-5 向猎豹机械臂增加 hand 和 wrist
将最后一段代码插入cougarbot.urdf 后，就可以使用 roslaunch urdf_tutorial display.launch 启动 rviz 进行检查了。同样要注意使能joint control。效果如图 18-3 ，我们可以滑动hip, shoulder, elbow 和 wrist 对应的滑块来控制机械臂。
到目前为止，机械臂的整体结构已初见雏形。下面我们开始对其进行仿真。
Figure 18-3 猎豹机械臂 base, torse, upper arm, lower arm 和 hand 的可视化图形
在 Gazebo 中进行仿真
上一节中，我们创建了猎豹机械臂的运动学模型，包括其各组件的外形尺寸和相对位置。这些信息对可视化而言已经足够，但是如果要仿真的话，还得补充每个连接段的碰撞几何和惯性特性。
碰撞几何部分的代码可以直接由已有的外观模型代码复制得到。打开 cougarbot.urdf，将<visual>/<geometry>中的代码复制一份，并将标签替换为<collision>/<geometry>即可。以 base_link 为例，最后结果如 Example 18-6。注意要复制后删掉其中<material>标签的内容。
Example 18-6 带有碰撞信息的猎豹机器人 base
惯性特性需要测量每个连接段的质量和转动惯量。为简便起见，我们将每个连接段的质量设置为1kg，转动惯量则使用圆柱体和立方体对应的公式计算得到。最后结果分别见 Example 18-7（base），Example 18-8（torso），Example 18-9（lower arm和upper arm，它们的物理特性相同） 和 Example 18-10（hand）。把这些 XML 代码分别插入到 cougarbot.urdf 中对应的连接段中即可。
Example 18-7 猎豹机器人 base 的惯性特性
Example 18-8 猎豹机器人 torso 的惯性特性
Example 18-9 猎豹机器人 lower arm 和upper arm 的惯性特性
Example 18-10 猎豹机器人 hand 的惯性特性
请注意，对于任何在<visual>和<collision>中使用<origin>来对坐标原点做偏移的连接段，我们在其<inertial>中也使用<origin>进行了同样的偏移。因为猎豹机器人要求保证外观，运动学和动力学模型的一致性。不过对于其他机器人而言，有时候我们反而会希望这几个模型之间存在一些差异。
现在可以把所有代码打包成一个 ROS 包了。包名为 cougarbot。在工作区创建一个名为cougarbot 的文件夹，添加一个合适的 package.xml 文件（可以使用catkin-create-pkg 自动完成），然后把 cougarbot.urdf 移动到该目录下。最后是一个 launch 文件，用来自动打开Gazebo。launch 文件中的代码见 Example18-11：
Example 18-11 启动 Gazebo 仿真猎豹机械臂的 cougarbot.launch 文件
启动过程中，我们先将 URDF 模型加载进参数服务器的/robot_description 名字下，然后使用已有gazebo-ros 包中的帮助 launch 文件打开一个带空仿真环境的Gazebo。接下来使用同样来自 gazebo-ros 包中的 spawn-model 让 Gazebo 实例化一个猎豹机械臂，机械臂的模型则从参数服务器中读取。
将上述代码保存至 cougarbot/cougarbot.launch 中，然后启动：
Gazebo 将会自动打开，显示出我们的猎豹机械臂，如图 18-4。
Figure 18-4 仿真猎豹机械臂
机械臂是出现了，不过耷拉在地上的样子有点奇怪，怎么回事？原因在于我们只是让Gazebo 对机械臂进行仿真，而没有做出任何控制，即每个连接点都没有被施加力矩。因此在重力的作用下，机械臂自然就会像破玩偶一样耷拉下来。注意，即便是耷拉状态，也是完全符合机器人的运动学和动力学模型的。
回忆本书 289 页“在 Gazebo 中进行仿真”一节中的内容，通过使用差分驱动插件，我们可以经由 cmd_vel/odom 接口对小龟机器人进行差分运动控制。而对于猎豹机械臂而言，我们需要的是能够操作 follow-joint-trajectory/joint-state 接口的插件。针对这样的需求，我们要用到两个插件，分别是用于从follow-joint-trajectory接收轨迹并产生控制的 ros-control 和 对外发布 joint-state 数据的 ros-joint-state-publisher。
先看ros_control。添加这个插件的过程稍显繁琐。主要原因是，我们需要让用于仿真的控制代码能同时工作在硬件实体上。因此需要对控制器和其他基础组件进行很多额外的抽象和配置，这都会给插件的添加过程增加一定的复杂度。拿配置的复杂与代码的可复用性做交换，也是一种权衡。
下面逐步讲解添加 ros_control 的方法。首先，对于URDF 模型中的每个连接点，我们都需要定义相应的 transmission。对一个连接点而言，假设其上安装了一个电机（驱动该连接点对应连接段之间的相对运动），则 transmission 描述了该连接点的运动与电机转动输出之间的关系。transmission 通常会包括一个减速比，代表了电机齿轮箱降低速度，增大扭矩的作用。除此之外，transmission 还可能包含其他更复杂的内容，如连接点之间的机械耦合等。Example 18-12 的代码中为猎豹机械臂的 hip 连接点定义了一个简单的trsnmission。关于 transmission 的更多细节，请参阅 URDF 和 ros-control 的相关文档。
上述代码实际上定义了一个减速比为1 的空 transmission。这是不现实的，不过对于仿真猎豹机器人来说足够了。将代码插入URDF 模型中，然后给其他三个连接点定义类似的transmission，具体代码见 Example 18-13。
Example 18-13 向shoulder ,elbow 和wrist 添加transmission。
现在可以加载 ros-control 插件了，代码见 Example18-14
Example 18-14 加载 ros-control 插件
将上述代码插入 cougarbot.urdf。接下来要从 ros_control 提供的控制器中选择一个进行配置。考虑到实际需求，控制器要能够接受由连接点位置组成的轨迹（而不是由速度，加速度或其他约束组成的）。在 cougarbot 包中创建一个名为 controllers.yaml 的新文件，并添加Example 18-15 所示的代码：
Example 18-15 猎豹机器人控制器的 YAML 配置文件
该文件定义了一个名为 arm_controller，类型为 position-controllers/JointTrajectoryController的新控制器，用来控制猎豹机械臂的连接点。下面的XML 则会把控制器的配置文件通过rosparam加载到参数服务器中，以便其他工具访问。
把上述代码添加到 cougarbot.launch 中。由于 ros_control 启动时默认不运行任何控制器，因此我们需要使用 controller-manager/spawner 工具来实例化我们新创建的arm-controller。相应的XML代码如下：
同样把上述代码插入 cougarbot.launch，然后启动仿真。这次机械臂就不会耷拉在地上了，Gazebo中的效果大致如图 18-5 所示：
Figure 18-5 仿真带控制器的猎豹机械臂
控制器默认会将每个连接点保持在0位置，因此机械臂才不会耷拉在地上。现在我们可以向 follow-joint-tragjectory 接口发送指令，来控制机械臂向某个方向运动。具体该怎么做呢？先看看当前生效的 topic:
可以看到，/arm_controller 名字空间下包含了许多有意思的话题。进一步看，该空间下 follow-joint-trajectory 这一次级名字空间所包含的话题直接组成了一个常用于控制器的动作接口。不过，鉴于/arm-controller 下还提供了command 这个话题，可以直接向其发送控制指令。让我们看看 command 话题的细节：
注意 command 话题的消息类型：trajectory_msgs/JointTrajectory。在本书321页“ROS 消息接口”一节，我们查看follow-joint-trajectory 动作能接受的控制目标类型时曾见过它。因此，只要构造并发布该类型的消息即可控制机械臂运动。要构造该消息，我们要提供的信息最少也需包括待控制的节点名字列表，以及一条至少由一个点组成的轨迹。对轨迹上的每个点，还要包含到达该点时的时间（从执行运动轨迹的时刻开始计时）和所有连接点的位置。这些数据还不算太多，我们可以用 rostopic 发布。下面的命令让每个连接点移动到一个新的角度，并要求所有的移动在1秒内完成。
你将会看到，机械臂平滑的移动到了一个新的位置，如图18-6 所示。这样的控制方法有点像动画师绘制动画角色的关键帧：我们仅仅指定机械臂的最终目标位置，而把中间的路径规划交给控制器去完成。
Figure 18-6 猎豹机械臂移动到了一个新的位置
你可以试着修改 rostopic 发出的控制指令，让机械臂移向其他位置。还可以给轨迹增加更多的点，使之延长。一开始可能还会比较有趣，不过很快就会令人乏味了-- 毕竟，使用命令行控制机械臂并不是一个好办法。我们还需要对猎豹机械臂进行改善，直至它可以自行完成规划路径的任务。
检查坐标变换信息
在前一节中，我们使用ros-control 插件，对外提供了follow-joint-tragjectory 接口，进而实现对机械臂的控制。而对于 joint-state 接口，我们则需要用到 ros-joint-state-publisher 这个插件。该插件会提供 joint-state 接口，并向这个接口发布机械臂的当前状态。
添加 ros-joint-state-publisher 插件要相对简单一点，只要告诉它该汇报哪些连接点的状态就行了。在猎豹机械臂上，就是 hip，shoulder，elbow和wrist这四个连接点。具体代码见Example 18-16。将其添加到 cougarbot.urdf 中，注意放在<robot>标签之间。
Example 18-16 发布连接点状态数据
现在可以启动 cougarbot.urdf，然后用 rostopic 检查一下 joint_state 话题下的数据，看看插件是否已经正确添加。
如果插件工作正常的话，就能看到反映各个连接点位置信息的数据流：
在控制器的作用下，连接点的位置会非常接近0，同时还会不停地在 0 附近波动，就像真实的机械臂在不断对抗重力，保持自己的位置不变一样。
接下来需要添加的是 robot-state-pubisher，它会拿 joint-state 的消息和机器人模型做前向运动学解算，产生 tf 消息，并发布出去。下面的代码用来启动robot-state-publisher，将其插入 cougarbot.launch 即可：
重新启动 cougarbot.launch，打开 rviz ，选择 base_link 为“fixed frame”，并在 Display 栏添加 RobotModel 和 tf。这样就能看到带TF 帧的机器人了，效果如图18-7所示：
Figure 18-7 在 rviz 中查看处于仿真中的猎豹机械臂
还可以用 rqt_plot 将 joint-state 的数据绘制出来（关于绘制数据，参见本书392页“绘制数据：使用rqt-plot”一节）：
你将会看到一张由四个连接点位置组成的实时数据图，而且每个连接点的位置都很接近0。再次向 command 话题发送我们之前的简单轨迹，让机械臂动起来：
机械臂顺利的移动到了新位置，效果如图 18-8。
与此同时， rqt_plot 窗口中的位置数据也从0变化为各自的新角度，如图 18-9所示：
Figure 18-8 在 rviz 中查看处于仿真中的猎豹机械臂
Figure 18-9 猎豹机械臂沿轨迹运动过程中，各个连接点的位置数据
配置 MoveIt!
MoveIt! 是一个用来做动作规划和控制的工具库。虽然在设计理念上与导航程序栈（我们在本书308页“配置导航程序栈”一节使用过它）很像，但是前者要更加复杂，拥有更丰富的配置选项。为了方便我们进行配置，MoveIt! 提供了一个叫做 Setup Assistant 的图形化工具。让我们来打开它：
正常情况下会弹出一个如图18-10所示的引导窗口：
单击“Create New MoveIt Configuration Package”，在文件选择栏找到 cougarbot.urdf，然后单击“Load Files”。现在你应该就能在 Setup Assistant 窗口的右侧看到猎豹机械臂的模型了，效果如图 18-11：
Figure 18-10 MoveIt! 的Setup Assistant
Figure 18-11 猎豹机械臂模型成功加载进 Setup Assistant
接下来就可以逐一对Setup Assistant 窗口左侧的配置项进行配置了：
Self-Collision
在这部分配置中，单击“Regenerate Default Collision Matrix”按钮。MoveIt! 会自动对机器人模型进行检查，并随机选取碰撞检测的策略，直至得到最优解。这样做的意义在于减少不必要的碰撞检测，从而提高MoveIt规划的效率。
Visual Joints
不用做任何修改
Planning Groups
我们需要创建一个包含整个机械臂的planning group。单击“Add Group”，在"Group Name"一项中填入“arm”（名字可以任取），在“Kinematic Solver”中选择“kdl-kinematics-plugins/KDLKinemativsPlugin”（这个插件提供了通用的反向运动学解算功能，虽然效率不是最高的，但是够用了）然后单击"Add Joints"，选中所有的5个连接点，最后单击 “Save”。最终结果如图18-12 所示：
Figure 18-12 在 Setup Assistant 中配置 Planning Groups
Robot Pose
不用做任何修改
End Effectors
我们需要向 MoveIt! 指定整个机械臂末端的执行器，对于猎豹机械臂而言应该是 hand 连接段。在"End Effector Name"中填入“hand”(名字可以任取)，在“Parent Link”中选择“hand”，然后单击"Save"。
Passive Joints
不用做任何修改
Configuration Files
我们需要告诉 MoveIt! 该在哪个路径下创建 一个包含新配置文件的 ROS 包。在“Configuration Package Save Path”中，填入 courgarbot 目录下尚不存在的 cougarbot-moveit-config 文件夹（稍后会被自动创建），然后单击“Generate Package”。
完成上述各项的配置后，单击“Exit Setup Assistant”退出配置工具。经过上面对MoveIt! 的配置，我们得到了一个名为 cougarbot-moveit-config 的 ROS 包，其中包含许多laucnch 文件和 YAML 文件。囿于篇幅，这里就不详细说明每个文件的作用了。你可以通过查阅 MoveIt! 的文档来深入了解它们。
最后一项配置是控制器信息，我们需要告诉 MoveIt! 机械臂控制器的配置情况。在 cougarbot-moveit-config 包中，创建一个名为 config/controller.yaml 的文件，并插入Example 18-17 中的代码：
Example 18-17 用猎豹机器人的 arm controller 配置MoveIt!
在这个配置文件中，我们将 ros_control 插件提供的 follow-joint-trajectory 动作服务器配置为MoveIt! 的控制接口，同时将MoveIt!的控制对象设置为 hip，shoudler，elbow，wrist 这四个连接点。除此之外，还要修改cougarbot-moveit-config 中的另一个文件：打开launch/cougarbot-moveit-controller-manager.launch.xml（初始为空），插入Example 18-18 中的 XML 代码：
Example 18-18 加载 MoveIt! 控制器配置的代码
这个文件设置了许多参数，其中就包括将我们刚才创建的 controllers.yaml加载到参数服务器中。
到此为止，MoveIt 的配置全部完成。接下来将介绍如何使用它。
使用 rviz 控制机械臂
启动对猎豹机械臂的仿真：
与此同时，使用我们刚才创建的配置文件启动MoveIt!：
现在猎豹机器人处于仿真状态，MoveIt也准备好接收目标位姿了。接下来使用MoveIt 提供的配置文件打开rviz：
以上三步可以合在一个launch 文件里。创建名为all.launch 的文件，插入Example18-19 中的代码：
Example 18-19 使用一个 launch 文件完成三步操作
rivz 打开后，你将会看到由 MotionPlanning 视图提供的一些 rviz 功能组件，如图18-13 所示：
Figure 18-13 MotionPlanning 视图下猎豹机械臂的可视化图形
在这个视图中可以做许多事，不过在这里我们只会涉及最基本的运动规划和执行操作。首先在 Motion Planning->Context 窗口中，选择"Allow Approximate IK Solutions"。这是由于猎豹机械臂的关节是单自由度的，因而很难在 rviz 中以交互式的方法指定精确的目标位姿。也正是因为这个原因，机械臂的关节一般会有2或3个自由度。
然后单击Motion Planning->Planning，现在可以试试机械臂的规划效果了。在 rviz 中，机械臂末端上的彩色标记表示允许以任意方式拖拽。当你完成一次拖拽后，反向运动学（Inverse Kinematics，IK）解算器就会尝试在你拖拽到的地方找到一个合适的机械臂位姿。这个位姿会被rivz 可视化，以供预览，效果见图 Figure 18-14：
Figure 18-14 使用 rviz 指定目标位姿
选中目标位姿后，单击“Plan”，你就能看到一条由出发点到目标的轨迹，这条轨迹会被 rviz 不断重放。由于只是在可视化的缘故，Gazebo 中还暂时没有动静。接下来单击 “Execute”，这条轨迹就会被执行，我们也就能在 Gazebo 中看到机械臂循着 rviz 中的轨迹动起来了。
试试把机械臂拖拽到其他位置，然后重复上述步骤。如果你不是很理解机械臂的运动过程，请务必同时在机械臂末端进行旋转和拖动，来产生一些稍复杂的位姿并执行，这有助于你理解机械臂是如何通过各个连接点的旋转组合出整体的运动的。当重复然你也可以试试随机位姿：在"Select Goal State"下，选择“<random valid>”，然后单击“Update”，这样就得到了一个随机的目标位姿。单击 “Plan and Execute”，机器人就会移动到那里。
小结
本章中，我们讲解了如何从零开始对一个机械臂建模，建模内容包括用于可视化和仿真的必要信息。进一步地，我们还为机械臂添加了一个控制器，并且基于末端目标位姿，使用MoveIt!实现了轨迹的规划和执行。这些功能都是通过编写 XML 和 YAML 配置文件实现的，无需编写任何代码。充分展现了将 Gazebo，rviz，MoveIt!和其他 ROS 工具组合成一个机器人系统的整个过程。
当然，我们的猎豹机械臂还远没有完成。首先，我们没有添加任何传感器。现在看来路径规划还不错，但是在存在障碍物的情况下，使用传感器来避障就显得十分重要。MoveIt! 支持障碍物感知下的路径规划，我们只需要向猎豹机械臂模型中添加一个传感器（比如类似 Kinect 的深度相机），然后在 MoveIt! 的配置中订阅该传感器的数据即可。具体细节可以参见 MoveIt! 的文档。
经过这几章的学习，我们已经掌握了在 ROS 中对一个新机器人进行建模和控制的方法。还有一种同样重要的 ROS 集成操作会在下一章中加以探讨：向 ROS 中添加软件程序库。