ROS 系统 MoveIt 玩转双臂机器人系列（一）--ROS 机器人建模

注：本篇博文全部源码下载地址为：Git Repo。

1. 下载到本地后解压到当前文件夹然后运行：catkin_make 编译。

2. 源码是在 Ubuntu14.04 + Indigo 环境下编写。

一、ROS 系统的 MoveIt 模块简介

机器人操作系统 ROS 目前最受关注的两个模块是导航（Navigation）和机械臂控制（MoveIt!），其中，机械臂控制模块（后面简称 MoveIt）可以让用户快速建立机械臂模型并实现机械臂的控制（包括建模、运动学求解、运动规划、避障等），后续我将分几篇博客分别介绍如何一步步使用 MoveIt 控制自己的机械臂，算是对以前的学习内容的记录和分享。

关于 MoveIt 最全面的讲解可以参考 MoveIt 官方网站，推荐大家多参考官方文档和例程，这里的博文系列权当简介和入门。

如果用几个特点来概括 MoveIt，主要有：

它是属于 ROS 系统的一部分，用于控制多关节机械臂；提供了一系列成熟的插件和工具，可以实现机械臂控制的快速配置；封装了大量 API，方便用户在 MoveIt 模块上进行二次开发，进而做出更多有意思的应用。

这里借用官方经典图例做简单说明，如下如：

                                                                                MoveIt结构图 

上图可以看出：

1.MoveIt 的核心节点（node）为 move_group，外围的几个部分分别为：ROS Param Server， Robot  Controllers， Robot 3D Sensors， User Interface， Robot Sensors，其中：

ROS Param Server：这部分载入的是用户定义的模型文件（xacro 或 urdf）和一些配置文件。（重要）Robot  Controllers：  这部分可以看做是和真正的机器人部分（硬件控制接口）打交道的部分，即运动规划的数据由此发给机器人驱动部分，后续会详细讲解。（重要）Robot 3D Sensors：  这部分作用是载入 RGB-D 相机或激光雷达等获得的点云数据用于机械手的抓取或避障等。User Interface：         这部分是用户接口，MoveIt 提供一系列的 API 供用户完成自定义的功能，这里主要。（重要）Robot Sensors：        这部分是接收机械臂的传感器数据，然后预估出机器人的状态并发布。

二、MoveIt 的安装（Ubuntu14.04，Indigo）

首先确保你的 Linux 上正确安装了 ROS 系统，安装步骤参考：http://wiki.ros.org/indigo/Installation/Ubuntu。然后安装 moveit，如果不想挑战源码编译，MoveIt 可以用 apt-get 快速安装，只需要输入：

$ sudo apt-get install ros-indigo-moveit-full

三、本系列博文所用双臂机器人简介

本系列博文将以一个双臂机器人为例，详细讲解基于 MoveIt 的使用方法，我为这个双臂机器人取名为：rob，并在 Solidworks 中简单绘制了该机器人的三维模型，结构示意图如下：

Rob 结构示意图

Rob 包含两个手臂，左右对称布局，每个手臂包含 5 个自由度，关节情况如下图所示。

关节分布情况示意图

这种关节分布是仿人形的双臂机器人常用的手臂设置形式，但这种结构并不能完全覆盖人手臂所有的自由度（人的手臂包含 7 个自由度），但用于说明 MoveIt 的使用已经足够了。此外，各个关节的自由度范围如下表所示。

四、机器人的 ROS 模型建立

这个系列主要介绍机器人 ROS 模型的建立方法，ROS 系统带来的好处之一就是：我们无需自己建立复杂的数学模型来描述自己的机器人几何尺寸、运动学和动力学等，只需要用它提供的模型描述方法即可实现快速建模。

ROS 系统的模型描述方法主要有两种格式: URDF 和 XACRO。

URDF：用于描述一些简单的机器人模型，参考官网：URDF 官方介绍（初学者一定要精读这些文档）。XACRO： 应用更普遍的机器人模型描述文件，参考官网：xacro，它有如下几个特点：

1. 语法简练。采用编程话的脚本语言格式，可以定义变量、常量、引入数学表达式等，极易上手。

2. 方便复用。它可以进行一系列的宏定义，并且可以包含其他.xacro 文件。

下面，开始用 xacro 文件建立 Rob 的机器人模型，完整的源代码可以在：git 上获取（git 建模源码），下文会截取一部分代码做简要说明。

第一部分是文件的头和一些宏定义，robot name 我们可以自己随便定义，然后分别给出了几组颜色和常数的定义，最后给出了关节传动部分的宏定义。

<?xml version="1.0"?><robot name="rob_robot" xmlns:xacro="http://ros.org/wiki/xacro">

<material name="White">    <color rgba="1.0 1.0 1.0 1.0"/></material>

