我會更新一系列的官方path planning demo注解，一方面最近自己在做路徑規(guī)劃相關(guān)的東西，可以作為自己學(xué)習(xí)的記錄和筆記，另一方面因為國內(nèi)的VREP教程很少，而且有些官方scene里面的代碼在Tutorials里面并沒有解釋，所以值得做一些注解。

官方的一些示例scene都是在C:\Program Files\V-REP3\V-REP_PRO_EDU\scenes下的，F(xiàn)ile->open scene,打開相應(yīng)的scene即可。另外，路徑規(guī)劃里用到的OMPL庫的API可以在help->help topics里面的API部分找到，很方便。

3DoFHolonomicPathPlanning.ttt

這個scene實現(xiàn)的主要功能是平面上的機器人導(dǎo)航，場景也非常簡單，只包括開始，目標和障礙。

直接看Lua代碼吧：

visualizePath=function(path)
    if not _lineContainer then
       --設(shè)置畫圖object，參數(shù)列表：
       --類型
       --尺寸，line時為像素
       --復(fù)制容忍度，0為默認，可以避免畫太密集的點
       --畫圖object的父物體Handle，-1為World
       --畫圖object中items的最大數(shù)量
       --RGB顏色
       --返回畫圖object 的Handle
       _lineContainer=sim.addDrawingObject(sim.drawing_lines,3,0,-1,99999,{0.2,0.2,0.2})
    end
    --在畫圖Object中增加一個item，可以理解為在線里增加一根短線，增加很多短線得到最終的線，第二個參數(shù)為點的數(shù)據(jù)
    sim.addDrawingObjectItem(_lineContainer,nil)
    if path then
        --path是一個列表，由于有三維，所以除以3，得到路徑中點的數(shù)量
        --#path path的長度
        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]}
            sim.addDrawingObjectItem(_lineContainer,lineDat)
        end
    end
end


function sysCall_threadmain()
    --平平無奇的獲取物體Handle
    robotHandle=sim.getObjectHandle('StartConfiguration')
    targetHandle=sim.getObjectHandle('GoalConfiguration')
    --獲取物體位置，第二個參數(shù)是relativeToObjectHandle，也是相對于的物體的Handle,-1指絕對位置，也就是獲取相對于World的位置
    initPos=sim.getObjectPosition(robotHandle,-1)
    initOrient=sim.getObjectOrientation(robotHandle,-1)
    --創(chuàng)建pathplan任務(wù)，參數(shù)是任務(wù)名字，返回task Handle
    t=simOMPL.createTask('t')
    --創(chuàng)建StateSpace，參數(shù)為：State Space的名字，類型，物體Handle，上界，下界，useForProjection(bool）
    --返回State Space的Handle，用大括號得到一個Handle的列表
    ss={simOMPL.createStateSpace('2d',simOMPL.StateSpaceType.pose2d,robotHandle,{-0.5,-0.5},{0.5,0.5},1)}
    --設(shè)置State Space，t: task Handle , ss: State space Handle 的列表
    simOMPL.setStateSpace(t,ss)
    --設(shè)置使用的算法，這里用的是RRTConnect
    simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRTConnect)
    --設(shè)置碰撞對，第一個參數(shù)是 task Handle，第二個參數(shù)是碰撞對Handle列表
    --一個碰撞對列表中有兩個物體的Handle，這兩個物體被稱為"a collider and a collidee"
    --collider 可以是一個object或者一個colliction（可以在VREP的collictions里設(shè)置）被用來做機械臂的自我碰撞檢測
    --collidee可以是一個object或者一個colliction，或者 sim_handle_all，用來做機械臂與環(huán)境的碰撞檢測
    simOMPL.setCollisionPairs(t,{sim.getObjectHandle('L_start'),sim.handle_all})
    --這里的代碼和上面的initPos其實重復(fù)了
    startpos=sim.getObjectPosition(robotHandle,-1)
    startorient=sim.getObjectOrientation(robotHandle,-1)
    startpose={startpos[1],startpos[2],startorient[3]}
    --設(shè)置StartState,t是task Handle ,startpose是一個列表，要和StateSpace的維度一致
    simOMPL.setStartState(t,startpose)
    goalpos=sim.getObjectPosition(targetHandle,-1)
    goalorient=sim.getObjectOrientation(targetHandle,-1)
    goalpose={goalpos[1],goalpos[2],goalorient[3]}
    simOMPL.setGoalState(t,goalpose)
    --計算路徑，參數(shù)列表：
    -- task Handle
    -- 最大路徑搜索時間（秒）超時就停了，會直接給出當前的路徑，可能跑不完，如果電腦很慢，可以把這個時間設(shè)大一點
    -- 最大簡化時間，用于路徑平滑的時間，-1為默認
    --返回的最少states數(shù)量，可以理解成返回的路徑中最少有多少點（state），0為默認
    --返回列表：
    --r: 是否得到解，bool
    --path: states,也就是路徑點列表
    r,path=simOMPL.compute(t,4,-1,800)
    while path do
        --可視化該路徑
        visualizePath(path)
        -- Simply jump through the path points, no interpolation here:
        for i=1,#path-3,3 do
            --位置：z不變
            pos={path[i],path[i+1],initPos[3]}
            --姿態(tài)：只需要繞z軸的角度變化
            orient={initOrient[1],initOrient[2],path[i+2]}
            sim.setObjectPosition(robotHandle,-1,pos)
            sim.setObjectOrientation(robotHandle,-1,orient)
            --  Allows specifying the exact moment at which the current thread should switch to another thread.
            --允許指定當前線程切換到另一個線程的確切時間
            sim.switchThread()
        end
    end
end

可以看到，一個完整的路徑規(guī)劃包括如下幾個基本要素：
創(chuàng)建task: simOMPL.createTask
創(chuàng)建狀態(tài)空間：simOMPL.createStateSpace
設(shè)置狀態(tài)空間：simOMPL.setStateSpace
設(shè)置使用的算法：simOMPL.setAlgorithm
設(shè)置碰撞對： simOMPL.setCollisionPairs
設(shè)置初始狀態(tài)和目標狀態(tài)：simOMPL.setStartState
simOMPL.setGoalState
求解：simOMPL.compute
根據(jù)結(jié)果可視化路徑，并更新機器人位姿

代碼效果如下：