V-rep学习笔记：机器人路径规划1

•  Motion Planning Library

　　V-REP 从3.3.0开始，使用运动规划库OMPL作为插件，通过调用API的方式代替以前的方法进行运动规划（The old path/motion planning functionality is still functional for backward compatibility and available, but it is recommended not to use it anymore），这样更具灵活性。

　　运动规划就是在给定的位置Start与位置Goal之间为机器人找到一条符合约束条件的路径。具体的案例可以是为移动机器人规划出到达指定地点的最短距离，或者是为机械臂规划出一条无碰撞的运动轨迹，从而实现物体抓取等。目前运动规划的研究取得长足进展，栅格法、 路标法、 人工势场法等全局或局部规划算法被相继提出。随着研究的深入，基本的运动规划问题不断扩展，空间的维度也随之增加。但基于确定性空间的精确解法是以巨大的存储空间和计算量为代价的，仅仅在低维空间或者特殊条件下才可能存在精确的解法。在高维空间面前，运动规划的解析算法面临极大的困难。

Motion planning task from Start to Goal while avoiding obstacles and joint limits

　　传统方法的问题在于，始终无法避免在一个确定性空间内对障碍物进行确定的建模和描述，而这对于复杂环境和高维空间很难做到。基于采样的运动规划方法在这一点与传统方法有本质的不同，它仅仅通过对位形空间或状态空间中的采样点进行碰撞检测来获取障碍物信息，并在此基础上进行运动规划。采样规划方法避免了对空间的建模，即对障碍空间和自由空间进行描述的复杂的前期计算，这使它完全能够处理高维空间和复杂系统的运动规划问题。 应用最广的两种基于随机采样的运动规划方法是：概率路标法 (Probabilistic Road Maps, PRM) 和快速探索随机树法(Randomly Exploring Randomized Trees, RRT)。OMPL是一个运动规划的C++开源库，其包含了很多运动规划领域的前沿算法，总体来说OMPL主要是一个采样规划的算法库（OMPL specializes in sampling-based motion planning）

• 基于采样的运动规划

　　运动规划算法通常有两个评价指标：

1. 完备性Complete：利用该算法，在有限时间内能解决所有有解问题；

2. 最优性Optimality：利用该算法，能找到最优路径（路径最短、能量最少等）；

　　PRM是一种基于图搜索的方法，它将连续空间转换成离散空间，再利用A*等搜索算法在路线图上寻找路径。这种方法能用相对少的随机采样点来找到一个解，对多数问题而言，相对少的样本足以覆盖大部分可行的空间，并且找到路径的概率为1（随着采样数增加，P（找到一条路径）指数的趋向于1）。显然，当采样点太少，或者分布不合理时，PRM算法是不完备的，但是随着采用点的增加，也可以达到完备。所以PRM是概率完备且不最优的。

　　大致步骤如下面几幅图所示：

　　除了基于图搜索的方法，还有另外一大类基于树状结构的搜索算法，其中最著名的就是快速扩展随机树法（Randomly Exploring Randomized Trees，RRT）了。RRT算法是从起始点开始向外拓展一个树状结构，而树状结构的拓展方向是通过在规划空间内随机采点确定的。与PRM类似，该方法是概率完备且不最优的。

　　虽然基于采样的规划算法速度很快，但他们也有缺点，那就是由随机采样引入的随机性。利用RRT和PRM算法进行运动规划，用户无法对规划结果进行预判，每次规划的结果都不一样，缺乏可重复性。与解析算法的确定结果不同，随机方法对同一个规划问题的表现可能时好时坏，连续出现完全相同的规划结果的概率很低。要判断算法对于某一规划问题的效果，往往需要多次反复的试验。基于随机采样的运动规划方法无法预测下一次规划是否能够成功，更无法预测下一次的路径或者轨迹的参数。 

---

•  V-REP OMPL plugin

　　V-REP中路径规划通常需要以下变量或参数作为输入：

1. a collidable or measurable robot entity: this entity represents the device that should avoid obstacles and is referred to hereafter as robot.
2. a start dummy: the start dummy represents the initial configuration (or start configuration)of the robot. The robot entity should be built on top of the start dummy. Make sure that the center of the robot approximately matches the position of the start dummy. 即需要创建start dummy（相当于一个marker）用于标记机器人的初始构型/位姿。 
3. a goal dummy: the goal dummy represents the desired configuration(or goal configuration) of the robot. The path planning algorithms will try to move the start dummy towards the goal dummy while avoiding collisions between the robot and obstacle.
4. a collidable or measurable obstacle entity: this entity represents the obstacles that the robot should avoid, while following a path from the start to the goal configuration.

 　　即进行路径规划需要已知：机器人，机器人的初始状态，机器人的目标状态以及周围环境（障碍物）。

