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

　　路径规划问题是机器人学研究的一个重要领域，它是指给定操作环境以及起始和目标的位置姿态，要求选择一条从起始点到目标点的路径，使运动物体(移动机器人或机械臂)能安全、无碰撞地通过所有的障碍物而达到目标位置。路径规划从研究对象上可分为关节式机械臂和移动机器人。一般来讲前者具有更多的*度，而后者的作业范围要更大一些，这两类对象具有不同的特点，因此在研究方法上略有不同。在V-rep学习笔记：机器人路径规划1中主要讲的是移动机器人的路径规划，接下来我们研究一下在VREP中如何实现机械臂的运动规划。

　　如果不考虑避障和关节限制，机械臂的路径规划通过运动学逆解就可以实现。但是考虑这些因素以后仅通过inverse kinematics有时无法顺利完成任务，情况如下图所示。When using only IK, we take the risk to move into a configuration that does not allow us to further follow the path (e.g. because of some joint limitations, or because of some obstacles in the way):

[Following a path via IK]

　　因此为了能让机械臂顺利地在空间中移动，进行路径规划时需要考虑下面这些因素：

　　A motion planning task allows to compute a trajectory (usually in the configuration space^{构型空间，或称C-空间}of the manipulator) from a start configuration to a goal configuration, by taking into account following main items:

• the manipulator kinematics
• the manipulator joint limits
• the manipulator self-collisions
• the collisions between manipulator and obstacles (or the environment)

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

　　When the goal configuration is not directly known, it needs to be computed from a goal pose (i.e. the position and orientation in Cartesian space of the end-effector). The motion planning task can then be divided into two distinct operations:

• Finding a goal configuration that matches a goal pose
• Finding a collision-free path from a start configuration to a goal configuration

[Two step motion planning: (a) finding a goal configuration, (b) finding a path in the configuration space]

Finding a goal configuration

　　When the goal configuration is not known, but only a goal pose is known (i.e. a position and orientation of the end-effector) then an appropriate manipulator configuration that satisfies the goal pose constraint has to be found. There can be multiple (or even an infinitie number of) solutions:

[Searching for a manipulator configuration corresponding to a given end-effector pose]

　　Finding an appropriate manipulator configuration that satisfies a given pose is computed in V-REP based on precalculated configuration nodes and inverse kinematics calculation. Imagine following 2 DoF manipulator:

[Simple 2 DoF manipulator (2 joints, each with specific joint limits)]

　　V-REP will generate a large amount of precalculated configuration nodes, where each node consists of a manipulator configuration (i.e. joint values) associated with a corresponding end-effector pose (i.e. a position and orientation in Cartesian space). This is visualized in following figure that shows 12 precalculated configuration nodes (i.e. 3x4, where the joint 1 range is subdivided in 3 joint values, and the joint 2 range is subdivided in 4 joint values):

[Precalculated configuration nodes (pairs of manipulator configuration and end-effector pose)]

　　The precalculated nodes are used, together with the inverse kinematics of the manipulator, to find manipulator configurations that match a given end-effector pose. The search algorithm will select all nodes that contain poses in proximity (i.e. close enough according to a specified Cartesian space metric) of a desired pose:

[2 precalculated configuration nodes close enough to a desired pose]

　　The inverse kinematics will then try to move the selected poses so that they overlap the desired pose:

[IK calculations in order to find valid configurations for a desired pose]

　　simGetConfigForTipPose函数实现了上面的功能，该函数用于随机寻找能满足末端位置姿态的机构构型(Searches for a manipulator configuration that matches a given end-effector position/orientation in space. Search is randomized)，并且可以设置碰撞检测和关节范围，在避开障碍物以及不超过关节限制的情况下寻找满足条件的构型。

table jointPositions = simGetConfigForTipPose(number ikGroupHandle,table jointHandles,number distanceThreshold,number maxTimeInMs,table_4 metric=nil,table collisionPairs=nil,table jointOptions=nil,table lowLimits=nil,table ranges=nil)

