VREP path planning (2) 6DoFHolon

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

只需要在路径坐标点处做一些修改即可，新的效果：

有了路径舒服多了，可以多跑几次，每次找到的路径都不太一样