- Motion Planning Library
V-REP 從3.3.0開始，使用運動規劃庫OMPL作為插件，通過調用API的方式代替以前的方法進行運動規劃（The old path/motion planning functionality is still functional for backward compatibility and available, but it is recommended not to use it anymore），這樣更具靈活性。
運動規劃就是在給定的位置Start與位置Goal之間為機器人找到一條符合限制條件的路徑。具體的案例可以是為移動機器人規劃出到達指定地點的最短距離，或者是為機械臂規劃出一條無碰撞的運動軌迹，進而實作物體抓取等。目前運動規劃的研究取得長足進展，栅格法、 路标法、 人工勢場法等全局或局部規劃算法被相繼提出。随着研究的深入，基本的運動規劃問題不斷擴充，空間的次元也随之增加。但基于确定性空間的精确解法是以巨大的存儲空間和計算量為代價的，僅僅在低維空間或者特殊條件下才可能存在精确的解法。在高維空間面前，運動規劃的解析算法面臨極大的困難。
Motion planning task from Start to Goal while avoiding obstacles and joint limits
傳統方法的問題在于，始終無法避免在一個确定性空間内對障礙物進行确定的模組化和描述，而這對于複雜環境和高維空間很難做到。基于采樣的運動規劃方法在這一點與傳統方法有本質的不同，它僅僅通過對位形空間或狀态空間中的采樣點進行碰撞檢測來擷取障礙物資訊，并在此基礎上進行運動規劃。采樣規劃方法避免了對空間的模組化，即對障礙空間和自由空間進行描述的複雜的前期計算，這使它完全能夠處理高維空間和複雜系統的運動規劃問題。 應用最廣的兩種基于随機采樣的運動規劃方法是：機率路标法 (Probabilistic Road Maps, PRM) 和快速探索随機樹法(Randomly Exploring Randomized Trees, RRT)。OMPL是一個運動規劃的C++開源庫，其包含了很多運動規劃領域的前沿算法，總體來說OMPL主要是一個采樣規劃的算法庫（OMPL specializes in sampling-based motion planning）
- 基于采樣的運動規劃
運動規劃算法通常有兩個評價名額：
- 完備性Complete：利用該算法，在有限時間内能解決所有有解問題；
- 最優性Optimality：利用該算法，能找到最優路徑（路徑最短、能量最少等）；
PRM是一種基于圖搜尋的方法，它将連續空間轉換成離散空間，再利用A*等搜尋算法在路線圖上尋找路徑。這種方法能用相對少的随機采樣點來找到一個解，對多數問題而言，相對少的樣本足以覆寫大部分可行的空間，并且找到路徑的機率為1（随着采樣數增加，P（找到一條路徑）指數的趨向于1）。顯然，當采樣點太少，或者分布不合理時，PRM算法是不完備的，但是随着采用點的增加，也可以達到完備。是以PRM是機率完備且不最優的。
大緻步驟如下面幾幅圖所示：
除了基于圖搜尋的方法，還有另外一大類基于樹狀結構的搜尋算法，其中最著名的就是快速擴充随機樹法（Randomly Exploring Randomized Trees，RRT）了。RRT算法是從起始點開始向外拓展一個樹狀結構，而樹狀結構的拓展方向是通過在規劃空間内随機采點确定的。與PRM類似，該方法是機率完備且不最優的。
雖然基于采樣的規劃算法速度很快，但他們也有缺點，那就是由随機采樣引入的随機性。利用RRT和PRM算法進行運動規劃，使用者無法對規劃結果進行預判，每次規劃的結果都不一樣，缺乏可重複性。與解析算法的确定結果不同，随機方法對同一個規劃問題的表現可能時好時壞，連續出現完全相同的規劃結果的機率很低。要判斷算法對于某一規劃問題的效果，往往需要多次反複的試驗。基于随機采樣的運動規劃方法無法預測下一次規劃是否能夠成功，更無法預測下一次的路徑或者軌迹的參數。
- V-REP OMPL plugin
V-REP中路徑規劃通常需要以下變量或參數作為輸入：
- a collidable or measurable robot entity: this entity represents the device that should avoid obstacles and is referred to hereafter as robot.
- a start dummy: the start dummy represents the initial configuration (or start configuration)of the robot. The robot entity should be built on top of the start dummy. Make sure that the center of the robot approximately matches the position of the start dummy. 即需要建立start dummy（相當于一個marker）用于标記機器人的初始構型/位姿。
- a goal dummy: the goal dummy represents the desired configuration(or goal configuration) of the robot. The path planning algorithms will try to move the start dummy towards the goal dummy while avoiding collisions between the robot and obstacle.
- a collidable or measurable obstacle entity: this entity represents the obstacles that the robot should avoid, while following a path from the start to the goal configuration.
即進行路徑規劃需要已知：機器人，機器人的初始狀态，機器人的目标狀态以及周圍環境（障礙物）。
V-REP中進行運動/路徑規劃時，可以按照以下幾步進行：
（1）調用simExtOMPL_createTask函數建立一個規劃任務；
（2）調用simExtOMPL_setAlgorithm函數選擇一個合适的算法；
（3）調用simExtOMPL_createStateSpace和simExtOMPL_setStateSpace建立所需的狀态空間（State space: The parameter space for the robot. This space represents all possible configurations of the robot in the workspace. A single point in the state space is a state）；
（4）調用simExtOMPL_setCollisionPairs設定不應與環境發生碰撞的對象（Specify which entities are not allowed to collide）；
（5）調用simExtOMPL_setStartState和simExtOMPL_setGoalState設定機器人的初始狀态以及目标狀态；
（6）調用simExtOMPL_compute計算路徑；調用simExtOMPL_destroyTask删除路徑規劃任務（Note: state space components created during simulation are automatically destroyed when simulation ends）。
注意OMPL計算出的路徑隻是無數多解裡面的一個，無法保證這個解最優，是以可以重複計算多次，選擇一條更好/短的路徑。The path provided by simExtOMPL_compute is usually just one path among an infinite number of other possible paths, and there is no guarantee that the returned path is the optimal solution. For that reason it is common to compute several different paths, then to select the better one (e.g. the shorter one).
下面是一個2維平面内路徑規劃的例子：如下圖所示的L型物體，從起始位置開始要通過長寬為1m×1m的迷宮并避免與障礙物發生碰撞，最終到達目标位置。
在V-REP自帶的例子3DoFHolonomicPathPlanning.ttt中可以找到這個模型，下面是其lua代碼:
1 visualizePath = function(path)
2 if not _lineContainer then
3 -- simAddDrawingObject: Adds a drawing object that will be displayed in the scene
4 -- Lua parameters:
5 --sim_drawing_lines:items are pixel-sized lines(6 values per item (x0;y0;z0;x1;y1;z1) + auxiliary values)
6 --size: size of the item (width of lines or size of points are in pixels, other sizes are in meters
7 --parentObjectHandle: handle of the scene object where the drawing items should keep attached to. if the scene
8 --object moves, the drawing items will also move,-1 if the drawing items are relative to the world (fixed)
9 --maxItemCount: maximum number of items this object can hold.
10 --ambient_diffuse: default ambient/diffuse color (pointer to 3 rgb values). Can be NULL
11 _lineContainer = simAddDrawingObject(sim_drawing_lines, 3, 0, -1, 99999, {0.2,0.2,0.2})
12 end
13
14 -- Adds an item (or clears all items) to a previously inserted drawing object.
15 -- itemData: data relative to an item. If the item is a point item, 3 values are required (x;y;z).
16 -- If the item is a line item, 6 values are required, If nil the drawing object is emptied of all its items
17 simAddDrawingObjectItem(_lineContainer, nil) -- clear all items
18
19 if path then
20 local pc = #path / 3
21 for i=1, pc - 1,1 do
22 lineDat = {path[(i-1)*3+1], path[(i-1)*3+2], initPos[3], path[i*3+1], path[i*3+2], initPos[3]}
23 simAddDrawingObjectItem(_lineContainer, lineDat)
24 end
25 end
26 end
27 ---------------------------------------------------------------------------------------------------------------------
28
29 robotHandle = simGetObjectHandle('StartConfiguration')
30 targetHandle = simGetObjectHandle('GoalConfiguration')
31
32 initPos = simGetObjectPosition(robotHandle, -1) --Specify -1 to retrieve the absolute position
33 initOrient = simGetObjectOrientation(robotHandle, -1)
34
35 -- Create a task object, used to represent the motion planning task
36 -- Lua synopsis: number taskHandle = simExtOMPL_createTask(string name)
37 t = simExtOMPL_createTask('t')
38
39 -- Create a component of the state space for the motion planning problem
40 -- stateSpaceHandle=simExtOMPL_createStateSpace(name,type,objectHandle,boundsLow,boundsHigh,useForProjection)
41 ss = {simExtOMPL_createStateSpace('2d',sim_ompl_statespacetype_pose2d,robotHandle,{-0.5,-0.5},{0.5,0.5},1)}
42
43 -- Set the state space of this task object
44 -- Lua synopsis: number result=simExtOMPL_setStateSpace(number taskHandle, table stateSpaceHandles)
45 simExtOMPL_setStateSpace(t, ss)
46
47 -- Set the search algorithm for the specified task. Default algorithm used is KPIECE1
48 -- Lua synopsis: number result=simExtOMPL_setAlgorithm(number taskHandle, number algorithm)
49 simExtOMPL_setAlgorithm(t,sim_ompl_algorithm_RRTConnect)
50
51
52 -- Set the collision pairs for the specified task object.
53 -- Lua synopsis: number result=simExtOMPL_setCollisionPairs(number taskHandle, table collisionPairHandles)
54 --[[
55 Lua parameters:
56 collisionPairHandles: a table containing 2 entity handles for each collision pair. A collision pair is represented
57 by a collider and a collidee, that will be tested against each other. The first pair could be used for robot
58 self-collision testing, and a second pair could be used for robot-environment collision testing. The collider can
59 be an object or a collection handle. The collidee can be an object or collection handle, or sim_handle_all, in which
60 case the collider will be checked agains all other collidable objects in the scene.
61 --]]
62 simExtOMPL_setCollisionPairs(t,{simGetObjectHandle('L_start'),sim_handle_all})
63
64 startpos = simGetObjectPosition(robotHandle, -1)
65 startorient = simGetObjectOrientation(robotHandle, -1)
66 startpose={startpos[1], startpos[2], startorient[3]}
67
68 -- Set the start state for the specified task object
69 -- Lua synopsis: number result=simExtOMPL_setStartState(number taskHandle, table state)
70 -- state: a table of numbers, whose size must be consistent with the robot's state space specified in this task object
71 simExtOMPL_setStartState(t, startpose)
72
73 goalpos = simGetObjectPosition(targetHandle,-1)
74 goalorient = simGetObjectOrientation(targetHandle,-1)
75 goalpose = {goalpos[1], goalpos[2], goalorient[3]}
76
77 -- Set the goal state for the specified task object
78 simExtOMPL_setGoalState(t, goalpose)
79
80 -- Use OMPL to find a solution for this motion planning task.
81 -- number result,table states=simExtOMPL_compute(number taskHandle,number maxTime,number maxSimplificationTime=-1,number stateCnt=0)
82 -- return value: states--a table of states, representing the solution, from start to goal. States are specified linearly.
83 r, path = simExtOMPL_compute(t, 4, -1, 800)
84
85 visualizePath(path)
86 ----------------------------------------------------------------------------------------------------------------------------------
87
88 -- Simply jump through the path points, no interpolation here:
89 for i=1, #path-3, 3 do -- count from 1 to len(path)-3 with increments of 3
90 pos={path[i], path[i+1], initPos[3]}
91 orient={initOrient[1], initOrient[2], path[i+2]}
92 simSetObjectPosition(robotHandle, -1, pos)
93 simSetObjectOrientation(robotHandle, -1, orient)
94 simSwitchThread()
95 end 下面做一些補充說明：
（1）函數simExtOMPL_createStateSpace的參數type表示建立的狀态空間的類型。type的值可以為：
sim_ompl_statespacetype_position2d：二維平面内的移動（X，Y方向移動）
sim_ompl_statespacetype_pose2d：二維平面内的移動加轉動（X，Y方向移動和繞Z軸的轉動）
sim_ompl_statespacetype_position3d：三維空間内的移動（X，Y，Z方向上的移動）
sim_ompl_statespacetype_pose3d：三維空間内的移動加轉動（X，Y，Z方向上的移動和繞X，Y，Z軸的轉動）
sim_ompl_statespacetype_joint_position：關節空間内的運動
上面的例子中“機器人”可以進行在平面上移動和轉動，是以參數選為sim_ompl_statespacetype_pose2d
（2）函數simExtOMPL_createStateSpace的參數boundsLow和boundsHigh為搜尋空間範圍的下限和上限，例子中搜尋範圍的下限為{Xmin,Ymin}，上限為{Xmax,Ymax}。将搜尋限制在有限的區域内有助于減少運算量。It is important to appropriately limit the search area in order to: (1) reduce calculation times, and (2) make the path planning task realizable.
（3）這個例子中我們隻考慮機器人與其外界環境發生碰撞的可能。如果考慮機器人在運動過程中自身部件之間可能發生的碰撞，則需要添加其它collision pairs，如motionPlanningAndGraspingDemo中的例子：
-- 2 collision pairs: the first for robot self-collision detection, the second for robot-environment collision detection:
collisionPairs={simGetCollectionHandle('Mico'),simGetCollectionHandle('Mico'),simGetCollectionHandle('Mico'),sim_handle_all} （4）要選擇合适的算法，可以參考：Available Planners
RRT Connect (RRTConnect)是RRT算法的一種改進：This planner is a bidirectional version of RRT (i.e., it grows two trees. The basic idea is to grow two RRTs, one from the start and one from the goal, and attempt to connect them). It usually outperforms the original RRT algorithm.
（5）增大計算步長可以降低運算量，但可能導緻機器人跳過障礙物，出現下圖所示的這種情況：
V-REP軟體自帶的模型3DoFHolonomicPathPlanning-stateValidationCallback.ttt與上面的例子大緻相同，不同的一點是使用simExtOMPL_setStateValidationCallback設定自定義的函數，驗證路徑上點的有效性。
stateValidation=function(state)
-- Read the current state:
local p=simGetObjectPosition(robotHandle,-1)
local o=simGetObjectOrientation(robotHandle,-1)
-- Apply the provided state:
simSetObjectPosition(robotHandle,-1,{state[1],state[2],p[3]})
simSetObjectOrientation(robotHandle,-1,{o[1],o[2],state[3]})
-- Test the state. Allowed are states where the robot is at least
-- 'minDistance' away from obstacles, and at most 'maxDistance':
-- simCheckDistance: Checks the minimum distance between two entities.
-- number result,table_7 distanceData=simCheckDistance(number entity1Handle,number entity2Handle,number threshold)
-- threshold: if distance is bigger than the threshold, the distance is not calculated and return value is 0
-- result: 1 if operation was successful (1 if distance is smaller than threshold)
-- distanceData is nil if result is different from 1
-- distanceData[7] is the distance between the entities
local res,d=simCheckDistance(collisionPairs[1],collisionPairs[2], maxDistance)
local pass= (res==1) and (d[7]>minDistance)
-- Following to visualize distances of the states that are valid:
if pass then
simAddDrawingObjectItem(cont,d) -- drawing_lines, 6 values per item (d1;d2;d3;d4;d5;d6)
end
-- Following to debug:
-- if pass then
-- simSwitchThread()
-- end
-- Restore the current state:
simSetObjectPosition(robotHandle,-1,p)
simSetObjectOrientation(robotHandle,-1,o)
-- Return whether the tested state is valid or not:
return pass
end
visualizePath=function(path)
if not _lineContainer then
_lineContainer=simAddDrawingObject(sim_drawing_lines,3,0,-1,99999,{0.2,0.2,0.2})
end
simAddDrawingObjectItem(_lineContainer,nil)
if path then
local pc=#path/3
for i=1,pc-1,1 do
lineDat={path[(i-1)*3+1],path[(i-1)*3+2],initPos[3],path[i*3+1],path[i*3+2],initPos[3]}
simAddDrawingObjectItem(_lineContainer,lineDat)
end
end
end
maxDistance = 0.05 -- max allowed distance
minDistance = 0.01 -- min allowed distance
cont=simAddDrawingObject(sim_drawing_lines,2,0,-1,99999,{1,0,0})
robotHandle=simGetObjectHandle('StartConfiguration')
targetHandle=simGetObjectHandle('GoalConfiguration')
initPos=simGetObjectPosition(robotHandle,-1)
initOrient=simGetObjectOrientation(robotHandle,-1)
t=simExtOMPL_createTask('t')
ss={simExtOMPL_createStateSpace('2d',sim_ompl_statespacetype_pose2d,robotHandle,{-0.5,-0.5},{0.5,0.5},1)}
simExtOMPL_setStateSpace(t,ss)
simExtOMPL_setAlgorithm(t,sim_ompl_algorithm_RRTConnect)
collisionPairs={simGetObjectHandle('L_start'),sim_handle_all}
--[[
Set a custom state validation. By default state validation is performed by collision checking, between robot's
collision objects and environment's objects. By specifying a custom state validation, it is possible to perform
any arbitrary check on a state to determine wether it is valid or not. Argument to the callback is the state to
validate, and return value must be a boolean indicating the validity of the state --]]
-- Lua synopsis: number result=simExtOMPL_setStateValidationCallback(number taskHandle, string callback)
simExtOMPL_setStateValidationCallback(t,'stateValidation')
startpos=simGetObjectPosition(robotHandle,-1)
startorient=simGetObjectOrientation(robotHandle,-1)
startpose={startpos[1],startpos[2],startorient[3]}
simExtOMPL_setStartState(t,startpose)
goalpos=simGetObjectPosition(targetHandle,-1)
goalorient=simGetObjectOrientation(targetHandle,-1)
goalpose={goalpos[1],goalpos[2],goalorient[3]}
simExtOMPL_setGoalState(t,goalpose)
simSetThreadAutomaticSwitch(false) -- Allows to temporarily forbid thread switches
r,path=simExtOMPL_compute(t,8,-1,800)
simSetThreadAutomaticSwitch(true) -- if true, the thread will be able to automatically switch to another thread
while path do
visualizePath(path)
-- Simply jump through the path points, no interpolation here:
for i=1,#path-3,3 do
pos={path[i],path[i+1],initPos[3]}
orient={initOrient[1],initOrient[2],path[i+2]}
simSetObjectPosition(robotHandle,-1,pos)
simSetObjectOrientation(robotHandle,-1,orient)
simSwitchThread()
end
end View Code
注意child script是一個仿真控制腳本. Each child script represents a small code or program written in Lua allowing handling a particular function in a simulation. Child scripts are attached to (orassociated with) scene objects. Child scripts can be of two different types: non-threaded child scripts or threaded child scripts:
Non-threaded child scripts are pass-through scripts. This means that every time they are called, they should perform some task and then return control. If control is not returned, then the whole simulation halts.
Threaded child scripts are scripts that will launch in a thread. The launch of threaded child scripts is handled by the default main script code, via the simLaunchThreadedChildScripts function, in a cascaded way. When a threaded child script's execution is still underway, it will not be launched a second time. When a threaded child script ended, it can be relaunched only if the execute once item in the script properties is unchecked. By default a threaded child script will execute for about 1-2 milliseconds before automatically switching to another thread. Once the current thread was switched, it will resume execution at next simulation pass (i.e. at a time currentTime+simulationTimeStep). The thread switching is automatic (occurs after the specified time). Some operation should not be interrupted in order to execute correctly (imagine moving several objects in a loop). In that case, you can temporarily forbid thread switches with the simSetThreadAutomaticSwitch function.
6DoFHolonomicPathPlanning是一個三維空間中路徑規劃的例子，如下圖所示，左側物體L_start要調整位置和姿态穿過矩形孔到達goalpose而不與周圍環境發生碰撞。
robotHandle = simGetObjectHandle('Start')
targetHandle = simGetObjectHandle('End')
t = simExtOMPL_createTask('t')
ss = {simExtOMPL_createStateSpace('6d',sim_ompl_statespacetype_pose3d,robotHandle,{-1,-0.5,0},{1,0.5,1},1)}
simExtOMPL_setStateSpace(t, ss)
simExtOMPL_setAlgorithm(t, sim_ompl_algorithm_RRTConnect)
simExtOMPL_setCollisionPairs(t,{simGetObjectHandle('L_start'),sim_handle_all})
startpos = simGetObjectPosition(robotHandle, -1)
startorient = simGetObjectQuaternion(robotHandle, -1)
startpose = {startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}
simExtOMPL_setStartState(t, startpose)
goalpos = simGetObjectPosition(targetHandle, -1)
goalorient = simGetObjectQuaternion(targetHandle, -1)
goalpose = {goalpos[1],goalpos[2],goalpos[3],goalorient[1],goalorient[2],goalorient[3],goalorient[4]}
simExtOMPL_setGoalState(t, goalpose)
r, path = simExtOMPL_compute(t,20,-1,200)
while true do
-- Simply jump through the path points, no interpolation here:
for i=1, #path-7, 7 do
pos = {path[i],path[i+1],path[i+2]}
orient = {path[i+3],path[i+4],path[i+5],path[i+6]}
simSetObjectPosition(robotHandle,-1,pos)
simSetObjectQuaternion(robotHandle,-1,orient)
simSwitchThread()
end
end 可以看出步驟與前面的例子基本一緻。這裡有兩個地方需要注意：1.由于規劃的路徑是三維空間内且涉及位置和姿态的同時調整，是以在設定狀态空間時要選擇sim_ompl_statespacetype_pose3d作為類型參數；2. simGetObjectQuaternion函數傳回的是描述姿态的四元數
參考：
運動規劃最全簡介
機器人運動規劃—百度文庫
Planning Algorithms
Motion planning—Wikipedia
Open Motion Planning Library: A Primer
Questions/Answers around V-REP
四元數（Quaternion）和旋轉
基于随機采樣的運動規劃綜述. 《控制與決策》, 2005, 20(7):721-726