常量的定义方法如下，在引用这里定义的常量的时候，用$XX，例如在如果在下面的代码中想引用M_{P}I，只需要用：${M_PI} 即可。

关节的传动参数宏定义方法如下，xacro:macro name 定义了本宏的名称，这个名称由用户定义，后续引用该宏的时候就是根据名字来的，具体如何操作参考下文介绍或 git 上的源代码，注意，这里宏定义内部的一些具体数值仅为说明而存在，具体的机器人关节应该是不一样的。

2. base_link 的定义。

base_link 是所有的其他关节的基础，也就是基坐标系所在的 link，这里他的几何图形我们直接引用他的 dae 文件，至于如何用 Solidworks 绘制模型然后制作成 dae 文件，后续找机会专门写一篇博客进行介绍，下面的 gazebo 句段是为了我们的模型在 gazebo 环境中仿真用的，注意，base_link 只是定义了“机械臂的基座” 它本身是不包含 joint 的。

  <collision>        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> <!--  -->        <geometry>              <mesh filename="package://rob_description/meshes/body_link_humanoid.dae"/>        </geometry></collision>

</link>

<gazebo reference="base_link"><material>Gazebo/green</material></gazebo>

3.  其他关节的定义。

至于其他关节的定义，一个 Link 就对应一个 Joint ，示例如下，这里定义的是 l_shoulder_joint  和 l_shoulder_link。在 Joint 的定义中，有一个 limit 的程序段，这里规定的是关节的力限制、速度限制和关节运动范围限制，速度限制的单位是 m/s（移动关节）或 rad/s（转动关节），详细的说面看：这里。

代码中间: <xacro: ...../> 就是引用上文中我们定义的关于传动参数的宏定义。

如果在 xacro 代码中想用数学表达式，使用的格式是：${ 数学表达式 } ，如下面代码中所示。

其余关节的定义详见源代码。

<xacro:transmission_block joint_name="l_shoulder_joint"/>

<link name="l_shoulder_link"><visual><origin xyz="-0.04 0.02 0.04" rpy="1.5708 1.5708 0" /><geometry><mesh filename="package://rob_description/meshes/l_shoulder_link.dae"/></geometry><material name="green" /></visual>

  <collision>        <origin xyz="-0.04 0.02 0.04" rpy="1.5708 1.5708 0" />        <geometry>            <mesh filename="package://rob_description/meshes/l_shoulder_link.dae"/>        </geometry>    </collision>><xacro:inertial_matrix mass="1"/>

</link>

<gazebo reference="l_shoulder_link"><material>Gazebo/green</material></gazebo>

4. 将 XACRO 文件转换成 URDF 文件 并检查。

转换的方法很简单，ROS 封装了实现方法，我们只需进入 xacro 所在的文件夹，然后键入如下命令即可：

rosrun xacro xacro.py rob.xacro > rob.urdf

为了检验我们的模型的准确性，我们进行简单的检查，在同一个目录下输入：

check_urdf rob.urdf

即可得到如下图显示，可以看到双臂的关节链接情况。

为了更直观的观看关节链接情况，我们在同一个目录下输入下面这行命令，就会得到 rob_robot.gv 和 file rob_robot.pdf  两个文件，打开后者如下图所示。

urdf_to_graphiz rob.urdf

5. 在 RViz 中观看模型。

编写 launch 文件，取名： description.launch ，内容如下：

<launch><arg name="model" />

<!-- Parsing xacro and setting robot_description parameter --><param name="robot_description" command="$(find xacro)/xacro.py $(find rob_description)/urdf/rob.xacro"/>
<!-- Setting gui parameter to true for display joint slider --><param name="use_gui" value="true"/>
<!-- Starting Joint state publisher node which will publish the joint values --><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<!-- Starting robot state publish which will publish tf --><node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<!-- Launch visualization in rviz --><node name="rviz" pkg="rviz" type="rviz" args="-d $(find rob_description)/urdf/urdf.rviz" required="true" />

</launch>

输入命令：

roslaunch rob_description description.launch

得到：

转动每个关节，检查无误后，模型建立完成，下一篇经介绍如何使用 MoveIt 控制 Rob 机器人。

6. Tips 建模常见错误：

问题 1：如果在查看模型时发现错误提示：No transform from [xxxx] to [base_link]

解决办法：出现这个错误首先要怀疑是你的 xacro 描述文件编写的格式出现了错误，如头部多了空格、中间关键字拼写错误等，建议耐下心来逐行逐句检查语法。

问题 2： 源码中和 xacro 文件同一目录的 urdf.rviz 文件是什么作用？

答：所有的  .rviz 文件都是 Rviz 的配置文件，这里我们在 launch 文件中制定了他的配置文件，如果不指定 Rviz 启动时会读取默认的配置文件，用户可以根据需求启动 Rviz 后在左上角工具栏上保存自己满意的配置文件。