运动规划:Moveit输出规划轨迹到Gazebo

对于XXX功能怎么实现，XXX错误怎么解决的问题。表面上看，解决这些问题的方法就是提供正确的代码，正确的编译方法，正确的运行步骤。 然而，这种解决方法只能解决这个特定的问题，而且解决之后我们也无法学到一些实际的东西。要想彻底明白，需要从源头入手，也就是说，不要问“MoveIt! 怎么把机械手从空间一个点移到另一个点？“，**而是要问"MoveIt! 为什么能把机械手从空间一个点移到另一个点？"。** 这一点明白之后，遇到类似的问题，才能从容应对。同理，这不仅适用于MoveIt，也同样适用于其他任何ROS功能。
规划问题是一个很重要，而且也很有挑战性的问题。好在ROS里提供了一些规划的接口，比如主要用于机械臂规划的MoveIt，其实它也可以用于规划底座.

1 基本概念

首先，我们要了解一些基础的概念，了解各个名词的意义和区别。

1.1. 运动规划 (Motion Planning)

我们这里讲的运动规划，有别于轨迹规划 (Path Planning)。一般来说，path planning用于无人车/无人机领域，而motion planning主要用于机械臂，类人机器人领域。

确定3D空间的一个姿势(pose)需要6个变量，而对于关节数大于6的机械臂结构，它的规划空间维度就大于6，成为冗余系统(redundant system)，从而使规划问题变得更为复杂。所谓冗余系统，就是说，存在多种关节角度配置能够使得终端达到相同的位姿，存在无数的解。这是达到的最终姿势有无数个解，那么如何到达这个最终姿势，整个运动的轨迹，更是存在无数个解。

1.2. 开源运动规划库 (OMPL).

接上文，而OMPL (Open Motion Planning Library)， 开源运动规划库，就是一个运动规划的C++库，其包含了很多运动规划领域的前沿算法。

OMPL能做什么？ 简单说，就是提供一个运动轨迹。给定一个机器人结构(假设有N个关节)，给定一个目标(比如终端移到xyz)，给定一个环境，那么OMPL会提供给你一个轨迹，包含M个数组，每一个数组长度是N，也就是一个完整的关节位置。沿着这个轨迹依次移动关节，就可以最终把终端移到xyz，当然，这个轨迹应当不与环境中的任何障碍发生碰撞。

为什么用OMPL？ 运动规划的软件库和算法有很多，而OMPL由于其模块化的设计和稳定的更新，成为最流行的规划软件库之一。很多新算法都在OMPL开发。很多其他软件（包括ROS/MoveIt）都使用OMPL做运动规划。

1.3. 逆运动学 (Inverse Kinematics)

什么是逆运动学(IK)？简单说，就是把终端位姿变成关节角度，q=IK(p)。p是终端位姿(xyz)，q是关节角度。

为什么要用IK？OMPL是采样算法，也就是要在关节空间采样。 这与无人车的规划有一个最明显的区别，无人车的目标就是在采样空间， e.g. 目标是(x,y), 采样空间也是(x,y). 但是对于机械臂，目标是终端空间位置(xyz), 但采样空间却是关节空间(q0,q1,…qN)。有了IK之后，我们就可以把三维空间的目标p转化为关节空间的目标q。那么这样就会让采样算法能算的更快，具体方法不赘述，这样的算法有RRT-Connect，BKPIECE等等双向采样算法。

1.4. MoveIt!

问：我不想看也看不懂OMPL和各种算法，但是我想让机械臂动起来，怎么办？

答：那这正是MoveIt!的设计初衷。Move It！让它动起来！

OMPL是运动规划的“规划”部分，而MoveIt!是OMPL的ROS接口。当然这不完全准确，OMPL有单独的ROS接口，但依旧很繁杂，而MoveIt是OMPL ROS接口的接口。。。而且MoveIt!还结合了其他一些功能，总之MoveIt!就是个大接口。。

MoveIt!能做什么？一句话，MoveIt!就是一个模块化的接口，让你在最短时间内，不用自己写太多代码，就能配置出一个ROS Package来为你的机械臂做运动规划。

2 MoveIt基础

MoveIt！由ROS中一系列移动操作的功能包组成，包含运动规划，操作控制，3D感知，运动学，碰撞检测等等，而且提供友好的GUI。官方网站：http://moveit.ros.org/，上边有MoveIt！的教程和API说明。

架构

下图是MoveIt整体框架



move_group是MoveIt的核心部分，可以综合机器人的各独立组件，为用户提供一系列需要的动作指令和服务。从架构图中我们可以看到，move_group类似于一个积分器，本身并不具备丰富的功能，主要做各功能包、插件的集成。它通过消息或服务的形式接收机器人上传的点云信息、joints的状态消息，还有机器人的tf tree，另外还需要ROS的参数服务器提供机器人的运动学参数，这些参数会在使用setup assistant的过程中根据机器人的URDF模型文件，创建生成（SRDF和配置文件）。

二 安装

sudo apt-get install ros-kinetic-moveit
source /opt/ros/kinetic/setup.bash

下边会使用《master ros for robotics programming》里的案例进行学习，在工作空间里git下来源码：

cd ~/catkin_ws/src
git clone https://github.com/qboticslabs/mastering_ros.git[/code] 
三. 创建MoveIt! Package

2.1 准备URDF package

2.2  配置

使用MoveIt!的第一步是要使用 Setup Assistant工具完成一些配置工作。Setup Assistant会根据用户导入的机器人的urdf模型，生成SRDF( Semantic Robot Description Format)文件，从而生成一个MoveIt!的功能包，来完成机器人的互动、可视化和仿真。

主要参考这个教程是对机器人pr2的一个moveit配置.

错误:URDF/COLLADA file is not a valid robot model
解决: 意思是: urdf / COLLADA文件不是一个有效的机器人模型  只有urdf文件可以进行moveit配置


如何运行机械臂呢,为了下次方便.

1)打开配置助手:根据教程来,这里只能打开urdf格式文件哦 .

MoveIt! Setup Assistant 是一个图形界面，可以让我们不用写代码看代码，直接用鼠标点击就可以配置机器人的运动规划所需要的信息。点击Create New MoveIt Configuration Package来创建新的配置包，选择刚刚下载的urdf，然后点击Load Files 载入文件。

source /opt/ros/kinetic/setup.bash
roslaunch moveit_setup_assistant setup_assistant.launch




2)创建碰撞免检矩阵(ACM)

点击Setup Assisant的左边第二项’Self-Collisions’，在这里我们将创建碰撞免检矩阵(Avoid Collision Matrix, ACM)。再次强调，怎么创建很简单，点击一下’Regenerate Default Collision Matrix’就可以了，问题是，为什么？ACM是做什么的？

我们知道，碰撞检测是非常复杂的运算过程。对于多关节机械臂或者类人机器人来说，机械结构复杂，肢体多，碰撞检测需要涉及很多的空间几何计算。但是对于刚体机器人来说，有些肢体之间是不可能发生碰撞的，比如原本就相邻的肢体，比如类人机器人的脚和头。这里生成的ACM就是告诉我们，这个URDF所描述的机器人，哪些肢体之间是不会发生碰撞的。那么在之后的碰撞检测算法中，我们就可以略过对这些肢体之间的检测，以提高检测效率。