　　函数参数如下：

• ikGroupHandle: the handle of an IK group that is in charge of bringing the manipulator's tip onto a target. The IK group can also be marked as explicit handling if needed.
• jointHandles: an array that specifies the joint handles for the joints we wish to retrieve the values calculated by the IK.
• thresholdDist: a distance indicating when IK should be computed in order to try to bring the tip onto the target: since the search algorithm proceeds by generating random configurations, many of them produce a tip pose that is too far from the target pose to run IK successfully. Choosing a large value will result in slow calculations, choosing a small value might produce a smaller subset of solutions. Distance between two poses is calculated using a metric.
• maxTimeInMs: a maximum time in ms after which the search is aborted.
• metric: an array to 4 values indicating a metric used to compute pose-pose distances: distance=sqrt((dx*metric[0])^2+(dy*metric[1])^2+(dz*metric[2])^2+(angle*metric[3])^2). Can be NULL for a default metric of {1.0,1.0,1.0,0.1}.
• collisionPairs: an array 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. Can be NULL if collision checking is not required.
• jointOptions: a bit-coded value corresponding to each specified joint handle. Bit 0 (i.e. 1) indicates the corresponding joint is dependent of another joint. Can be NULL.
• lowLimits: an optional array pointing to different low limit values for each specified joint. This can be useful when you wish to explore a sub-set of the joint's intervals. Can be NULL for the default joint's low limit values. If not NULL, then ranges should also not be NULL.
• ranges: an optional array pointing to different range values for each specified joint. This can be useful when you wish to explore a sub-set of the joint's intervals. Can be NULL for the default joint's range values. If not NULL, then lowLimits should also not be NULL.

　　其中collisionPairs为设置好的碰撞检测对，比如机器人自身和外界环境，如果为空则不进行碰撞检测。lowLimits和ranges一起规定了关节运动范围的上下限。建立一个三连杆机构，控制目标点Target的位置。下面左边场景中在simGetConfigForTipPose函数里设置了碰撞检测而右边没有，可以很明显看出区别来：左边的机构能避开环境中的障碍物（L型和圆形柱子）。将仿真时间步长设为1s，开启real-time模式进行仿真。可以看出由于函数的随机性，每秒计算一次得到的机构构型都不一致：

   

代码如下：

if (sim_call_type==sim_childscriptcall_initialization) then

    -- Put some initialization code here
    jh={-,-,-}
    for i=,, do
        jh[i]=simGetObjectHandle('J'..i)
    end

    ikGroup=simGetIkGroupHandle('IK_Group')
    ikTarget=simGetObjectHandle('Target')
    collisionPairs={simGetCollectionHandle('manipulator'),simGetCollectionHandle('environment')}
end

if (sim_call_type==sim_childscriptcall_actuation) then

    simSetObjectPosition(ikTarget, -, {,,})

    -- Searches for a manipulator configuration that matches a given end-effector position/orientation in space. Search is randomized.
    jointPositions = simGetConfigForTipPose(ikGroup, jh, 0.1,  ,nil, collisionPairs)

    if jointPositions then
        local info = string.format("%.1f,%.1f,%.1f",jointPositions[]*/math.pi,jointPositions[]*/math.pi,jointPositions[]*/math.pi)
        simAddStatusbarMessage(info)

        for i=,, do
            simSetJointPosition(jh[i], jointPositions[i])
        end
    end

end

if (sim_call_type==sim_childscriptcall_sensing) then

    -- Put your main SENSING code here

end

if (sim_call_type==sim_childscriptcall_cleanup) then

    -- Put some restoration code here

end

 

Finding a collision-free path

　　When searching for a collision-free path from a start configuration to a goal configuration, depending on the manipulator type, there can be multiple (or even an infinitie number of) solutions, mainly also because the search algorithm is based on a randomized resolution method:

