碼上快樂

相關內容    簡體    繁體

運動規划 (Motion Planning): MoveIt! 與 OMPL---44

本文轉載自   查看原文   2016-11-23 10:39   10744    機器人系統ROS的學習之路

原創博文：轉載請標明出處：http://www.cnblogs.com/zxouxuewei

最近有不少人詢問有關MoveIt!與OMPL相關的話題，但是大部分問題都集中於XXX功能怎么實現，XXX錯誤怎么解決。表面上看，解決這些問題的方法就是提供正確的代碼，正確的編譯方法，正確的運行步驟。

然而，這種解決方法只能解決這個特定的問題，而且解決之后我們也無法學到一些實際的東西。要想徹底明白，需要從源頭入手，也就是說，不要問“MoveIt! 怎么把機械手從空間一個點移到另一個點？“，而是要問”MoveIt! 為什么能把機械手從空間一個點移到另一個點？“。 這一點明白之后，遇到類似的問題，才能從容應對。同理，這不僅適用於MoveIt，也同樣適用於其他任何ROS功能。

所以，下文中我們會見到一些具體的例子，但整體上，更傾向於宏觀的概念和一些基礎的方法，希望對大家能有所幫助。這里的幫助指的是增強對運動規划和Moveit, OMPL的整體理解，而非局限於完成某一個功能，編譯運行某一個文件。


RRT) 和 Probabilistic Roadmap (PRM)了, 當然，這兩個是比較老的，還有很多其他新算法。

  • OMPL能做什么？ 簡單說，就是提供一個運動軌跡。給定一個機器人結構(假設有N個關節)，給定一個目標(比如終端移到xyz)，給定一個環境，那么OMPL會提供給你一個 軌跡，包含M個數組，每一個數組長度是N，也就是一個完整的關節位置。沿着這個軌跡依次移動關節，就可以最終把終端移到xyz，當然，這個軌跡應當不與環 境中的任何障礙發生碰撞。
  • 為什么用OMPL？ 運動規划的軟件庫和算法有很多，而OMPL由於其模塊化的設計和穩定的更新，成為最流行的規划軟件庫之一。很多新算法都在OMPL開發。很多其他軟件（包括ROS/MoveIt）都使用OMPL做運動規划。


1.3. 逆運動學 (Inverse Kinematics)

    • 什么是逆運動學(IK)？簡單說，就是把終端位姿變成關節角度，q=IK(p)。p是終端位姿(xyz)，q是關節角度。
      cd path_to_catkin_ws/src
