VREP path planning (1) 3DoFHolonomicPathPlanning 三自由度 路径规划 仿真

我会更新一系列的官方path planning demo注解，一方面最近自己在做路径规划相关的东西，可以作为自己学习的记录和笔记，另一方面因为国内的VREP教程很少，而且有些官方scene里面的代码在Tutorials里面并没有解释，所以值得做一些注解。

官方的一些示例scene都是在C:\Program Files\V-REP3\V-REP_PRO_EDU\scenes下的，File->open scene,打开相应的scene即可。另外，路径规划里用到的OMPL库的API可以在help->help topics里面的API部分找到，很方便。

3DoFHolonomicPathPlanning.ttt

这个scene实现的主要功能是平面上的机器人导航，场景也非常简单，只包括开始，目标和障碍。

直接看Lua代码吧：

visualizePath=function(path)
    if not _lineContainer then
       --设置画图object，参数列表：
       --类型
       --尺寸，line时为像素
       --复制容忍度，0为默认，可以避免画太密集的点
       --画图object的父物体Handle，-1为World
       --画图object中items的最大数量
       --RGB颜色
       --返回画图object 的Handle
       _lineContainer=sim.addDrawingObject(sim.drawing_lines,3,0,-1,99999,{0.2,0.2,0.2})
    end
    --在画图Object中增加一个item，可以理解为在线里增加一根短线，增加很多短线得到最终的线，第二个参数为点的数据
    sim.addDrawingObjectItem(_lineContainer,nil)
    if path then
        --path是一个列表，由于有三维，所以除以3，得到路径中点的数量
        --#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')
    --获取物体位置，第二个参数是relativeToObjectHandle，也是相对于的物体的Handle,-1指绝对位置，也就是获取相对于World的位置
    initPos=sim.getObjectPosition(robotHandle,-1)
    initOrient=sim.getObjectOrientation(robotHandle,-1)
    --创建pathplan任务，参数是任务名字，返回task Handle
    t=simOMPL.createTask('t')
    --创建StateSpace，参数为：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)}
    --设置State Space，t: task Handle , ss: State space Handle 的列表
    simOMPL.setStateSpace(t,ss)
    --设置使用的算法，这里用的是RRTConnect
    simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRTConnect)
    --设置碰撞对，第一个参数是 task Handle，第二个参数是碰撞对Handle列表
    --一个碰撞对列表中有两个物体的Handle，这两个物体被称为"a collider and a collidee"
    --collider 可以是一个object或者一个colliction（可以在VREP的collictions里设置）被用来做机械臂的自我碰撞检测
    --collidee可以是一个object或者一个colliction，或者 sim_handle_all，用来做机械臂与环境的碰撞检测
    simOMPL.setCollisionPairs(t,{sim.getObjectHandle('L_start'),sim.handle_all})
    --这里的代码和上面的initPos其实重复了
    startpos=sim.getObjectPosition(robotHandle,-1)
    startorient=sim.getObjectOrientation(robotHandle,-1)
    startpose={startpos[1],startpos[2],startorient[3]}
    --设置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)
    --计算路径，参数列表：
    -- task Handle
    -- 最大路径搜索时间（秒）超时就停了，会直接给出当前的路径，可能跑不完，如果电脑很慢，可以把这个时间设大一点
    -- 最大简化时间，用于路径平滑的时间，-1为默认
    --返回的最少states数量，可以理解成返回的路径中最少有多少点（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]}
            --姿态：只需要绕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

可以看到，一个完整的路径规划包括如下几个基本要素：
创建task: simOMPL.createTask
创建状态空间：simOMPL.createStateSpace
设置状态空间：simOMPL.setStateSpace
设置使用的算法：simOMPL.setAlgorithm
设置碰撞对： simOMPL.setCollisionPairs
设置初始状态和目标状态：simOMPL.setStartState
simOMPL.setGoalState
求解：simOMPL.compute
根据结果可视化路径，并更新机器人位姿

代码效果如下：