　　V-REP中进行运动/路径规划时，可以按照以下几步进行：

　　（1）调用simExtOMPL_createTask函数创建一个规划任务；

　　（2）调用simExtOMPL_setAlgorithm函数选择一个合适的算法；

　　（3）调用simExtOMPL_createStateSpace和simExtOMPL_setStateSpace创建所需的状态空间（State space: The parameter space for the robot. This space represents all possible configurations of the robot in the workspace. A single point in the state space is a state）；

　　（4）调用simExtOMPL_setCollisionPairs设置不应与环境发生碰撞的对象（Specify which entities are not allowed to collide）；

　　（5）调用simExtOMPL_setStartState和simExtOMPL_setGoalState设置机器人的初始状态以及目标状态；

　　（6）调用simExtOMPL_compute计算路径；调用simExtOMPL_destroyTask删除路径规划任务（Note: state space components created during simulation are automatically destroyed when simulation ends）。

　　注意OMPL计算出的路径只是无数多解里面的一个，无法保证这个解最优，因此可以重复计算多次，选择一条更好/短的路径。The path provided by simExtOMPL_compute is usually just one path among an infinite number of other possible paths, and there is no guarantee that the returned path is the optimal solution. For that reason it is common to compute several different paths, then to select the better one (e.g. the shorter one).

　　下面是一个2维平面内路径规划的例子：如下图所示的L型物体，从起始位置开始要通过长宽为1m×1m的迷宫并避免与障碍物发生碰撞，最终到达目标位置。

　　在V-REP自带的例子3DoFHolonomicPathPlanning.ttt中可以找到这个模型，下面是其lua代码:

visualizePath = function(path)
    if not _lineContainer then
-- simAddDrawingObject: Adds a drawing object that will be displayed in the scene
-- Lua parameters:
    --sim_drawing_lines:items are pixel-sized lines(6 values per item (x0;y0;z0;x1;y1;z1) + auxiliary values)
    --size: size of the item (width of lines or size of points are in pixels, other sizes are in meters
    --parentObjectHandle: handle of the scene object where the drawing items should keep attached to. if the scene 
            --object moves, the drawing items will also move,-1 if the drawing items are relative to the world (fixed)
    --maxItemCount: maximum number of items this object can hold.
    --ambient_diffuse: default ambient/diffuse color (pointer to 3 rgb values). Can be NULL
        _lineContainer = simAddDrawingObject(sim_drawing_lines, 3, 0, -1, 99999, {0.2,0.2,0.2})
    end

-- Adds an item (or clears all items) to a previously inserted drawing object.
-- itemData: data relative to an item. If the item is a point item, 3 values are required (x;y;z). 
-- If the item is a line item, 6 values are required, If nil the drawing object is emptied of all its items
    simAddDrawingObjectItem(_lineContainer, nil) -- clear all items

    if path then
        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]}
            simAddDrawingObjectItem(_lineContainer, lineDat)
        end
    end
end
---------------------------------------------------------------------------------------------------------------------

robotHandle = simGetObjectHandle('StartConfiguration')
targetHandle = simGetObjectHandle('GoalConfiguration')

initPos = simGetObjectPosition(robotHandle, -1)  --Specify -1 to retrieve the absolute position
initOrient = simGetObjectOrientation(robotHandle, -1)

-- Create a task object, used to represent the motion planning task
-- Lua synopsis: number taskHandle = simExtOMPL_createTask(string name)
t = simExtOMPL_createTask('t')

-- Create a component of the state space for the motion planning problem
-- stateSpaceHandle=simExtOMPL_createStateSpace(name,type,objectHandle,boundsLow,boundsHigh,useForProjection)
ss = {simExtOMPL_createStateSpace('2d',sim_ompl_statespacetype_pose2d,robotHandle,{-0.5,-0.5},{0.5,0.5},1)}

-- Set the state space of this task object
-- Lua synopsis: number result=simExtOMPL_setStateSpace(number taskHandle, table stateSpaceHandles)
simExtOMPL_setStateSpace(t, ss)

