V-rep學習筆記：機器人路徑規劃2

時間 2019-11-06

標籤 rep 學習筆記機器人路徑規劃简体版

原文原文鏈接

　　路徑規劃問題是機器人學研究的一個重要領域，它是指給定操做環境以及起始和目標的位置姿態，要求選擇一條從起始點到目標點的路徑，使運動物體(移動機器人或機械臂)能安全、無碰撞地經過全部的障礙物而達到目標位置。路徑規劃從研究對象上可分爲關節式機械臂和移動機器人。通常來說前者具備更多的自由度，然後者的做業範圍要更大一些，這兩類對象具備不一樣的特色，所以在研究方法上略有不一樣。在V-rep學習筆記：機器人路徑規劃1中主要講的是移動機器人的路徑規劃，接下來咱們研究一下在VREP中如何實現機械臂的運動規劃。html

　　若是不考慮避障和關節限制，機械臂的路徑規劃經過運動學逆解就能夠實現。可是考慮這些因素之後僅經過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):node

[Following a path via IK]安全

　　所以爲了能讓機械臂順利地在空間中移動，進行路徑規劃時須要考慮下面這些因素：app

　　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:dom

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]ide

　　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]oop

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]ui

　　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={-1,-1,-1}
    for i=1,3,1 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, -1, {1,0,0})

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

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

        for i=1,3,1 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

View Code

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>0 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+1
        if forbidLevel==1 then
            simSetThreadAutomaticSwitch(false)
        end
    else
        forbidLevel=forbidLevel-1
        if forbidLevel==0 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=1,12,1 do
        m[i]=matrix[i]
    end
    m[4]=m[4]+m[3]*localZShift
    m[8]=m[8]+m[7]*localZShift
    m[12]=m[12]+m[11]*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,-1,matrix)
    -- Here we check point 1 & 2:
    local c=simGetConfigForTipPose(ikGroup,jh,0.65,10,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,-1,matrix)
    local cc=getConfig()
    local cs={}
    local l={}
    for i=1,trialCnt,1 do
        local c=findCollisionFreeConfigAndCheckApproach(matrix)
        if c then
            local dist=getConfigConfigDistance(cc,c)
            local p=0
            local same=false
            for j=1,#l,1 do
                if math.abs(l[j]-dist)<0.001 then
                    -- we might have the exact same config. Avoid that
                    same=true
                    for k=1,#jh,1 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+1]=c
                l[#l+1]=dist
            end
        end
        if #l>=maxConfigs then
            break
        end
    end
    forbidThreadSwitches(false)
    if #cs==0 then
        cs=nil
    end
    return cs
end

getConfig=function()
    -- Returns the current robot configuration
    local config={}
    for i=1,#jh,1 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=1,#jh,1 do
            simSetJointPosition(jh[i],config[i])
        end
    end
end

getConfigConfigDistance=function(config1,config2)
    -- Returns the distance (in configuration space) between two configurations
    local d=0
    for i=1,#jh,1 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=0
    local l=#jh
    local pc=#path/l
    for i=1,pc-1,1 do
        local config1={path[(i-1)*l+1],path[(i-1)*l+2],path[(i-1)*l+3],path[(i-1)*l+4],path[(i-1)*l+5],path[(i-1)*l+6],path[(i-1)*l+7]}
        local config2={path[i*l+1],path[i*l+2],path[i*l+3],path[i*l+4],path[i*l+5],path[i*l+6],path[i*l+7]}
        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=1,pc,1 do
            local config={path[(i-1)*l+1],path[(i-1)*l+2],path[(i-1)*l+3],path[(i-1)*l+4],path[(i-1)*l+5],path[(i-1)*l+6],path[(i-1)*l+7]}
            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[1],{-170*math.pi/180},{170*math.pi/180},1)
    local j2_space=simExtOMPL_createStateSpace('j2_space',sim_ompl_statespacetype_joint_position,jh[2],{-120*math.pi/180},{120*math.pi/180},2)
    local j3_space=simExtOMPL_createStateSpace('j3_space',sim_ompl_statespacetype_joint_position,jh[3],{-170*math.pi/180},{170*math.pi/180},3)
    local j4_space=simExtOMPL_createStateSpace('j4_space',sim_ompl_statespacetype_joint_position,jh[4],{-120*math.pi/180},{120*math.pi/180},0)
    local j5_space=simExtOMPL_createStateSpace('j5_space',sim_ompl_statespacetype_joint_position,jh[5],{-170*math.pi/180},{170*math.pi/180},0)
    local j6_space=simExtOMPL_createStateSpace('j6_space',sim_ompl_statespacetype_joint_position,jh[6],{-120*math.pi/180},{120*math.pi/180},0)
    local j7_space=simExtOMPL_createStateSpace('j7_space',sim_ompl_statespacetype_joint_position,jh[7],{-170*math.pi/180},{170*math.pi/180},0)
    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=1,#goalConfigs,1 do
        simExtOMPL_addGoalState(task,goalConfigs[i])
    end

    local path = nil
    local length = 999999999999
    forbidThreadSwitches(true)
    for i=1, cnt, 1 do
        local res,_path = simExtOMPL_compute(task,4,-1,300)
        if res>0 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,-1,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,1,-1 do
        for j=1,#jh,1 do
            retPath[#retPath+1]=path[(i-1)*#jh+j]
        end
    end
    return retPath
end

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

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

        -- 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,300,60)

        -- 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,6)
        displayInfo(nil)

        -- Follow the path:
        followPath(path)

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

        -- 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={-1,-1,-1,-1,-1,-1,-1}
for i=1,7,1 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,1,1,0.5,0.1,0.2,0.1}
ikShift=0.1
ikSteps=20
forbidLevel=0

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

View Code

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>0 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=1,pc,1 do
            local config={path[(i-1)*l+1],path[(i-1)*l+2],path[(i-1)*l+3]}
            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=1,#jh,1 do
            simSetJointPosition(jh[i],config[i])
        end
    end
end



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

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

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

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

followPath(path)