[Searching for a collision-free path from one to another manipulator configuration]

　　从V-REP 3.3.0开始使用OMPL(The Open Motion Planning Library)作为插件提供运动规划函数，可以让用户更灵活的实现机器人的运动规划。在已知机器人初始构型和目标构型后就可以使用OMPL实现多关节机器人的运动规划。注意与目标姿态对应的目标构型可能有很多个，不是每一个都能规划出无障碍的路径，因此一般要多选几个目标构型进行尝试。

　　If the goal state had to be computed from a goal pose, then several goal states are usually tested, since not all goal states might be reachable or close enough (in terms of state space distance). A common practice is to first find several goal states, then order them according their state space distance to the start state. Then perform path planning calculations to the closest goal state, then to the next closest goal state, etc., until a satisfactory path was found.

　　另外，由于随机性不能保证计算出的路径是最短的，因此要在满足要求的路径中进行比较，选择一条最短的路径。对多关节机械臂进行路径规划的基本思路如下：

Executing motion planning

1. Starting with a manipulator start configuration, we want the end-effector to move without collisions to a specific goal pose.
2. Search for a goal configuration that matches the goal pose (simGetMpConfigForTipPose).
3. Search for a path between the start and goal configurations.
4. Repeat above steps 2-3 n times.
5. Select the shortest calculated path.

　　下面以C:\Program Files\V-REP3\V-REP_PRO_EDU\scenes中的motionPlanningDemo1.ttt为例（进行了一定的修改），规划出一条路径让7*度机械臂运动到指定的位置和姿态，同时避免与外界环境发生碰撞，并避免关节超限：

displayInfo=function(txt)
    if dlgHandle then
        simEndDialog(dlgHandle)
    end
    dlgHandle=nil
    if txt and #txt> then
        dlgHandle=simDisplayDialog('info',txt,sim_dlgstyle_message,false)
        simSwitchThread()
    end
end

forbidThreadSwitches=function(forbid)
    -- Allows or forbids automatic thread switches.
    -- This can be important for threaded scripts. For instance,
    -- you do not want a switch to happen while you have temporarily
    -- modified the robot configuration, since you would then see
    -- that change in the scene display.
    if forbid then
        forbidLevel=forbidLevel+
        if forbidLevel== then
            simSetThreadAutomaticSwitch(false)
        end
    else
        forbidLevel=forbidLevel-
        if forbidLevel== then
            simSetThreadAutomaticSwitch(true)
        end
    end
end

getMatrixShiftedAlongZ=function(matrix,localZShift)
    -- Returns a pose or matrix shifted by localZShift along the matrix's z-axis
    local m={}
    for i=,, do
        m[i]=matrix[i]
    end
    m[]=m[]+m[]*localZShift
    m[]=m[]+m[]*localZShift
    m[]=m[]+m[]*localZShift
    return m
end

findCollisionFreeConfigAndCheckApproach=function(matrix)
    -- Here we search for a robot configuration..
    -- 1. ..that matches the desired pose (matrix)
    -- 2. ..that does not collide in that configuration
    -- 3. ..that does not collide and that can perform the IK linear approach
    simSetObjectMatrix(ikTarget,-,matrix)
    -- Here we check point 1 & 2:
    local c=simGetConfigForTipPose(ikGroup,jh,0.65,,nil,collisionPairs)
    if c then
        -- Here we check point 3:
        local m=getMatrixShiftedAlongZ(matrix,ikShift)
        local path=generateIkPath(c,m,ikSteps)
        if path==nil then
            c=nil
        end
    end
    return c
end