-- Set the search algorithm for the specified task. Default algorithm used is KPIECE1
-- Lua synopsis: number result=simExtOMPL_setAlgorithm(number taskHandle, number algorithm)
simExtOMPL_setAlgorithm(t,sim_ompl_algorithm_RRTConnect)


-- Set the collision pairs for the specified task object.
-- Lua synopsis: number result=simExtOMPL_setCollisionPairs(number taskHandle, table collisionPairHandles)
--[[
Lua parameters:
collisionPairHandles: a table containing 2 entity handles for each collision pair. A collision pair is represented
by a collider and a collidee, that will be tested against each other. The first pair could be used for robot 
self-collision testing, and a second pair could be used for robot-environment collision testing. The collider can 
be an object or a collection handle. The collidee can be an object or collection handle, or sim_handle_all, in which 
case the collider will be checked agains all other collidable objects in the scene.
--]]
simExtOMPL_setCollisionPairs(t,{simGetObjectHandle('L_start'),sim_handle_all})

startpos = simGetObjectPosition(robotHandle, -1)  
startorient = simGetObjectOrientation(robotHandle, -1)
startpose={startpos[1], startpos[2], startorient[3]}

-- Set the start state for the specified task object
-- Lua synopsis: number result=simExtOMPL_setStartState(number taskHandle, table state)
-- state: a table of numbers, whose size must be consistent with the robot's state space specified in this task object
simExtOMPL_setStartState(t, startpose)

goalpos = simGetObjectPosition(targetHandle,-1)
goalorient = simGetObjectOrientation(targetHandle,-1)
goalpose = {goalpos[1], goalpos[2], goalorient[3]}

-- Set the goal state for the specified task object
simExtOMPL_setGoalState(t, goalpose)

-- Use OMPL to find a solution for this motion planning task.
-- number result,table states=simExtOMPL_compute(number taskHandle,number maxTime,number maxSimplificationTime=-1,number stateCnt=0)    
-- return value: states--a table of states, representing the solution, from start to goal. States are specified linearly.
r, path = simExtOMPL_compute(t, 4, -1, 800)

visualizePath(path)
----------------------------------------------------------------------------------------------------------------------------------

-- Simply jump through the path points, no interpolation here:
for i=1, #path-3, 3 do   -- count from 1 to len(path)-3 with increments of 3
    pos={path[i], path[i+1], initPos[3]}
    orient={initOrient[1], initOrient[2], path[i+2]}
    simSetObjectPosition(robotHandle, -1, pos)
    simSetObjectOrientation(robotHandle, -1, orient)
    simSwitchThread()
end

 　　下面做一些补充说明：

　　（1）函数simExtOMPL_createStateSpace的参数type表示创建的状态空间的类型。type的值可以为：
　　sim_ompl_statespacetype_position2d：二维平面内的移动（X，Y方向移动）
　　sim_ompl_statespacetype_pose2d：二维平面内的移动加转动（X，Y方向移动和绕Z轴的转动）
　　sim_ompl_statespacetype_position3d：三维空间内的移动（X，Y，Z方向上的移动）
　　sim_ompl_statespacetype_pose3d：三维空间内的移动加转动（X，Y，Z方向上的移动和绕X，Y，Z轴的转动）
　　sim_ompl_statespacetype_joint_position：关节空间内的运动

　　上面的例子中“机器人”可以进行在平面上移动和转动，因此参数选为sim_ompl_statespacetype_pose2d

　　（2）函数simExtOMPL_createStateSpace的参数boundsLow和boundsHigh为搜索空间范围的下限和上限，例子中搜索范围的下限为{Xmin,Ymin}，上限为{Xmax,Ymax}。将搜索限制在有限的区域内有助于减少运算量。It is important to appropriately limit the search area in order to: (1) reduce calculation times, and (2) make the path planning task realizable. 

　　（3）这个例子中我们只考虑机器人与其外界环境发生碰撞的可能。如果考虑机器人在运动过程中自身部件之间可能发生的碰撞，则需要添加其它collision pairs，如motionPlanningAndGraspingDemo中的例子：

-- 2 collision pairs: the first for robot self-collision detection, the second for robot-environment collision detection:
collisionPairs={simGetCollectionHandle('Mico'),simGetCollectionHandle('Mico'),simGetCollectionHandle('Mico'),sim_handle_all}

