雷鋒網按：本文作者qqfly，上海交通大學機器人所博士生，本科畢業於清華大學機械工程系，主要研究方向機器視覺與運動規劃，本文首發作者微信公眾號：Nao(ID：qRobotics)，雷鋒網已獲授權。
兩個月前給自己挖了個坑，說要寫寫MoveIt，但一直沒動手。主要有兩個原因：
1）這兩個月主要在寫小論文，畢竟博士生要畢業還得看論文，不能靠公眾號閱讀量分享率；
2）直接講MoveIt似乎需要挖更多坑，一直沒想好怎麼寫比較好。
主要是因為機器人運動規劃涉及太多基礎內容，如果跳過不講就會變成新坑；一時半會又沒法講完。
所以，這次就從初學者如何利用MoveIt快速搭建機器人運動規劃平臺來講吧，先展示Big Picture，其他細節內容以後有空再慢慢填。但可能會坑的地方我會用（坑）標註出來。
什麼是MoveIt
先看個視頻介紹吧： https://v.qq.com/x/page/r0349ddxsqf.html
看完視頻，大家應該對MoveIt有一個大概的印象了。用MoveIt官網（moveit.ros.org）的說法：
它是目前最先進的移動（坑）操作機器人軟體，整合了最先進的運動規劃、操作、3D感知、運動學、控制與導航算法。為這方面的開發人員提供了一個十分便利的開發平臺。
這個說法不太直觀，換個說法就是MoveIt = RobotGo，翻譯成中文就是「機器人，走你！」
所以，MoveIt的主要就是一款致力於讓機器人能夠自主運動及其相關技術的軟體，它的所有模塊都是圍繞著運動規劃的實現而設計的。
下面大概介紹下它的一些功能模塊。
運動規劃（Motion Planning）：運動規劃的介紹內容之前公眾號已經發過了，要讓一個機器人實現運動規劃，需要先將機器人抽象到構形空間（C-Space）。MoveIt就可以幫大家把這些工作給做了，只需提供機器人URDF模型，就可以調用幾大運動規劃庫（坑）的規划算法（如OMPL，SBPL，CHMOP），自動生成機器人運動軌跡。操作（Manipulation）：這個目前還比較弱，就是根據識別的物體生成一系列動作抓取物體（pick-and-place），不涉及任何反饋、動力學、re-grasp等操作問題，所以我一般都不用這個模塊。3D感知（Perception）：這個並不是說MoveIt整合了物體識別、環境建模等模塊，而是它可以利用傳感器（坑）採集的信息（點雲或深度圖像）生成用於碰撞檢測的OctoMap。OctoMap這個東西挺好的，做SLAM的同學應該了解，它就是以八叉樹形式表示點雲，可以大大降低存儲空間，它看起來就跟你們玩的minecraft差不多。同時，這些3D OctoMap也可以依據貝葉斯準則不斷實時更新。這樣，機器人就可以避開真實世界的障礙物了。
運動學（Kinematics）：運動學機器人工作空間與構形空間（C-Space）的映射關係，所以MoveIt就它也包括在自己系統內。目前它可以支持多種運動學求解器，如OpenRave的ikfast（封閉解）、Orocos的KDL（數值解）、Trac_ik（考慮關節極限的數值解）、基於service的求解器（用戶自己定義）。（坑）碰撞檢測（Collision Checking）：碰撞檢測是運動規劃的一大難題，如果採用基於採用的規划算法，那麼我們需要對每個採樣點做有效性判斷，這時候就需要進行碰撞檢測。所以，運動規劃需要提供一個高效的碰撞檢測算法。幸好，香港城市大學的潘佳大神寫了個FCL（Flexible Collision Library），可以非常快速地實現各種幾何體（3D面片、OctoMap、基本幾何體）的碰撞檢測。（這個不是坑，直接用很好用，但以後有機會可以好好說說，反正潘大神不會看朋友圈，不至於班門弄斧）。軌跡插值（Trajectory Processing）：由於大多數規劃器只能返回一系列路徑點，MoveIt可以根據機器人的控制參數（速度、加速度限制等）重新處理路徑，生成一條帶有時間戳、位置、速度、加速度信息的完整軌跡。控制（Controll）：這個其實不能算控制，只是一個機器人控制接口問題。由於不同機器人的控制接口都不一樣，開發者只需簡單修改配置文件，就可以讓MoveIt發布出機器人相應的控制指令（只是修改action名字而已）。導航（Navigation）：這是個大坑，MoveIt雖然原理上可以進行移動機器人的導航，但是它沒提供針對移動機器人的規劃器。也就是說目前它的Navigation功能是不能直接使用的（自己寫規劃器和碰撞檢測已經超出初學者的要求了）。
交互（Interaction）：MoveIt給開發者提供了三種方式交互方式，Rviz圖形界面（直觀）、Python（快速編程）、C++（豐富的高級功能）。
目前而言，MoveIt還只是一個針對機械臂運動規劃問題的軟體平臺，暫時還不是適合用在Navigation、manipulation、control、perception等領域。
為什麼選擇MoveIt
其實，要做運動規劃可以用很多方法：自己寫、OMPL、OpenRave等。我個人認為選擇MoveIt大概有三個原因：
| 對初學者很友好
前面我們已經知道，要想自己從頭建立一個運動規劃的軟體平臺需要花費非常多力氣：運動學正逆解、碰撞檢測算法、環境識別、規划算法，任何一點都是需要一段不短的時間積累才可能親手實現的。初學者可能一開始就被這些次要問題打退了。
自己寫：對初學者簡直是災難，尤其是沒人帶的情況；OMPL：完全沒有機器人的概念，需要用戶自己集成運動學、碰撞檢測算法，它的官方文檔也不涉及如何利用OMPL做機器人的運動規劃問題；OpenRave：雖然OpenRave已經有自己的一套機器人描述方法，但是它的文檔不太友好。
用MoveIt的話，初學者只需準備機器人的模型，跟著教程走，便可以在半小時內實現仿真環境中的機器人運動規劃演示。
| 方便研究
這個應該是更重要的因素。運動規劃由很多子問題構成，每個子問題都可以成為一個研究點。MoveIt幾乎所有組件都是以Plugin的形式工作的，也就是說我們可以隨時更換它的任一模塊。目前它支持以下組件的修改：
運動學求解器規划算法 同時OMPL自身也支持用戶自己編寫規劃器規劃器初始化方法控制器接口傳感器接口規劃器的採樣算法碰撞檢測算法OctoMap更新算法
如果是做這些課題研究的人，完全可以先用MoveIt建立一個環境，之後修改相應Plugin，換成自己的算法。這樣可以讓我們將側重點放在主要矛盾上。
| 活躍的社群
這點其實就是ROS相對於其他機器人開發平臺的優點。MoveIt依託於ROS，也擁有很高的人氣（去年的調研結果看，MoveIt是ROS中使用度排名第三的package）。
活躍的社群對於學習是大有裨益的：
①遇到問題很容易問到能解決的人，剛開始MoveIt還沒出文檔，我就是靠著MoveIt的mail lists入門的；②網站、教程、代碼維護更新很好，MoveIt剛推出時，總是有一大堆Bug，現在才過幾年，已經非常好用了，官方教程也已經非常人性化了。
怎麼樣使用MoveIt
要用MoveIt控制機器人大概分為以下幾步：
建立機器人URDF模型（必須）建立機器人ROS驅動生成MoveIt配置文件（必須）標定相機修改MoveIt配置文件與launch文件機器人，走你！（必須）
其中，上面未標明「（必須）」字樣的只有在使用實際機器人時才需要。初學者如果只想在仿真裡看看的話，可以先跳過。
| 建立機器人URDF模型（必須）
URDF（Unified Robot Description Format）是ROS中使用的一種機器人描述文件，它以HTML的形式定義一個機器人。包含的內容有：連杆、關節名稱，運動學參數、動力學參數、可視化模型、碰撞檢測模型等。
後續碰撞檢測、運動學求解、規劃等都依賴於URDF文件。
那麼，要如何建立URDF文件呢？如果你用的是單臂、串聯機器人，並且你本人沒有強迫症的話，可以使用ROS官方發布的sw_urdf_exporter，它可以幫你從SolidWorks中導出URDF文件。
但如果不幸你使用的是雙臂（雙臂機器人用這個插件經常出問題）或者非並聯機器人（需要自己用mimic_joint改成串聯形式），又或者你有強迫症（想要儘量簡潔、漂亮的模型）的話，可以考慮自己手寫URDF或者xacro文件（坑）。
這點我就不具體說了，簡單寫幾個要點：
多臂用xacro來減少工作量；坐標系設置儘量滿足所有關節為0°時候，所有坐標系同姿態（這樣可以避免引入pi）；如果想要有顏色的模型，可以自己生成每個零件的dae模型，而不使用stl模型；可視化模型採用漂亮、精細的模型，碰撞模型可以使用簡化的模型。
| 建立機器人ROS驅動
如果你不用真實機器人，這步可以先跳過。
機器人的ROS驅動並沒有什麼標準的格式或者規定。對於MoveIt而言，只要求你有個ROS node，它有兩個功能：
1）發布關節角度/joint_states
如果連接實際機器人，MoveIt需要從機器人當前狀態開始規劃，因此這個ROS驅動需要能夠實時獲取機器人的各關節信息（如角度），並用過/joint_states消息發布；
2）接收規劃結果，並下發給機器人
由於MoveIt規劃的結果會以一個action的形式發布，所以我們的ROS驅動就應該提供一個action server，這個功能就是接收規劃結果，下發給機器人，並反饋執行情況。action的類型是control_msgs/FollowJointTrajectory。 具體action的寫法可以參照ROS官網（坑）。
簡單而言，一個action有五個部分：
action_name/goal：這個就是規劃的路徑，我們需要接收這個路徑，並將所有路徑點解析成機器人控制器可以識別的形式，之後下發給機器人，必須要有；action_name/cancel：這個指令可以隨時中斷正在執行的動作，但並不是必須的功能；action_name/feedback：這個是實時反饋執行狀態，最簡單的就是將機器人當前關節角度等信息反饋回去，非必須；action_name/status：這個用於顯示機器人狀態，如正在執行動作、等待、執行結束等待，非必須；action_name/result：這個就是在動作執行完之後給MoveIt反饋一個執行結果，這個是必須要有的，當然，為了簡單，可以已接收到goal就反饋執行成功。
這部分在MoveIt部分是看不到文檔的，所以也是阻礙初學者使用MoveIt控制自己機器人的最大問題之一。但是了解了它的機理之後，就比較簡單了。 如果你是第一次使用MoveIt，極力推薦你先試試UR、Baxter等已經寫好這部分驅動的機器人。
| 生成MoveIt配置文件（必須）
這個利用MoveIt的setup assistant界面，按照教程很容易就能配置好。
這步做完，就可以直接在仿真裡面看運動規劃效果了。這是我覺得MoveIt對初學者最友善的地方，不用寫一行代碼就可以看到運動規劃。
| 標定相機
這個主要涉及相機模型與AX=XB求解問題，不多贅述。 這步就是為了讓機器人知道攝像頭放在機器人的哪個位置。大家可以看看我實驗室師弟寫的自動標定演示：
一個launch文件就能完成標定，不知比我當年寫的手動標定方法高多少。這個標定程序我們之後可能會開源出來。如果有機會我也會順便講講它的原理（坑）。
| 修改MoveIt配置文件與launch文件
因為前面生成的文件都是針對虛擬機器人的，如果需要連接實際機器人，需要修改一些配置文件，我可能記不太清具體要修改幾個文件了，請以官方教程為主：
controllers.yaml：這個就是要根據你的ROS驅動中的action來修改，MoveIt可以根據這個配置文件發布出與機器人驅動相匹配的action。簡單地說，就是action的名字、類型、關節名字幾個信息。robot_moveit_controller_manager.launch：這個額外新增，就是在不適用fake controller的時候能找到上述controller.yaml文件，發布出正確的action類型。sensors.yaml：這個需要額外增加，它主要定義了點雲的消息名稱、OctoMap屬性等。moveit_sensor_manager.launch：同樣的，增加傳感器配置文件後，我們也需要在launch文件中增加對配置文件的讀取。其他（可選）：industrial_robot_simulator、warehouse、joystick、規劃器、規划算法庫……機器人，走你！（必須）
上述內容完成後，就可以Enjoy Yourself了，無論是Rviz, Python 還是C++，都可以用來進行運動規劃，如果連接了真實機器人，那麼也可以在實際機器人上完成運動規劃。
其他
雖然寫了這麼多，但感覺還是沒寫清楚，最後隨便列點之前大家在後臺問過的比較多的問題：
| 什麼機器人能用MoveIt
MoveIt其實跟機器人關係不大，只要你有URDF文件，能接受控制指令，那麼就能用MoveIt，移動機器人的話也可以，只是MoveIt現在沒有針對Navigation做規劃器。
| 怎麼用MoveIt做移動機器人3D Navigation
相對於傳統Navigation包，MoveIt中可以做3D的碰撞檢測，但是它尚未加入適合移動平臺的規划算法。大概做法如下：
修改FCL，開放出碰撞檢測函數（最新版本好像已經可以直接調用了）；寫一個規劃器：最簡單的就是自己寫一個A*或Dijkstra，也可以想辦法將SBPL用起來（我沒在移動機器人上試過），這樣就可以進行全局規劃了；寫一個action server，接受規劃結果，同時將其轉換成Navigation包的gloal_planner相同格式，利用Navigation的local_planner完成路徑跟蹤；當然，這一步也可以自己寫local_planner。
| 怎麼用MoveIt做飛行器或潛艇的路徑規劃
這個與上個問題類似，MoveIt沒有針對剛體的規划算法，如果可以接受RRT的規劃結果，那麼理論上講是可以直接使用的。
| 如何在MoveIt上使用自己的規划算法
我只嘗試過先在OMPL中寫自己的規劃器，之後通過修改moveit_planner中的ompl_interface，將自己的規劃器用到MoveIt中； 如果是非Sampling-based方法，那就要去看看MoveIt的Plugin怎麼改了，這部分我沒經驗。
| 如何學習MoveIt
關注我的公眾號（劃重點）；
學ROS基本概念：三種消息機制等；學教程：按照官網教程走一遍；遇到問題，先在ROS問答區或MoveIt的mail lists搜索是否有同類問題，如沒有，則自己在上述平臺提問； （至此， 你已經會用MoveIt了，但用得效果肯定不好）看MoveIt各部分API，闡釋其高級功能；根據自己需要，修改部分源碼（例如開放出FCL的各種功能），之後再MoveIt官方github上提出修改源碼請求（PR）； （至此，你已經掌握了MoveIt這個工具，可以充分發揮MoveIt的功能）根據自己的研究內容，寫自己的Plugin，充分發掘MoveIt的潛力；如果效果好，那麼在IROS/ICRA發paper，會議中找MoveIt的作者們聊天，回家後到github上將自己的代碼開源，並PR到MoveIt上。
機器人，走你！