ROS - MoveIt 快速搭建機器人運動規劃平台

　　MoveIt是由ROS中一系列運動操作的功能包组成，包含運動規劃、操作控制、3D感知、運動學、碰撞檢測等等，並包含GUI控制介面。另外，MoveIt官網上還有相當詳細的教學與API說明。
　　MoveIt官網：http://moveit.ros.org/

MoveIt架構

下圖是MoveIt的系統架構圖

move_group是MoveIt的核心部分，可以綜合機器人的各獨立組件，為用戶提供一系列需要的動作指令和服務。從架構圖中我們可以看到，move_group類似於一個積分器，本身並不具備豐富的功能，主要做各功能包、插件的集成。它通過消息或服務的形式接收機器人上傳的點雲信息、joints的狀態消息，還有機器人的tf tree，另外還需要ROS的參數服務器提供機器人的運動學參數，這些參數會在使用setup assistant的過程中根據機器人的URDF模型文件，創建生成（SRDF和配置文件）。

一、安裝

sudo apt-get install ros-kinetic-moveit

 以下將使用《master ros for robotics programming》的案例進行學習，tutorial source code如下：

git clone https://github.com/qboticslabs/mastering_ros.git

二、Setup Assistant Tutorial

1. To start the MoveIt Setup Assistant:

roslaunch moveit_setup_assistant setup_assistant.launch

開啟「MoveIt Setup Assistant」之後，選擇「Create New MoveIt Configuration Package」建立新的Configuration Package。

這裡有兩個選擇，一個是新建一個配置功能包，一個是使用已有的配置功能包。這裡我們選擇新建配置功能包，並在程式碼中找到urdf文件(seven_dof_arm.urdf)。如果已有配置功能包，那麼新建後會覆蓋原有的文件。

2. Generate Self-Collision Matrix

然後點擊左側的第二項配置步驟，配置自碰撞矩陣，MoveIt！可以讓我們設置一定數量的隨機採樣點，根據這些點生成配裝參數，可想而知，過多的點會造成運算速度慢，過少的點會導致參數不完善，默認的採樣點數量是10000個，官方稱經過實踐，要獲得不錯的效果，10000是最低要求，我們就按照這個預設值生成碰撞矩陣。

3. Add Virtual Joints

虛擬關節主要是用來描述機器人在world坐標系下的位置，如果機器人是移動的，虛擬關節可以與移動基座關聯，不過一般的機械臂都是固定不動的，所以也可以不需要虛擬關節。

4. Add Planning Groups
Planning Group用於描述機器人的不同部分，例如定義手臂或末端夾具部分。這一步可以將機器人的多個組成部分（links，joints）集成到一個Planning Group當中。之後Motion Planning會針對個別的Planning Group完成運動規劃，在規劃的過程中還可以選擇運動學解析器（ kinematic solver）。

這裡創建兩個組，一個是機械臂部分（arm），一個是夾具部分（gripper）。

5. Add Robot Poses
Setup Assistant允許在配置中添加某些固定的姿勢（Pose），例如機器人的零點位置（Home）、初始位置等等，當然這些位置是用戶根據場景自定義的，不一定要和機器人本身的零點位置、初始位置相同。這樣做的好處就是我們使用MoveIt的API時，可以直接調用這些位置。

6. Label End Effectors
在添加了夾具的Planning Group之後，將這些群組指定為end-effectors可以在內部進行一些特殊操作。

7. Add Passive Joints
機器人上的某些關節可能在規劃控制的過程中不會使用到，可以先宣告出來。這裡沒有就不用管了。

8. Generate Configuration Files
最後一步，生成配置文件。一般會取名[robot_name]_moveit_config。這裡，會提示覆蓋已有的配置文件。

MotionPlanning

在完成配置之後即可退出 Setup Assistant，接著執行「demo.launch」看看：

$ roslaunch seven_moveit_config/launch/demo.launch

＊若是要執行Motion Planning的話，還需要再執行「move_group.launch」，導入Planning Library：

$ roslaunch seven_moveit_config/launch/move_group.launch

　　

這個介面是在rviz的基礎上加入了MoveIt Plugin，通過左下角的Plugin，我們可以選擇機械手臂的目標位置，進行運動規劃。或者拖動機械臂的前端，也可以改變機械臂的姿態。

在規劃頁面點擊「Plan and Execute」，MoveIt即會開始路徑規劃，並且可以看到機械臂開始向目標位置移動!

參考資料：
ROS MoveIt - tutorials
古月居　ROS探索总结—MoveIt基础
运动规划 (Motion Planning): MoveIt! 与 OMPL