　　（4）要选择合适的算法，可以参考：Available Planners

　　RRT Connect (RRTConnect)是RRT算法的一种改进：This planner is a bidirectional version of RRT (i.e., it grows two trees. The basic idea is to grow two RRTs, one from the start and one from the goal, and attempt to connect them). It usually outperforms the original RRT algorithm.

　　（5）增大计算步长可以降低运算量，但可能导致机器人跳过障碍物，出现下图所示的这种情况：

---

　　V-REP软件自带的模型3DoFHolonomicPathPlanning-stateValidationCallback.ttt与上面的例子大致相同，不同的一点是使用simExtOMPL_setStateValidationCallback设置自定义的函数，验证路径上点的有效性。

stateValidation=function(state)
    -- Read the current state:
    local p=simGetObjectPosition(robotHandle,-1)
    local o=simGetObjectOrientation(robotHandle,-1)

    -- Apply the provided state:
    simSetObjectPosition(robotHandle,-1,{state[1],state[2],p[3]})
    simSetObjectOrientation(robotHandle,-1,{o[1],o[2],state[3]})

    -- Test the state. Allowed are states where the robot is at least
    -- 'minDistance' away from obstacles, and at most 'maxDistance':
    -- simCheckDistance: Checks the minimum distance between two entities.
-- number result,table_7 distanceData=simCheckDistance(number entity1Handle,number entity2Handle,number threshold)
    -- threshold: if distance is bigger than the threshold, the distance is not calculated and return value is 0
    -- result: 1 if operation was successful (1 if distance is smaller than threshold)
            -- distanceData is nil if result is different from 1
    -- distanceData[7] is the distance between the entities
    local res,d=simCheckDistance(collisionPairs[1],collisionPairs[2], maxDistance)
    local pass= (res==1) and (d[7]>minDistance)

    -- Following to visualize distances of the states that are valid:
    if pass then
        simAddDrawingObjectItem(cont,d)  -- drawing_lines, 6 values per item (d1;d2;d3;d4;d5;d6)
    end

    -- Following to debug:
--    if pass then
--        simSwitchThread()
--    end

    -- Restore the current state:
    simSetObjectPosition(robotHandle,-1,p)
    simSetObjectOrientation(robotHandle,-1,o)

    -- Return whether the tested state is valid or not:
    return pass
end




visualizePath=function(path)
    if not _lineContainer then
        _lineContainer=simAddDrawingObject(sim_drawing_lines,3,0,-1,99999,{0.2,0.2,0.2})
    end
    simAddDrawingObjectItem(_lineContainer,nil)
    if path then
        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]}
            simAddDrawingObjectItem(_lineContainer,lineDat)
        end
    end
end

maxDistance = 0.05 -- max allowed distance
minDistance = 0.01 -- min allowed distance

cont=simAddDrawingObject(sim_drawing_lines,2,0,-1,99999,{1,0,0})

robotHandle=simGetObjectHandle('StartConfiguration')
targetHandle=simGetObjectHandle('GoalConfiguration')

initPos=simGetObjectPosition(robotHandle,-1)
initOrient=simGetObjectOrientation(robotHandle,-1)

t=simExtOMPL_createTask('t')

ss={simExtOMPL_createStateSpace('2d',sim_ompl_statespacetype_pose2d,robotHandle,{-0.5,-0.5},{0.5,0.5},1)}

simExtOMPL_setStateSpace(t,ss)

simExtOMPL_setAlgorithm(t,sim_ompl_algorithm_RRTConnect)

collisionPairs={simGetObjectHandle('L_start'),sim_handle_all}

--[[
Set a custom state validation. By default state validation is performed by collision checking, between robot's 
collision objects and environment's objects. By specifying a custom state validation, it is possible to perform 
any arbitrary check on a state to determine wether it is valid or not. Argument to the callback is the state to 
validate, and return value must be a boolean indicating the validity of the state --]]
-- Lua synopsis: number result=simExtOMPL_setStateValidationCallback(number taskHandle, string callback)
simExtOMPL_setStateValidationCallback(t,'stateValidation')

startpos=simGetObjectPosition(robotHandle,-1)
startorient=simGetObjectOrientation(robotHandle,-1)
startpose={startpos[1],startpos[2],startorient[3]}

simExtOMPL_setStartState(t,startpose)

