V-rep學習筆記：機器人路徑規劃1

•  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）

• 基於取樣的運動規劃

　　運動規劃演算法通常有兩個評價指標：

1. 完備性Complete：利用該演算法，在有限時間內能解決所有有解問題；

2. 最優性Optimality：利用該演算法，能找到最優路徑（路徑最短、能量最少等）；

　　PRM是一種基於圖搜尋的方法，它將連續空間轉換成離散空間，再利用A*等搜尋演算法在路線圖上尋找路徑。這種方法能用相對少的隨機取樣點來找到一個解，對多數問題而言，相對少的樣本足以覆蓋大部分可行的空間，並且找到路徑的概率為1（隨著取樣數增加，P（找到一條路徑）指數的趨向於1）。顯然，當取樣點太少，或者分佈不合理時，PRM演算法是不完備的，但是隨著採用點的增加，也可以達到完備。所以PRM是概率完備且不最優的。

　　大致步驟如下面幾幅圖所示：

　　除了基於圖搜尋的方法，還有另外一大類基於樹狀結構的搜尋演算法，其中最著名的就是快速擴充套件隨機樹法（Randomly Exploring Randomized Trees，RRT）了。RRT演算法是從起始點開始向外擴充一個樹狀結構，而樹狀結構的擴充方向是通過在規劃空間內隨機採點確定的。與PRM類似，該方法是概率完備且不最優的。

　　雖然基於取樣的規劃演算法速度很快，但他們也有缺點，那就是由隨機取樣引入的隨機性。利用RRT和PRM演算法進行運動規劃，使用者無法對規劃結果進行預判，每次規劃的結果都不一樣，缺乏可重複性。與解析演算法的確定結果不同，隨機方法對同一個規劃問題的表現可能時好時壞，連續出現完全相同的規劃結果的概率很低。要判斷演算法對於某一規劃問題的效果，往往需要多次反覆的試驗。基於隨機取樣的運動規劃方法無法預測下一次規劃是否能夠成功，更無法預測下一次的路徑或者軌跡的引數。 

---

•  V-REP OMPL plugin

　　V-REP中路徑規劃通常需要以下變數或引數作為輸入：

1. a collidable or measurable robot entity: this entity represents the device that should avoid obstacles and is referred to hereafter as robot.
2. 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）用於標記機器人的初始構型/位姿。 
3. 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.
4. 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程式碼:

visualizePath = function(path)
    if not _lineContainer then
-- simAddDrawingObject: Adds a drawing object that will be displayed in the scene
-- Lua parameters:
    --sim_drawing_lines:items are pixel-sized lines(6 values per item (x0;y0;z0;x1;y1;z1) + auxiliary values)
    --size: size of the item (width of lines or size of points are in pixels, other sizes are in meters
    --parentObjectHandle: handle of the scene object where the drawing items should keep attached to. if the scene 
            --object moves, the drawing items will also move,-1 if the drawing items are relative to the world (fixed)
    --maxItemCount: maximum number of items this object can hold.
    --ambient_diffuse: default ambient/diffuse color (pointer to 3 rgb values). Can be NULL
        _lineContainer = simAddDrawingObject(sim_drawing_lines, 3, 0, -1, 99999, {0.2,0.2,0.2})
    end

-- Adds an item (or clears all items) to a previously inserted drawing object.
-- itemData: data relative to an item. If the item is a point item, 3 values are required (x;y;z). 
-- If the item is a line item, 6 values are required, If nil the drawing object is emptied of all its items
    simAddDrawingObjectItem(_lineContainer, nil) -- clear all items

    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
---------------------------------------------------------------------------------------------------------------------

robotHandle = simGetObjectHandle('StartConfiguration')
targetHandle = simGetObjectHandle('GoalConfiguration')

initPos = simGetObjectPosition(robotHandle, -1)  --Specify -1 to retrieve the absolute position
initOrient = simGetObjectOrientation(robotHandle, -1)

-- Create a task object, used to represent the motion planning task
-- Lua synopsis: number taskHandle = simExtOMPL_createTask(string name)
t = simExtOMPL_createTask('t')

-- Create a component of the state space for the motion planning problem
-- stateSpaceHandle=simExtOMPL_createStateSpace(name,type,objectHandle,boundsLow,boundsHigh,useForProjection)
ss = {simExtOMPL_createStateSpace('2d',sim_ompl_statespacetype_pose2d,robotHandle,{-0.5,-0.5},{0.5,0.5},1)}

-- Set the state space of this task object
-- Lua synopsis: number result=simExtOMPL_setStateSpace(number taskHandle, table stateSpaceHandles)
simExtOMPL_setStateSpace(t, ss)

-- Set the search algorithm for the specified task. Default algorithm used is KPIECE1
-- Lua synopsis: number result=simExtOMPL_setAlgorithm(number taskHandle, number algorithm)
simExtOMPL_setAlgorithm(t,sim_ompl_algorithm_RRTConnect)


-- Set the collision pairs for the specified task object.
-- Lua synopsis: number result=simExtOMPL_setCollisionPairs(number taskHandle, table collisionPairHandles)
--[[
Lua parameters:
collisionPairHandles: a table containing 2 entity handles for each collision pair. A collision pair is represented
by a collider and a collidee, that will be tested against each other. The first pair could be used for robot 
self-collision testing, and a second pair could be used for robot-environment collision testing. The collider can 
be an object or a collection handle. The collidee can be an object or collection handle, or sim_handle_all, in which 
case the collider will be checked agains all other collidable objects in the scene.
--]]
simExtOMPL_setCollisionPairs(t,{simGetObjectHandle('L_start'),sim_handle_all})

startpos = simGetObjectPosition(robotHandle, -1)  
startorient = simGetObjectOrientation(robotHandle, -1)
startpose={startpos[1], startpos[2], startorient[3]}

-- Set the start state for the specified task object
-- Lua synopsis: number result=simExtOMPL_setStartState(number taskHandle, table state)
-- state: a table of numbers, whose size must be consistent with the robot's state space specified in this task object
simExtOMPL_setStartState(t, startpose)

goalpos = simGetObjectPosition(targetHandle,-1)
goalorient = simGetObjectOrientation(targetHandle,-1)
goalpose = {goalpos[1], goalpos[2], goalorient[3]}

-- Set the goal state for the specified task object
simExtOMPL_setGoalState(t, goalpose)

-- Use OMPL to find a solution for this motion planning task.
-- number result,table states=simExtOMPL_compute(number taskHandle,number maxTime,number maxSimplificationTime=-1,number stateCnt=0)    
-- return value: states--a table of states, representing the solution, from start to goal. States are specified linearly.
r, path = simExtOMPL_compute(t, 4, -1, 800)

visualizePath(path)
----------------------------------------------------------------------------------------------------------------------------------

-- Simply jump through the path points, no interpolation here:
for i=1, #path-3, 3 do   -- count from 1 to len(path)-3 with increments of 3
    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

 　　下面做一些補充說明：

　　（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

　　注意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