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），这样更具灵活性。

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

View Code



　　注意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