goalpos=simGetObjectPosition(targetHandle,-1)
goalorient=simGetObjectOrientation(targetHandle,-1)
goalpose={goalpos[1],goalpos[2],goalorient[3]}

simExtOMPL_setGoalState(t,goalpose)

simSetThreadAutomaticSwitch(false)  -- Allows to temporarily forbid thread switches

r,path=simExtOMPL_compute(t,8,-1,800)

simSetThreadAutomaticSwitch(true) -- if true, the thread will be able to automatically switch to another thread

while path do
    visualizePath(path)
    -- Simply jump through the path points, no interpolation here:
    for i=1,#path-3,3 do
        pos={path[i],path[i+1],initPos[3]}
        orient={initOrient[1],initOrient[2],path[i+2]}
        simSetObjectPosition(robotHandle,-1,pos)
        simSetObjectOrientation(robotHandle,-1,orient)
        simSwitchThread()
    end
end

　　注意child script是一个仿真控制脚本.  Each child script represents a small code or program written in Lua allowing handling a particular function in a simulation. Child scripts are attached to (orassociated with) scene objects. Child scripts can be of two different types: non-threaded child scripts or threaded child scripts:

　　Non-threaded child scripts are pass-through scripts. This means that every time they are called, they should perform some task and then return control. If control is not returned, then the whole simulation halts.

　　Threaded child scripts are scripts that will launch in a thread. The launch of threaded child scripts is handled by the default main script code, via the simLaunchThreadedChildScripts function, in a cascaded way. When a threaded child script's execution is still underway, it will not be launched a second time. When a threaded child script ended, it can be relaunched only if the execute once item in the script properties is unchecked. By default a threaded child script will execute for about 1-2 milliseconds before automatically switching to another thread. Once the current thread was switched, it will resume execution at next simulation pass (i.e. at a time currentTime+simulationTimeStep). The thread switching is automatic (occurs after the specified time). Some operation should not be interrupted in order to execute correctly (imagine moving several objects in a loop). In that case, you can temporarily forbid thread switches with the simSetThreadAutomaticSwitch function.

---

 　　6DoFHolonomicPathPlanning是一个三维空间中路径规划的例子，如下图所示，左侧物体L_start要调整位置和姿态穿过矩形孔到达goalpose而不与周围环境发生碰撞。

robotHandle = simGetObjectHandle('Start')
targetHandle = simGetObjectHandle('End')

t = simExtOMPL_createTask('t')

ss = {simExtOMPL_createStateSpace('6d',sim_ompl_statespacetype_pose3d,robotHandle,{-1,-0.5,0},{1,0.5,1},1)}

simExtOMPL_setStateSpace(t, ss)

simExtOMPL_setAlgorithm(t, sim_ompl_algorithm_RRTConnect)

simExtOMPL_setCollisionPairs(t,{simGetObjectHandle('L_start'),sim_handle_all})

startpos = simGetObjectPosition(robotHandle, -1)
startorient = simGetObjectQuaternion(robotHandle, -1)
startpose = {startpos[1],startpos[2],startpos[3],startorient[1],startorient[2],startorient[3],startorient[4]}
simExtOMPL_setStartState(t, startpose)

goalpos = simGetObjectPosition(targetHandle, -1)
goalorient = simGetObjectQuaternion(targetHandle, -1)
goalpose = {goalpos[1],goalpos[2],goalpos[3],goalorient[1],goalorient[2],goalorient[3],goalorient[4]}
simExtOMPL_setGoalState(t, goalpose)

r, path = simExtOMPL_compute(t,20,-1,200)

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]}
        simSetObjectPosition(robotHandle,-1,pos)
        simSetObjectQuaternion(robotHandle,-1,orient)
        simSwitchThread()
    end
end

　　可以看出步骤与前面的例子基本一致。这里有两个地方需要注意：1.由于规划的路径是三维空间内且涉及位置和姿态的同时调整，因此在设定状态空间时要选择sim_ompl_statespacetype_pose3d作为类型参数；2. simGetObjectQuaternion函数返回的是描述姿态的四元数

 

 

 

参考：

运动规划最全简介

机器人运动规划—百度文库

Planning Algorithms

Motion planning—Wikipedia

Open Motion Planning Library: A Primer

Questions/Answers around V-REP

四元数（Quaternion）和旋转

基于随机采样的运动规划综述. 《控制与决策》, 2005, 20(7):721-726