catkin_create_pkg lwr_description
cd lwr_description
mkdir urdf
      將下載好的lwr_simplified.urdf放入urdf文件夾中，這樣一個urdf package便創建好了。

      2.2 MoveIt!配置助手 (MoveIt! Setup Assistant)
      2.2.1 打開MoveIt! Setup Assistant       
      roslaunch moveit_setup_assistant setup_assistant.launch
         
      moveit_setup_assistant
      MoveIt! Setup Assistant 是一個圖形界面，可以讓我們不用寫代碼看代碼，直接用鼠標點擊就可以配置機器人的運動規划所需要的信息。點擊Create New MoveIt Configuration Package來創建新的配置包，選擇剛剛下載的urdf，然后點擊Load Files 載入文件。

      2.2.2 創建碰撞免檢矩陣(ACM)

      點擊Setup Assisant的左邊第二項'Self-Collisions'，在這里我們將創建碰撞免檢矩陣(Avoid Collision Matrix, ACM)。再次強調，怎么創建很簡單，點擊一下'Regenerate Default Collision Matrix'就可以了，問題是，為什么？ACM是做什么的？
      我們知道，碰撞檢測是非常復雜的運算過程。對於多關節機械臂或者類人機器人來說，機械結構復雜，肢體多，碰撞檢測 需要涉及很多的空間幾何計算。但是對於剛體機器人來說，有些肢體之間是不可能發生碰撞的，比如原本就相鄰的肢體，比如類人機器人的腳和頭。這里生成的 ACM就是告訴我們，這個URDF所描述的機器人，哪些肢體之間是不會發生碰撞的。那么在之后的碰撞檢測算法中，我們就可以略過對這些肢體之間的檢測，以 提高檢測效率。

      2.2.3 創建虛擬關節 (Virtual Joints)
      在Setup Assistant 第三項Virtual Joints里面，我們要創建所謂的虛擬關節。這個虛擬關節，可以理解為一個連接機器人和世界的關節。

      一般來說，Virtual Joint Name我們命名為‘world_joint’，而'Child Link'指的是我們要把‘世界’和機器人的那個部位連接，那么很顯然我們選擇基座'base'。‘Parent Frame Name’，是世界坐標的名字，在ROS中一般叫'world_frame'。關節類型 Joint Type, 很顯然這里我們選擇固定Fixed. 代表機器人相對於世界是固定的。而另外兩種, Planar指的是平面移動底座(xy平面+角度)，用於移動機器人比如PR2；
       
      還有一種Floating, 指的是浮動基座(xyz position+orientation)，比如類人機器人。
       
      2.2.4 創建規划群 (Planning Groups)
      創建Planning Group是MoveIt的核心之一。首先，點擊Add Group, 我們會看到一個界面，如下圖，

      那么這些都是什么呢？
      • Group Name: 不用多說，名字。。。我們就叫Arm。
      • Kinematic Solver: 運動學求解工具，這個就是負責求解正向運動學(Forward Kinematics)和逆運動學(IK, 見1.3節)的。 一般我們選用KDL， The Kinematics and Dynamics Library。這是一個運動學與動力學的庫，可以很好的解決6自由度以上的單鏈機械結構的正逆運動學問題。當然你也可以用其他IK Solver, 比如SRV或者IK_FAST，甚至你可以自己開發新的Solver然后插入進來，如果有空，我以后會發帖講解如何創建新的運動學求解庫並插入到MoveIt。
      • Kin. Search Resolution: 關節空間的采樣密度
      • Kin. Search TImeout: 求解時間
      • Kin. Solver Attempts: 求解失敗嘗試次數，一般來說這三項使用默認值就可以。你也可以根據具體需要做出適當調整。
      接下來，我們要正式創建這個組群，有很多不同的方法，Add Joints, Add Links, Add Kin. Chain, Add Subgroups。我們這里選擇'Add Kin. Chain'，這樣我們可以清楚的看到整個機器人的機械機構，

      在正中方我們可以看到這個機械臂的結構，一個link接着一個 link。下方我們可以看到有'Base Link'和'Tip Link'，我們選擇'lwr_arm_0_link'作為Base，選擇'lwr_arm_7_link'作為Tip. 然后點擊Save，這樣一個規划組群就創建好了。同樣的，我們可以再創建一個手的組群(Hand)，這一次我們用Add Links，然后選擇'lwr_arm_7_link'。


      2.2.5 創建機器人預設位姿 (Robot Poses)
      在Setup Assistant 第五項, ‘Robot Poses’，我們創建預設的機器人位姿。點擊‘Add Pose’，我們為機械臂創建一個向上直立的位姿UpRight，選擇Planning Group為Arm。可以看到很多滾動條，全設為0就是垂直向上的位姿。然后點擊保存。當然，你可以根據需要設置其他不同位姿。

      2.2.6 配置終端控制器(End Effectors)
      終 端控制器，就是機械臂的手，以后用來在工作環境中直接控制的部位。我們添加一個叫做HandEff的終端控制器，End Effector Group選擇之前創建好的Hand，Parent_Link選擇機械臂的最后一個肢體lwr_arm_7_link。Parent Group選擇Arm。

      2.2.7 配置被動關節(Passive Joints)
      所謂被動關節，就是指現實中不配置電機的關節，也就是不會出現在機器人的Joint State Msg里，以避免MoveIt與JointState出現匹配錯誤。這里我們的LWR機械臂並沒有此類被動關節，所以可以直接跳過。

      2.2.8 生成配置文件(Configuration Files)
      最后一步，在Configuration Package Save Path里面選擇一個保存地址，一般我們把他放在path_to_catkin_ws/src/lwr_moveit_config然后點擊Generate Package，這樣一個完整的MoveIt Configuration Package就創建好了！先不要急着運行，我們先來看看都生成了哪些東西，還有一些重要的配置參數都是在哪定義的。

      三. MoveIt 配置包詳解
      打開剛剛創建好的lwr_moveit_config文件夾，我們發現有config和launch兩個文件夾。3.1 MoveIt! 配置文件先看config，里面有
      • fake_controllers.yaml：這是虛擬控制器配置文件，方便我們在沒有實體機器人，甚至沒有任何模擬器（如gazebo）開啟的情況下也能運行MoveIt。
      • joint_limits.yaml：這里記錄了機器人各個關節的位置速度加速度的極限，這些都會被用於以后的規划中。
      • kinematics.yaml：這里就是上一章2.2.4里面設置的東西，用於初始化運動學求解庫
      • lwr.srdf：這個是一個重要的MoveIt配置文件，我們將在下一節詳解。
      • ompl_planning.yaml：這里是配置OMPL各種算法的各種參數。
      3.2 SRDF文件
      SRDF是moveit的配置文件，配合URDF使用。打開lwr.srdf，
      我 們可以看到這是一個xml格式的配置文件，根是robot，並有一個屬性值name='lwr'。下面各個項目應該很明顯，就是我們剛剛在Setup Assistant里面所設置的東西，包含了組群，位姿，終端控制器，虛擬關節，以及碰撞免測矩陣ACM的定義。理論上，只要有了srdf和urdf，我 們就可以完全定義一個機器人moveit信息。

      3.3 Launch文件
      下面，我們看看launch文件夾，一打開發現有很多文件，瞬間不想看了。。不要急，我們來看看幾個重要的文件。

      3.3.1 demo.launch
      demo是運行的總結點，打開我們可以看到他include了其他的launch文件。其中第14行說，如果有需 要，發布靜態的tf。比如說，你的機器人基座不在世界坐標的原點，你可以發布一個靜態tf來描述機器人在世界坐標中的位置。第17-21行，就是我們發布 虛擬機器人狀態的地方了，當然，如果你有實體機器人或者有gazebo之類的模擬器，你需要去掉這一部分，有其他相應的節點來發布機器人狀態。26-32 行運行了另一個moveit重要的節點，move group。

      3.3.2 move_group.launch
      顧名思義，move group的功能是讓一個規划組群動起來。怎么動，那就要做運動規划了，在move_group.launch第24-26行定義了運動規划庫的使用，我 們可以看到，默認的是使用ompl運動規划庫。同樣的，如果以后有時間，我會發帖詳解如何創建新的運動規划庫插件並讓moveit使用其他的運動規划算 法。其他的都是設置一些基本參數，暫時可以略過。

      3.3.3 planning_context.launch
      這里我們可以看到，定義了所使用的urdf和srdf文件，以及運動學求解庫。不建議手動更改這些，但是如果你需要使用不同的urdf，srdf，可以在這里更改。

      3.3.4 setup_assistant.launch
      如果你需要更改一些配置，那么可以直接運行

      復制代碼       
      roslaunch lwr_moveit_config setup_assistant.launch
      這樣就可以基於當前設置做更改，而不是重新設置。

      四. 運行MoveIt!

      4.1 Launch Demo
      現在我們可以來嘗試運行moveit了！

      復制代碼       
      roslaunch lwr_moveit_config demo.launch
       
      等待幾秒，當看到 All is well! Everyone is happy! You can start planning now! 的時候，就代表啟動成功了。我們可以看到一個Rivz窗口，左下角有一個運動規划MotionPlanning模塊。

      第一個進入視野的就是Planning Library, OMPL。沒錯，這里告訴你當前用的是OMPL運動規划算法。在中間的下來菜單里面有很多的具體算法，之后你可以嘗試不同的算法，看看他們的區別。
       
      4.2 選擇目標位姿
      如果如上文第二章中設置，你會在rviz主窗口中看到一個互動標記位於機械臂終端位置。移動這個標記到另外一個地方，你可以看到一個橙色的目標位姿( 每一次移動標記，就運行了一次逆運動學IK求解過程)。
      同樣的，你也可以在MotionPlanning模塊下的Planning子模塊寫的Query子模塊里面設置隨機的或者預設的目標位置。
      4.3 運動規划終於，到了運動規划的時候了。。在Planning子模塊中單擊 

      [ INFO] [1453481861.884163555]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1453481861.884336258]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1453481861.884489778]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1453481861.884523826]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1453481861.884547702]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1453481861.884564358]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1453481861.884587404]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1453481861.884604829]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1453481861.884626253]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1453481861.905034917]: LBKPIECE1: Created 99 (46 start + 53 goal) states in 88 cells (45 start (45 on boundary) + 43 goal (43 on boundary))
