一.安装MoveIt!
我用的ROS系统的版本为melodic,因此安装MoveIt!的命令为：
$sudo apt install ros-melodic-moveit ros-melodic-moveit-plugins ros-melodic-moveit-planners
二.ＭoveIt!架构
理解MoveIt!的架构有助于我们的编程，并有助于将机器人与MoveIt!进行交互。
MoveIt!的架构如图所示，官方网址为http://moveit.org/documentation/concepts
MoveIt!架构图
move_group节点
我们可以认为move_group就是MoveIt!的核心，因为该节点作为机器人的各个部件的集合，并根据用户的需求提供动作和服务。
在架构图中可以清楚地看出来，move_group节点用话题和服务的形式搜集机器人的信息、例如点云、关节状态、坐标变换（tf)
MoveIt!从参数服务器收集了机器人的运动学数据，例如robot_description(URDF)、语义机器人描述格式(Semantic Robot Description Format,URDF)及配置文件，
在我们为机器人生成MoveIt!软件包的同时也生成了SRDF文件和配置文件。配置文件包含设置关节约束、感知、运动学、末端执行器等方面的参数文件。
当MoveIt!可以获取关于机器人及其配置的所有信息时，就已经配置好了，我们可以从用户界面开始控制机器人了，我们可以使用MoveIt!的Ｃ＋＋或者PythonAPI接口
向move_group节点发指令以执行一些动作，如拾取/放置、逆向运动学（IK) 和正向运动(FK)。
move_group节点是一个集合，它没有直接运行任何类型的运动规划算法，而是将所有的功能作为插件连接起来。
在运动规划之后，生成的运动轨迹可以使用FollowJointTrajectoryAction接口与机器人的控制器进行交互。这是一个动作接口，通过该接口动作服务器在机器人上运行，
move_node节点负责初始化一个动作客户端，该动作客户端与该服务器通信并在真实机器人或Gazebo仿真器中执行这些规划的轨迹。
用MoveIt!进行运动规划假如我们知道了机器人的初始姿态，机器人的期望目标姿态，机器人的几何描述以及周围环境的几何描述，
那么路径规划就是寻找将机器人逐步从初始姿态移动到目标姿态的最佳路径，在运动过程中不能碰到障碍物，
也不能与自身的机器人连杆碰撞。
在这里，机器人的外观几何形状由URDF文件描述，我们还可以为机器人环境创建描述文件，
并用机器人上的激光或视觉传感器为其操作空间构建地图，避免在执行规划的路径时与出现的
静态或动态的障碍物发生碰撞。
在机械臂的例子中，运动规划器需要找出一个可以让机器人的所有连杆都不与环境发生碰撞的轨迹(由每个关节的关节空间组成），也要避免自碰撞（机器连杆之间的碰撞），还要避免超过关节的极限。
MoveIt!可以通过插件接口与运动规划器进行交互。我们只需要通过更改插件即可使用任意的运动规划器，
此方法具有高度可扩展性，因此我们可以使用此接口来尝试自定义的运动规划器。move_group节点通过ROS动作和服务与运动规划器插件进行交互。
move_group节点默认的规划是OMPL（http://ompl.kavrakilab.org/)
为了启动运动规划，我们需要向运动规划器发送运动规划请求，以指定我们具体的规划要求，规划需求可以是设定末端执行器的目标姿态。
我们可以为运动规划器设置附加运动学约束，下面是MoveIt!中的一些内置约束：
*位置约束 ：这些约束限制连杆的位置。
*方向约束 ：这些约束限制连杆的方向。
*可见性约束 ：这些约束限制连杆上的点在区域中的可见性（传感器的视野）
*关节约束 ：这些约束限制关节的限值。
*用户指定的约束 ：利用这些约束，用户可以使用回调函数定义自己的约束。
利用这些约束，我们可以发送运动规划请求，规划器将根据请求生成适当的轨迹，
move_group节点将从运动规划器中生成适当的轨迹，该轨迹将遵循所有的约束，最终的规划路径将发送给机器人的关节轨迹控制器。
运动规划请求适配器规划请求适配器(planning Request Adapter)有助于预处理运动规划请求并对运动规划响应
进行后续处理。预处理请求的一个用途是帮助纠正关节状态的非法情况，另外在后续处理时，
它还可以将规划器生成的路径转换为以时间为参数的轨迹，下面是MoveIt!中一些默认的规划请求适配器：
*FixStarStateBounds : 如果关节状态轻微超出了关节的限制，则此适配器可以将关节限值固定在限制范围内。
*FixWorkspaceBounds : 该适配器指定了用于规划多维数据集大小的工作空间为１０m*10m*10m.
*FixStarStateCollision : 如果现有的关节位姿(configuration)处于碰撞状态，该适配器将采样生成新的无碰撞位姿。
它通过一个名为jiggle_factor的因子更改位姿来进行新配置。
*FixStarStatePathConstraints : 该适配器将在机器人的初始状态不符合路径约束时使用，这里它将寻找满足路径约束的近似姿态并将此姿态当作初始姿态。
*AddTimeParameterization : 该适配器通过添加速度和加速度约束来参数化运动规划。
MoveIt!规划场景
“规划场景”是用来表示机器人周围的世界并存储机器人本身的状态。在move_group内的规划场景监视器维护着规划场景的表示，
move_group节点由另一个称作世界几何结构监测器的部分组成，该部分通过机器人的传感器和用户的输入构建世界的几何结构。
规划场景监视器从机器人读取joint_states话题，从世界几何结构监视器读取传感器信息和世界几何结构，世界场景监视器从从占用
地图监视器中读取数据，该监视器使用3D感知构建周围环境的３D表示，称为Octomap。Octomap可以由点云生成，这些点云由点云占用地图更新插件来处理，
深度图像由深度图像占用地图更新器处理。如图是MoveIt!规划场景概览图：
MoveIt!规划场景概览图
MoveIt!处理运动学
MoveIt!使用机器人插件为切换逆向运动学算法提供了极大的灵活性。用户可以将自己的IK解算器编写为MoveIt!的插件，
并在需要时替换默认的结算器插件。MoveIt!中的默认IK解算器是基于雅可比行列式的数值解算器。
与解析解算器相比，数值解算器可能需要更多的时间来处理IK，这个名为IKFast的软件包可以用来生成用于使用解析方法来求解IK的C++代码，
该方法可用于不同类型的机器人执行器，且ＤＯＦ小于６的时候表现更好，此Ｃ＋＋代码可以用一些ROS工具转换为MoveIt!插件。
正向运动学和计算雅可比矩阵已经集成了MoveIt! RobotState类中，因此我们不需要使用插件来解决FK问题。
MoveIt!碰撞检测
MoveIt!中的Collision对象用来查找规划场景内的碰撞。该场景使用Flexible Collision Library (FCL)软件包作为后端，MoveIt!支持多种不同类型对象的碰撞检测，
比如网格物理，基本形状如立方体、圆柱、圆锥、球体和Octomap。
碰撞检测是运动规划期间计算成本高昂的任务之一，为了减少计算资源的消耗，MoveIt!提供了允许碰撞的矩阵(Allowed Collision Matrix,ACM)。它包含
一个二进制数值，确定是否要检测两物体之间的碰撞。如果矩阵中的数值是１，意味着无须为相应的两个物体检测碰撞，我们可以将离得较远彼此之间不可能
发生碰撞的两个物体的该值设置为１，这样优化ACM可以减少避免碰撞规避所需的总计算量。
三.使用配置助手工具生成MoveIt!配置软件包MoveIt!配置助手是一个图形化的用户工具，用来将任何机器人安装配置到MoveIt!中。
简单的说，这个工具可以生成SRDF配置文件，启动文件和机器人URDF模型生成的脚本，
这是配置move_group节点所必需的。
SRDF文件详细描述了有关机械臂关节、末端执行器关节、虚拟关节和碰撞连杆对的详细信息，
这些都是可以用配置助手工具进行MoveIt!配置过程中的配置。
配置文件包含了运动学解算器、关节约束、控制器等方面的详细信息，这些信息也在配置过程中进行配置和保存。
使用生成的机器人配置软件包，我们可以在没有真实机器人和仿真界面的情况下在RViz中进行运动规划。
下面启动配置界面，我们就可以看到构建机械臂配置软件包的分步过程。
第一步：启动配置助手工具
我们使用以下命令来启动MoveIt!配置助手工具：
$ roslaunch moveit_setup_assistant setup_assistant.launch
这将打开一个窗口并提供两个选择：Create New MoveIt! Configuration Package或者Edit Existing MoveIt! Configuration Package。
这里我们正在创建一个新配置软件包，所以我们选择第一个选项。如果我们已经有一个MoveIt!配置软件包了，那么我们就可以选择第二项。
点击Create New MoveIt! Configuration按钮，将出现一个新的窗口，如图所示：
在此步骤中，该助手会提示需要提供新机器人的URDF模型，这里我们点击Browse按钮，选择该路径下的mastering_ros_robot_description_pkg/urdf/seven_dof_arm.urdf文件。
选择该文件后，点击Load按钮来加载URDF，我们可以将机器人模型作为纯URDF或者xacro文件，如果我们提供xacro模型，那么该工具将在内部转换为URDF模型。
如果机器人模型解析成功，我们将在窗口中看到机器人的模型，如图所示：
配置助手工具成功解析机器人模型
第二步：生成自碰撞矩阵现在我们可以查看该窗口的所有面板来正确配置我们的机器人，在Self-Collisions选项中，MoveIt!查询机器人中可以安全禁用碰撞检测的连杆对，这样可以减少处理时间。
此工具对每个连杆进行分析，并将每对连杆对分类为始终处于碰撞状态（Always in Collision),永远不会碰撞(Never Collision),默认处于碰撞状态（Default in Collision），
禁用相邻连杆(Adjacent Links Disabled)、有时处于碰撞状态(Sometimes in Collision），该工具将对发生碰撞的连杆禁用碰撞检测。如图所示：
采用密度是检查自碰撞的随机位置的数量，如果采样密度高，则计算量较大，但自碰撞机率会较小。采样密度的默认值为１００００，
我们可以通过点击Regenerate Default Collision Matrix按钮看到禁用的连杆对，列出禁用的连杆对需要几秒钟时间。
第三步：增加虚拟关节
虚拟关节(Virtual Joints)将机器人与世界连接起来，对非移动的静态机器人，虚拟关节不是必须设置的。
当机械臂的底座非固定时，我们需要虚拟关节，当一个机械臂固定在移动机器人上时，我们就需要定义
一个相对于里程计(odom)的虚拟关节。对于我们的机器人，我们不需要创建虚拟关节。
第四步：添加规划组
规划组（Planning Group)基本上就是机械臂中的一组关节/连杆，该规划组一起进行规划以实现到连杆或末端执行器的目标位置 。
我们需要创建两个规划组，一个用于机械臂，另一个用于夹爪。
点击左侧的Planning Group选项卡，然后再点击Add Group按钮，你将看到如图所示的界面，其中包含了机械臂组的各项设置：
在这里，我们将Group Name设置为arm，Kinematic Solver设置为kdl_kinematics_plugin/KDLKinematicsPlugin,
这是MoveIt!默认的数值IK解算器，我们可以将其他参数设置为默认值。此外我们还可以在规划组中用不同的方法添加元素，
例如，我们可以指定组的关节，添加其连杆，或直接指定运动链。
在arm组内，首先我们需要添加一个运动链，从第一个连杆base_link开始至连杆grasping_frame.
添加一个名为gripper的组，我们不需要再为gripper组提供运动解算器，在该组内，我们可以增加gripper的关节和连杆。设置如图所示：
如图 添加arm和gripper规划组
第五步：添加机器人姿态
在此步骤中，我们可以在机器人配置中添加一些固定的姿态。例如，我们可以在此步骤中指定起始位置或者拾取/防放置的位置，
优点是我们在用MoveIt!API编程时可以直接调用这些姿态，这些姿态也叫作组状态，他们在拾取/放置和抓取操作中有很多的应用，
机器人可以毫无困难地切换到固定姿态。
第六步：设置机器人的末端执行器
在此步骤中，我们需要给机器人的末端执行器命名并分配末端执行器组，父连杆和父级组。
我们可以给这个机器人添加任意数量的末端执行器，在我们的例子中，它是一个专门用来执行拾取/放置操作的夹爪。
点击Add End Effector按钮并将末端执行器命名为robot_eef，将规划组设置为gripper（前面我们已经创建过），
父连杆设置为grasping_frame，父级组设置为arm,如图所示：
如图 添加末端执行器
第七步：添加被动关节
在此步骤中，我们可以指定机器人中的被动关节，被动关节是指关节没有任何执行机构。
脚轮是一个被动关节的例子，规划器将在运动规划时忽略这些类型的关节。
第八步：作者信息
在此步骤中，机器人模型的作者可以增加catkin所需的个人信息，包括作者的名字和邮件地址，以便将该模型发布到RoS社区。
第九步：生成配置文件我们基本上算是完成了，现在我们到了最后一步，即生成配置文件，在此步骤中，该工具将生成包含与MoveIt!接口交互所需文件的配置软件包。
点击Browse按钮选择用来保存由配置助手工具生成的配置文件的文件夹，在这里我们可以看到文件在一个名为seven_dof_arm_config的文件夹中创建。
你可以将配置软件包命名为机器人名称加_config或_generated这样的后缀。
点击Generate Package按钮后，它将在指定的文件夹中生成配置文件。
如果该过程成功，我们可以点击Exit Setup Assistant退出配置助手工具。
如图展示了生成配置文件的过程
生成MoveIt!配置软件包后，我们可以将它复制到我们的catkin工作区，在下一节，我们将使用此软件包。
创建机器人模型可以从对应的Github代码仓库中下载：
$ git clone https://github.com/jocacace/seven_dof_arm_config