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

時間 2019-11-10

標籤 rep 學習筆記機器人路徑規劃简体版

原文原文鏈接

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），這樣更具靈活性。php

　　運動規劃就是在給定的位置Start與位置Goal之間爲機器人找到一條符合約束條件的路徑。具體的案例能夠是爲移動機器人規劃出到達指定地點的最短距離，或者是爲機械臂規劃出一條無碰撞的運動軌跡，從而實現物體抓取等。目前運動規劃的研究取得長足進展，柵格法、路標法、人工勢場法等全局或局部規劃算法被相繼提出。隨着研究的深刻，基本的運動規劃問題不斷擴展，空間的維度也隨之增長。但基於肯定性空間的精確解法是以巨大的存儲空間和計算量爲代價的，僅僅在低維空間或者特殊條件下才可能存在精確的解法。在高維空間面前，運動規劃的解析算法面臨極大的困難。html

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

基於採樣的運動規劃

　　運動規劃算法一般有兩個評價指標：app

完備性Complete：利用該算法，在有限時間內能解決全部有解問題；dom
最優性Optimality：利用該算法，能找到最優路徑（路徑最短、能量最少等）；ide

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

　　大體步驟以下面幾幅圖所示：oop

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

　　雖然基於採樣的規劃算法速度很快，但他們也有缺點，那就是由隨機採樣引入的隨機性。利用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