運動規劃 (Motion Planning): MoveIt! 與 OMPL

轉發：http://www.360doc.com/editarticle.aspx?articleid=740247964

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

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

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

一. 基礎概念

首先，我們要了解一些基礎的概念，瞭解各個名詞的意義和區別。

1.1. 運動規劃 (Motion Planning)
我們這裏講的運動規劃，有別於軌跡規劃 (Path Planning)。一般來說，path planning用於無人車/無人機領域，而motion planning主要用於機械臂，類人機器人領域。當然了，這兩者沒有本質的區別，理論上說MoveIt!和OMPL同樣可以用於無人車無人機的規劃，但不免有些殺雞用牛刀的感覺。兩者規劃的空間維度不同，導致他們的難易程度不同。舉例說明，如果不考慮速度加速度，只考慮位置的話，無人車軌跡規劃維度是3 (x,y,和角度), 無人機是6 (x,y,z,和另外3個量確定空間的旋轉角度)。確定3D空間的一個姿勢(pose)需要6個變量，而對於關節數大於6的機械臂結構，它的規劃空間維度就大於6，成爲冗餘系統(redundant system)，從而使規劃問題變得更爲複雜。所謂冗餘系統，就是說，存在多種關節角度配置能夠使得終端達到相同的位姿，存在無數的解。這是達到的最終姿勢有無數個解，那麼如何到達這個最終姿勢，整個運動的軌跡，更是存在無數個解。

既然存在無數的解，那麼問題來了。很明顯，存在兩種不同的方向，一種是找到最好的那個解，另一種是快速的找到一個有效的解。前者，大部分算法使用最優規劃 (Optimization-based Planning)，後者使用採樣規劃 (Sampling-based Planning)。具體的區別和算法，不在這裏贅述。

1.2. 開源運動規劃庫 (OMPL).
接上文，而OMPL (Open Motion Planning Library)，開源運動規劃庫，就是一個運動規劃的C++庫，其包含了很多運動規劃領域的前沿算法。雖然OMPL裏面提到了最優規劃，但總體來說OMPL還是一個採樣規劃算法庫。而採樣規劃算法中，最出名的莫過於 Rapidly-exploring Random Trees (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是關節角度。
爲什麼要用IK？OMPL是採樣算法，也就是要在關節空間採樣。這與無人車的規劃有一個最明顯的區別，無人車的目標就是在採樣空間， e.g. 目標是(x,y), 採樣空間也是(x,y). 但是對於機械臂，目標是終端空間位置(xyz), 但採樣空間卻是關節空間(q0,q1,...qN)。有了IK之後，我們就可以把三維空間的目標p轉化爲關節空間的目標q。那麼這樣就會讓採樣算法能算的更快，具體方法不贅述，這樣的算法有RRT-Connect，BKPIECE等等雙向採樣算法。

1.4. MoveIt!
問：我不想看也看不懂OMPL和各種算法，但是我想讓機械臂動起來，怎麼辦？
答：那這正是MoveIt!的設計初衷。Move It！讓它動起來！
OMPL是運動規劃的“規劃”部分，而MoveIt!是OMPL的ROS接口。當然這不完全準確，OMPL有單獨的ROS接口，但依舊很繁雜，而MoveIt是OMPL ROS接口的接口。。。而且MoveIt!還結合了其他一些功能，總之MoveIt!就是個大接口。。

MoveIt!能做什麼？一句話，MoveIt!就是一個模塊化的接口，讓你在最短時間內，不用自己寫太多代碼，就能配置出一個ROS Package來爲你的機械臂做運動規劃。

二. 創建MoveIt! Package

2.1 準備URDF package
首先我們要準備一個機械臂的urdf，如果你已有URDF，可以使用自己的urdf模型。若手頭沒有現成的URDF，可以從此處下載一個庫卡LWR簡化模型URDF，這是一個固定底座7自由度的機械臂。
從該連接處依次進入examples/sovlers/ik_solver_demo/resources，下載裏面的lwr_simplified.urdf。

複製代碼

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

複製代碼

1	`roslaunch moveit_setup_assistant setup_assistant.launch`

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。

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
如果你需要更改一些配置，那麼可以直接運行

複製代碼

1	`roslaunch lwr_moveit_config setup_assistant.launch`

這樣就可以基於當前設置做更改，而不是重新設置。

四. 運行MoveIt!

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

複製代碼

1	`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子模塊中單擊Plan，一個運動軌跡就會出現與Rviz窗口中並循環播放。你可以在Display->MotionPlanning->Planned Path裏面設置各種顯示參數。

同時，在terminal裏面我們可以看到一些輸出

複製代碼

[ 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線程並行規劃，規劃時間爲0.063719秒。你也可以在OMPL算法裏選擇其他規劃算法比如RRT，RRT-Connect，PRM，EST等等等等。。看看他們之間的軌跡區別和速度區別。

4.4 避障運動規劃 Collision Free Motion Planning

上面這個簡單的運動規劃，環境中並沒有障礙物。下面我們來爲環境中增加幾個物體。創建一個demo.scene文件，將以下代碼複製進去（你可以自己設計不同的scene）

複製代碼

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

(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運動規劃算法，還是講如何模擬具體實例。