我会更新一系列的官方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
根据结果可视化路径，并更新机器人位姿
代码效果如下：
