通过ROS控制真实世界的机械臂(4)---- MoveIt !
- OMPL能做什么？ 简单说，就是提供一个运动轨迹。给定一个机器人结构(假设有N个关节)，给定一个目标(比如终端移到xyz)，给定一个环境，那么OMPL会提供给你一个 轨迹，也就是一个完整的关节位置。沿着这个轨迹依次移动关节，就可以最终把终端移到xyz，当然，这个轨迹应当不与环 境中的任何障碍发生碰撞。
- 为什么用OMPL？ 运动规划的软件库和算法有很多，而OMPL由于其模块化的设计和稳定的更新，成为最流行的规划软件库之一。很多新算法都在OMPL开发。很多其他软件（包括ROS/MoveIt）都使用OMPL做运动规划。
根据之前创建好的URDF文件，使用Moveit！配置助手完成redwall_arm_moveit_config包的配置
1 .打开MoveIt! Setup Assistant
$ roslaunch moveit_setup_assistant setup_assistant.launch
MoveIt! Setup Assistant 是一个图形界面，直接用鼠标点击就可以配置机器人的运动规划所需要的信息。点击Create New MoveIt Configuration Package来创建新的配置包，选择urdf所在的包，然后点击Load Files 载入文件。
2. 创建碰撞免检矩阵(ACM)
点击Setup Assisant的左边第二项'Self-Collisions'创建碰撞免检矩阵，点击一下'Regenerate Default Collision Matrix'就可以了
这里生成的 ACM就是这个URDF所描述的机器人，哪些肢体之间是不会发生碰撞的。
3 创建虚拟关节 (Virtual Joints)
在Setup Assistant 第三项Virtual Joints里面，我们要创建所谓的虚拟关节。这个虚拟关节，可以理解为一个连接机器人和世界的关节。一般来说，Virtual Joint Name命名为‘world’，而'Child Link'指的是我们要把‘世界’和机器人的那个部位连接，那选择基座'base_link'。‘Parent Frame Name’，是世界坐标的名字, 设置为 'world' , 很显然这里我们选择固定Fixed. 代表机器人相对于世界是固定的。
4 创建规划群 (Planning Groups)
首先，点击Add Group, 我们会看到一个界面，如下图，接下来，我们要正式创建这个组群，有很多不同的方法，Add Joints, Add Links, Add Kin. Chain, Add Subgroups。我们这里选择'Add Kin. Chain'，这样我们可以清楚的看到整个机器人的机械机构，在正中方我们可以看到这个机械臂的结构，一个link接着一个 link，在为末端执行器单独添加一个group。注意这里的group名称为arm_group!!!
5 创建机器人预设位姿 (Robot Poses)
在Setup Assistant 第五项, ‘Robot Poses’，我们创建预设的机器人位姿。点击‘Add Pose’，我们为机械臂创建一个向上直立的位姿，选择Planning Group为Arm。可以看到很多滚动条，全设为0就是垂直向上的位姿。然后点击保存。当然，你可以根据需要设置其他不同位姿。
6 配置终端控制器(End Effectors)
终 端控制器，就是机械臂的手，以后用来在工作环境中直接控制的部位。我们添加一个叫做endEff的终端控制器，End Effector Group选择之前创建好的finger，Parent_Link选择机械臂的最后一个肢体hand。Parent Group选择arm_group。
7 配置被动关节(Passive Joints)
所谓被动关节，就是指现实中不配置电机的关节，也就是不会出现在机器人的Joint State Msg里，以避免MoveIt与JointState出现匹配错误。这里我们的机械臂并没有被动关节，所以可以直接跳过。
8 生成配置文件(Configuration Files)
最后一步，在Configuration Package Save Path里面选择一个保存地址，放在catkin_ws/src/redwall_arm_moveit_config然后点击Generate Package，这样一个完整的MoveIt Configuration Package就创建好了.
MoveIt ! 配置包详解
打开刚刚创建好的redwall_arm_moveit_config文件夹，我们发现有config和launch两个文件夹。config中文件中需要关注的如下:
SRDF是moveit的配置文件，配合URDF使用,其他的几个都不用修改，暂时忽略就好。
1. config文件夹中有一个fake_controllers.yaml文件，内容如下，创建了一个虚拟的ros_control控制器，当我们使用Gazebo仿真，也就是使用虚拟机械臂的时候，某个launch文件启动了该控制器，给Gazebo提供了一个控制器接口，当我们点击 plan and execute(下面会提到)的时候，moveit就会规划一条包含各个关机信息的路径，发布在该控制器接口处，Gazebo订阅该内容，经过内部的计算完成仿真(我们不必考虑，控制真实机械臂的时候需要考虑)
controller_list:
- name: fake_arm_group_controller
joints:
- Link1
- Link2
- Link3
- Link4
- Link5
- name: fake_end_effect_controller
joints:
- Link52. Launch文件
文件非常多，目前我所用到的只有几个文件，修改起来也很简单。
demo.launch是运行的总结点。需要关注的内容如下：
[/move_group/fake_controller_joint_states]
- 第一大段说明了当我们使用机械臂的时候，使用的是moveit帮我们配置好的虚拟关节控制器，而不需要我们修改。当我们使用真实机械臂的时候我们修改将其改成自定义的关节控制器
- 第二大段发布静态的tf。比如说，你的机器人基座不在世界坐标的原点，你可以发布一个静态tf来描述机器人在世界坐标中的位置。目前就是我们发布 虚拟机器人状态的地方了，如果你有实体机器人，你需要去掉这一部分，有其他相应的节点来发布机器人状态。
- 第三大段说明了是让虚拟机械臂还是真实机械臂去执行规划好的路径。目前我们没有真实机械臂，所以使用默认的fake_execution。当我们控制真实机械臂的时候，需要将
的true改成false，并且配置真实的控制器controller替换默认的fake_controllers.yaml
move_group.launch运行了另一个moveit重要的节点move group。
move_group.launch的功能是让一个规划组群动起来。运动规划在move_group.launch中定义了运动规划库的使用，默认的是使用ompl运动规划库。其中需要关注的地方如下：
表明了默认使用fake_controllers去执行规划的路径，然后程序会去寻找一个文件名称为fake_moveit_controller_manager.launch.xml的文件，内容如下：
使用上述的fake_controllers.yaml文件配置启动了一个虚拟的控制器moveit_fake_controller_manager/MoveItFakeControllerManager。
还是一样的套路，上述fake的都是默认的，当需要使用真实机械臂时，就需要修改这些文件，
planning_context.launch定义了所使用的urdf和srdf文件，以及运动学求解库。不建议手动更改这些，但是如果你需要使用不同urdf，srdf可以在这里更改。setup_assistant.launch如果你需要更改一些配置，那么可以直接运行而不需要重新建一个包。
使用Moveit规划的路径，在Gazebo仿真
在上一篇文章中，在Gazebo中仿真出了机械臂之后，通过命令行的方式使Gazebo中的机械臂运动，现在有了moveit了，可以在Rviz中使用Moveit规划好的运动轨迹，让Gazebo中的机械臂运动。如下图，点击plan and execute之后，发现Rviz和Gazebo中的机械臂同时按照一定轨迹运动起来了！点击update，随机选取一些位置，然后点击plan and execute即可。这样rviz和gazebo中的fake机械臂会同步运动。
$ roslaunch redwall_arm gazebo.launch
$ roslaunch moveit move_group.launch
$ roslaunch moveit moveit_rviz.launch config:=True
