6DoFHolonomicPathPlanning.ttt
這個scene更簡單，從hierarchy里可以看到，一目了然，而且直接拋棄了可視化路徑，用的函數和上一篇基本上一樣，就是升級到了三維空間，不再是平面的。

直接看下代碼：

function sysCall_threadmain()
    robotHandle=sim.getObjectHandle('Start')
    targetHandle=sim.getObjectHandle('End')
    t=simOMPL.createTask('t')
    --注意這里的Tpye變成了3d
    ss={simOMPL.createStateSpace('6d',simOMPL.StateSpaceType.pose3d,robotHandle,{-1,-0.5,0},{1,0.5,1},1)}
    simOMPL.setStateSpace(t,ss)
    simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRTConnect)
    simOMPL.setCollisionPairs(t,{sim.getObjectHandle('L_start'),sim.handle_all})
    startpos=sim.getObjectPosition(robotHandle,-1)
    startorient=sim.getObjectQuaternion(robotHandle,-1)
    startpose={startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}
    simOMPL.setStartState(t,startpose)
    goalpos=sim.getObjectPosition(targetHandle,-1)
    goalorient=sim.getObjectQuaternion(targetHandle,-1)
    goalpose={goalpos[1],goalpos[2],goalpos[3],goalorient[1],goalorient[2],goalorient[3],goalorient[4]}
    simOMPL.setGoalState(t,goalpose)
    r,path=simOMPL.compute(t,20,-1,200)
    while true do
        -- Simply jump through the path points, no interpolation here:
        --注意這里的步長為7，姿態有四個量，可能是四元數
        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]}
            sim.setObjectPosition(robotHandle,-1,pos)
            sim.setObjectQuaternion(robotHandle,-1,orient)
            sim.switchThread()
        end
    end
end

熟悉的配方，熟悉的味道，熟悉的流程，跑一下，效果如下：

沒有路徑可視化，總覺的有點難受，我們來加個路徑，把上一篇的可視化函數加上，新的代碼：

visualizePath=function(path)
    if not _lineContainer then
        _lineContainer=sim.addDrawingObject(sim.drawing_lines,3,0,-1,99999,{0.2,0.2,0.2})
    end
    sim.addDrawingObjectItem(_lineContainer,nil)
    if path then
        local pc=#path/7
        for i=1,pc-1,1 do
            lineDat={path[(i-1)*7+1],path[(i-1)*7+2],path[(i-1)*7+3],path[i*7+1],path[i*7+2],path[i*7+3]}
            sim.addDrawingObjectItem(_lineContainer,lineDat)
        end
    end
end


function sysCall_threadmain()
    robotHandle=sim.getObjectHandle('Start')
    targetHandle=sim.getObjectHandle('End')
    t=simOMPL.createTask('t')
    ss={simOMPL.createStateSpace('6d',simOMPL.StateSpaceType.pose3d,robotHandle,{-1,-0.5,0},{1,0.5,1},1)}
    simOMPL.setStateSpace(t,ss)
    simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRTConnect)
    simOMPL.setCollisionPairs(t,{sim.getObjectHandle('L_start'),sim.handle_all})
    startpos=sim.getObjectPosition(robotHandle,-1)
    startorient=sim.getObjectQuaternion(robotHandle,-1)
    startpose={startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}
    simOMPL.setStartState(t,startpose)
    goalpos=sim.getObjectPosition(targetHandle,-1)
    goalorient=sim.getObjectQuaternion(targetHandle,-1)
    goalpose={goalpos[1],goalpos[2],goalpos[3],goalorient[1],goalorient[2],goalorient[3],goalorient[4]}
    simOMPL.setGoalState(t,goalpose)
    r,path=simOMPL.compute(t,20,-1,200)
    visualizePath(path)

    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]}
            sim.setObjectPosition(robotHandle,-1,pos)
            sim.setObjectQuaternion(robotHandle,-1,orient)
            sim.switchThread()
        end
    end
end

只需要在路徑坐標點處做一些修改即可，新的效果：

有了路徑舒服多了，可以多跑幾次，每次找到的路徑都不太一樣