findSeveralCollisionFreeConfigsAndCheckApproach=function(matrix,trialCnt,maxConfigs)
    -- Here we search for several robot configurations...
    -- 1. ..that matches the desired pose (matrix)
    -- 2. ..that does not collide in that configuration
    -- 3. ..that does not collide and that can perform the IK linear approach
    forbidThreadSwitches(true)
    simSetObjectMatrix(ikTarget,-,matrix)
    local cc=getConfig()
    local cs={}
    local l={}
    for i=,trialCnt, do
        local c=findCollisionFreeConfigAndCheckApproach(matrix)
        if c then
            local dist=getConfigConfigDistance(cc,c)
            local p=
            local same=false
            for j=,#l, do
                if math.abs(l[j]-dist)<0.001 then
                    -- we might have the exact same config. Avoid that
                    same=true
                    for k=,#jh, do
                        if math.abs(cs[j][k]-c[k])>0.01 then
                            same=false
                            break
                        end
                    end
                end
                if same then
                    break
                end
            end
            if not same then
                cs[#cs+]=c
                l[#l+]=dist
            end
        end
        if #l>=maxConfigs then
            break
        end
    end
    forbidThreadSwitches(false)
    if #cs== then
        cs=nil
    end
    return cs
end

getConfig=function()
    -- Returns the current robot configuration
    local config={}
    for i=,#jh, do
        config[i]=simGetJointPosition(jh[i])
    end
    return config
end

setConfig=function(config)
    -- Applies the specified configuration to the robot
    if config then
        for i=,#jh, do
            simSetJointPosition(jh[i],config[i])
        end
    end
end

getConfigConfigDistance=function(config1,config2)
    -- Returns the distance (in configuration space) between two configurations
    local d=
    for i=,#jh, do
        local dx=(config1[i]-config2[i])*metric[i]
        d=d+dx*dx
    end
    return math.sqrt(d)
end

getPathLength=function(path)
    -- Returns the length of the path in configuration space
    local d=
    local l=#jh
    local pc=#path/l
    for i=,pc-, do
        local config1={path[(i-)*l+],path[(i-)*l+],path[(i-)*l+],path[(i-)*l+],path[(i-)*l+],path[(i-)*l+],path[(i-)*l+]}
        local config2={path[i*l+],path[i*l+],path[i*l+],path[i*l+],path[i*l+],path[i*l+],path[i*l+]}
        d=d+getConfigConfigDistance(config1,config2)
    end
    return d
end

followPath=function(path)
    -- Follows the specified path points. Each path point is a robot configuration. Here we don't do any interpolation
    if path then
        local l=#jh
        local pc=#path/l
        for i=,pc, do
            local config={path[(i-)*l+],path[(i-)*l+],path[(i-)*l+],path[(i-)*l+],path[(i-)*l+],path[(i-)*l+],path[(i-)*l+]}
            setConfig(config)
            simSwitchThread()
        end
    end
end

findShortestPath=function(startConfig,goalConfigs,cnt)
    -- Here we do path planning between the specified start and goal configurations. We run the search cnt times,
    -- and return the shortest path, and its length
    local task=simExtOMPL_createTask('task')
    simExtOMPL_setAlgorithm(task,sim_ompl_algorithm_RRTConnect)
    local j1_space=simExtOMPL_createStateSpace('j1_space',sim_ompl_statespacetype_joint_position,jh[],{-*math.pi/},{*math.pi/},)
    local j2_space=simExtOMPL_createStateSpace('j2_space',sim_ompl_statespacetype_joint_position,jh[],{-*math.pi/},{*math.pi/},)
    local j3_space=simExtOMPL_createStateSpace('j3_space',sim_ompl_statespacetype_joint_position,jh[],{-*math.pi/},{*math.pi/},)
    local j4_space=simExtOMPL_createStateSpace('j4_space',sim_ompl_statespacetype_joint_position,jh[],{-*math.pi/},{*math.pi/},)
    local j5_space=simExtOMPL_createStateSpace('j5_space',sim_ompl_statespacetype_joint_position,jh[],{-*math.pi/},{*math.pi/},)
    local j6_space=simExtOMPL_createStateSpace('j6_space',sim_ompl_statespacetype_joint_position,jh[],{-*math.pi/},{*math.pi/},)
    local j7_space=simExtOMPL_createStateSpace('j7_space',sim_ompl_statespacetype_joint_position,jh[],{-*math.pi/},{*math.pi/},)
    simExtOMPL_setStateSpace(task,{j1_space,j2_space,j3_space,j4_space,j5_space,j6_space,j7_space})
    simExtOMPL_setCollisionPairs(task,collisionPairs)
    simExtOMPL_setStartState(task,startConfig)

    for i=,#goalConfigs, do
        simExtOMPL_addGoalState(task,goalConfigs[i])
    end

    local path = nil
    local length =
    forbidThreadSwitches(true)
    for i=, cnt,  do
        local res,_path = simExtOMPL_compute(task,,-,)
        if res> and _path then
            local _l = getPathLength(_path)
            if _l < length then
                length = _l
                path = _path
            end
        end
    end
    forbidThreadSwitches(false)
    simExtOMPL_destroyTask(task)
    return path, length
end

generateIkPath=function(startConfig,goalPose,steps)
    -- Generates (if possible) a linear, collision free path between a robot config and a target pose
    forbidThreadSwitches(true)
    local currentConfig=getConfig()
    setConfig(startConfig)
    simSetObjectMatrix(ikTarget,-,goalPose)
    local c=simGenerateIkPath(ikGroup,jh,steps,collisionPairs)
    setConfig(currentConfig)
    forbidThreadSwitches(false)
    return c
end

getReversedPath=function(path)
    -- This function will simply reverse a path
    local retPath={}
    local ptCnt=#path/#jh
    for i=ptCnt,,- do
        for j=,#jh, do
            retPath[#retPath+]=path[(i-)*#jh+j]
        end
    end
    return retPath
end

threadFunction=function()
    local allTargets={target1,target2,target3,target4}
    local targetIndex=
    while true do
        -- This is the main loop. We move from one target to the next
        local theTarget=allTargets[targetIndex]
        targetIndex=targetIndex+
        if targetIndex> then
            break
        end

        -- m is the transformation matrix or pose of the current target:
        local m=simGetObjectMatrix(theTarget,-)

        -- Compute a pose that is shifted by ikDist along the Z-axis of pose m,
        -- so that we have a final approach that is linear along target axis Z:
        m=getMatrixShiftedAlongZ(m,-ikShift)

        -- Find several configs for pose m, and order them according to the
        -- distance to current configuration (smaller distance is better).
        -- In following function we also check for collisions and whether the
        -- final IK approach is feasable:
        displayInfo('searching for a maximum of 60 valid goal configurations...')
        local c=findSeveralCollisionFreeConfigsAndCheckApproach(m,,)

        -- Search a path from current config to a goal config. For each goal
        -- config, search 6 times a path and keep the shortest.
        local txt='Found '..#c..' different goal configurations for the desired goal pose.'
        txt=txt..'&&nNow searching the shortest path of 6 searches...'
        displayInfo(txt)
        local path=findShortestPath(getConfig(),c,)
        displayInfo(nil)

        -- Follow the path:
        followPath(path)

        -- For the final approach, the target is the original target pose:
        m=simGetObjectMatrix(theTarget,-)

        -- Compute a straight-line path from current config to pose m:
        path=generateIkPath(getConfig(),m,ikSteps)

        simWait(0.5)

        -- Follow the path:
        followPath(path)

        -- Generate a reversed path in order to move back:
        path=getReversedPath(path)

        -- Follow the path:
        followPath(path)
    end
end

-- Initialization phase:

jh={-,-,-,-,-,-,-}
for i=,, do
    jh[i]=simGetObjectHandle('j'..i)
end
ikGroup=simGetIkGroupHandle('ik')
ikTarget=simGetObjectHandle('target')
collisionPairs={simGetCollectionHandle('manipulator'),simGetCollectionHandle('environment')}
target1=simGetObjectHandle('testTarget1')
target2=simGetObjectHandle('testTarget2')
target3=simGetObjectHandle('testTarget3')
target4=simGetObjectHandle('testTarget4')
metric={0.5,,,0.5,0.1,0.2,0.1}
ikShift=0.1
ikSteps=
forbidLevel=

res,err=xpcall(threadFunction,function(err) return debug.traceback(err) end)
if not res then
    simAddStatusbarMessage('Lua runtime error: '..err)
end

Generating a linear path via IK in the Cartesian space

　　In some situations, it is required to move along a straight line in Cartesian space, i.e. using the manipulator's inverse kinematics. In that case, V-REP can generate a straight trajectory, given that the robot does not collide along that trajectory:

[Generating a path via IK]

　　函数simGenerateIkPath可以实现这一功能，注意函数返回的path是在Configuration space而不是Cartesian space。Generates a path that drives a robot from its current configuration to its target dummy in a straight line (i.e. shortest path in Cartesian space).

table path = simGenerateIkPath(number ikGroupHandle, table jointHandles, number ptCnt, table collisionPairs=nil, table jointOptions=nil)

　　函数参数如下：

• ikGroupHandle: the handle of an IK group that is in charge of bringing the manipulator's tip onto a target. The IK group can also be marked as explicit handling if needed.
• jointHandles: an array that specifies the joint handles for the joints we wish to retrieve the values calculated by the IK.
• ptCnt: the desired number of path points. Each path point contains a robot configuration. A minimum of two path points is required. If the tip-target dummy distance is large, a larger number for ptCnt leads to better results for this function.
• collisionPairs: an array 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. Can be NULL if collision checking is not required.
• jointOptions: a bit-coded value corresponding to each specified joint handle. Bit 0 (i.e. 1) indicates the corresponding joint is dependent of another joint. Can be NULL.

　　还是以简单的3连杆机构为例，下图最左边场景中simGenerateIkPath的参数collisionPairs为空，中间的不为空。可以看出左边生成的路径不会考虑碰撞，而中间由于存在障碍物无法生成一条不发生碰撞的路径。将障碍物挪动一下位置，如果能计算出无碰撞路径，机械臂将沿着路径运动，如最右图所示。

  

代码如下：

displayInfo=function(txt)
    if txt and #txt> then
        simDisplayDialog('info',txt,sim_dlgstyle_message,false)
    end
end

followPath=function(path)
    -- Follows the specified path points. Each path point is a robot configuration. Here we don't do any interpolation
    if path then
        local l=#jh
        local pc=#path/l
        for i=,pc, do
            local config={path[(i-)*l+],path[(i-)*l+],path[(i-)*l+]}
            setConfig(config)
            simWait(0.5)
        end
    else
        displayInfo("No path found!")
    end
end

setConfig=function(config)
    -- Applies the specified configuration to the robot
    if config then
        for i=,#jh, do
            simSetJointPosition(jh[i],config[i])
        end
    end
end

-- Put some initialization code here
jh={-,-,-}
for i=,, do
    jh[i]=simGetObjectHandle('J'..i)
end

ikGroup=simGetIkGroupHandle('IK_Group')
ikTarget=simGetObjectHandle('Target')
collisionPairs={simGetCollectionHandle('manipulator'),simGetCollectionHandle('environment')}

simSetObjectPosition(ikTarget, -, {0.5,0.5,})

-- Generates a path that drives a robot from its current configuration to its target dummy in a straight line
path = simGenerateIkPath(ikGroup, jh, , collisionPairs, nil)

followPath(path)

参考：

Motion planning

Robotic Motion Planning-CMU Robotics Institute

Motion Planning in Real and Virtual Worlds

Planning Algorithms

Using the motion planning functionality

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