[ INFO] [1453481861.905633020]: LBKPIECE1: Created 87 (33 start + 54 goal) states in 76 cells (31 start (31 on boundary) + 45 goal (45 on boundary))
[ INFO] [1453481861.913846457]: LBKPIECE1: Created 126 (76 start + 50 goal) states in 115 cells (75 start (75 on boundary) + 40 goal (40 on boundary))
[ INFO] [1453481861.914639489]: LBKPIECE1: Created 220 (72 start + 148 goal) states in 201 cells (70 start (70 on boundary) + 131 goal (131 on boundary))
[ INFO] [1453481861.948016518]: ParallelPlan::solve(): Solution found by one or more threads in 0.063719 seconds
      我們可以看到，本次運動規划使用了LBKPIECE算法，並且使用了4線程並行規划，規划時間為1 
      (noname)+++++
* obs1.dae
1
mesh
24 12
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 0.0315993
0 1 2
1 3 2
4 5 6
4 7 5
8 9 10
11 8 10
12 13 14
12 14 15
16 17 18
19 16 18
20 21 22
20 22 23
0.7 0 0.8
0.706825 0 0 0.707388
0.5 0 0 1
* obs2.dae
1
mesh
24 12
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 0.0315993
0 1 2
1 3 2
4 5 6
4 7 5
8 9 10
11 8 10
12 13 14
12 14 15
16 17 18
19 16 18
20 21 22
20 22 23
0.6 0.5 0.8
0.706825 0 0 0.707388
0.5 0 0 1
* obs3.dae
1
mesh
24 12
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 0.0315993
0 1 2
1 3 2
4 5 6
4 7 5
8 9 10
11 8 10
12 13 14
12 14 15
16 17 18
19 16 18
20 21 22
20 22 23
0.3 0 0.8
0.999784 0 0 0.0207948
0.5 0 0 1
* table.dae
1
mesh
24 12
-0.420619 0.0420619 -0.841237
-0.420619 -0.0420619 -0.841237
-0.420619 0.0420619 0.841237
-0.420619 -0.0420619 0.841237
0.420619 0.0420619 0.841237
0.420619 -0.0420619 -0.841237
0.420619 0.0420619 -0.841237
0.420619 -0.0420619 0.841237
-0.420619 0.0420619 0.841237
-0.420619 -0.0420619 0.841237
0.420619 -0.0420619 0.841237
0.420619 0.0420619 0.841237
0.420619 -0.0420619 -0.841237
-0.420619 -0.0420619 -0.841237
-0.420619 0.0420619 -0.841237
0.420619 0.0420619 -0.841237
-0.420619 -0.0420619 0.841237
-0.420619 -0.0420619 -0.841237
0.420619 -0.0420619 -0.841237
0.420619 -0.0420619 0.841237
0.420619 0.0420619 -0.841237
-0.420619 0.0420619 -0.841237
-0.420619 0.0420619 0.841237
0.420619 0.0420619 0.841237
0 1 2
1 3 2
4 5 6
4 7 5
8 9 10
8 10 11
12 13 14
15 12 14
16 17 18
19 16 18
20 21 22
20 22 23
0.7 0 0.63
0.706825 0 0 0.707388
0 0.5 0.5 1
* target
1
mesh
24 12
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 0.0315993
0 1 2
1 3 2
4 5 6
4 7 5
8 9 10
11 8 10
12 13 14
12 14 15
16 17 18
19 16 18
20 21 22
20 22 23
0.6 0.2 0.8
0.706825 0 0 0.707388
0 1 0 1
.
      然后在SceneObjects模塊中點擊ImportFromText，選擇剛剛創建的demo.scene文件，一個簡單的桌面環境就被導入進了rviz。你可以通過選擇各個物體，來調整他們的位置。
      回到Context子模塊，點擊Publish Current Scene，將當前的環境發布出去。
      然后再次點擊Plan，你會看到一條不同的軌跡，這一軌跡應該繞過所有障礙物並且達到目標位姿。
       
      因為OMPL是采樣算法，由於其隨機采樣的特性，每次的路徑是不同的，如下圖。而且有可能失敗。
       
      接下來可以嘗試不同的OMPL算法，不同的目標位姿和不同的環境。來看看MoveIt的魯棒性如何。


      那么，MoveIt!和OMPL的運動規划就差不多講完了，當然這是很淺顯的，與實用性的東西都還有距離。以上都是純手打，現做的例子，希望有所幫助。接下來該講什么，你們可以在下面留言，是更深入的講MoveIt!，還是講OMPL運動規划算法，還是講如何模擬具體實例。


免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。



猜您在找 運動（motion）規划、路徑（path）規划和軌跡（trajectory）規划之區別 如何用MoveIt快速搭建機器人運動規划平台？ Motion Planning 是什么 【Unity筆記】根骨骼動畫/運動（Root Motion） 運動規划——PT and PVT 模式 運動規划/路徑規划/軌跡規划區別與聯系 機械臂正逆運動學及軌跡規划基礎 44. 通配符匹配 （動態規划法、貪婪算法） ros moveit 安裝 MoveIt! 源安裝
 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM