VREP path planning (3) motion pl

第三篇，终于来到了激动人心的机械臂路径规划环节
motionPlanningDemo1.ttt
这个scene相比于前面两个就稍微复杂一点了，除了机械臂，障碍物，还多了approachDirectionObstacle，而且涉及到机械臂逆解和碰撞检测的相关内容，我在文章里会稍微介绍一下，但是最好还是先看一下User Manual里面Calculation modules这一节

首先看一下Scene Hierarchy

有一个approachDirectionObstacle障碍物，四个obstacle，四个目标，以及机械臂LBR4p_base:

approachDirectionObstacle是那个蓝色的八边形，至于他的作用，会在下文代码中解释，三个obstacle是三个方块，第四个是地板
再看一下机械臂的结构：

除了常规的逆解结构，包括target和tip，还在tip的相同位置和姿态下多了一个ReferenceFrame，其作用同样放在代码中解释:

逆运动学参数：

关节参数：

可以发现关节是Passive mode(随动模式)
再看一下碰撞的配置：

有机械臂和环境两个
配置基本上是这样了，下面正餐来了，开始看Lua代码吧：

displayInfo=function(txt)
    if dlgHandle then
        sim.endDialog(dlgHandle)
    end
    dlgHandle=nil
    if txt and #txt>0 then
        dlgHandle=sim.displayDialog('info',txt,sim.dlgstyle_message,false)
        sim.switchThread()
    end
end

getMatrixShiftedAlongZ=function(matrix,localZShift)
    --Returns a pose or matrix shifted by localZShift along the matrix z-axis
    local m={}
    for i=1,12,1 do
        m[i]=matrix[i]
    end
    --m[4] m[8] m[12] 是位置，m[3] m[7] m[11] 是z轴单位向量，所以就是沿本身的z轴平移localZShift之后的矩阵
    m[4]=m[4]+m[3]*localZShift
    m[8]=m[8]+m[7]*localZShift
    m[12]=m[12]+m[11]*localZShift
    return m
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
            sim.setThreadAutomaticSwitch(false)
        end
    else
        forbidLevel=forbidLevel-1
        if forbidLevel==0 then
            sim.setThreadAutomaticSwitch(true)
        end
    end
end

findCollisionFreeConfigAndCheckApproach=function(matrix)
    --用于寻找满足预目标位姿的机械臂构型，generateIkPath函数用于判断预目标位姿和目标位姿之间能否直接通过
    -- 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
    sim.setObjectMatrix(ikTarget,-1,matrix)
    -- Here we check point 1 & 2:
    --寻找满足target位姿的机械臂构型，参数：
    --ikHandle
    --机械臂关节 Handle
    --thresholdDist，与精确解的距离阈值，小于这个算满足的逆解
    --maxTimeInMs:最大计算时间
    --距离计算方式
    --collisionPairs，默认二范数
    local c=sim.getConfigForTipPose(ikGroup,jh,0.65,10,nil,collisionPairs)
    if c then
        -- Here we check point 3:
        --这里又把m平移了回去，是为了把target放在偏置后的位置
        local m=getMatrixShiftedAlongZ(matrix,ikShift)
        local path=generateIkPath(c,m,ikSteps)
        --如果找不到路径，那么这个c（初始臂型）没有意义
        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)
    sim.setObjectMatrix(ikTarget,-1,matrix)
    --获取当前机械臂关节角
    local cc=getConfig()
    local cs={}
    local l={}
    --找trialCnt次，将获得的所有不同臂型返回
    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]=sim.getJointPosition(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
            sim.setJointPosition(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 do not 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)
            sim.switchThread()
        end
    end
end

findPath=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=simOMPL.createTask('task')
    simOMPL.setAlgorithm(task,simOMPL.Algorithm.RRTConnect)
    --注意这里机械臂State Space的设置方法
    local j1_space=simOMPL.createStateSpace('j1_space',simOMPL.StateSpaceType.joint_position,jh[1],{-170*math.pi/180},{170*math.pi/180},1)
    local j2_space=simOMPL.createStateSpace('j2_space',simOMPL.StateSpaceType.joint_position,jh[2],{-120*math.pi/180},{120*math.pi/180},2)
    local j3_space=simOMPL.createStateSpace('j3_space',simOMPL.StateSpaceType.joint_position,jh[3],{-170*math.pi/180},{170*math.pi/180},3)
    local j4_space=simOMPL.createStateSpace('j4_space',simOMPL.StateSpaceType.joint_position,jh[4],{-120*math.pi/180},{120*math.pi/180},0)
    local j5_space=simOMPL.createStateSpace('j5_space',simOMPL.StateSpaceType.joint_position,jh[5],{-170*math.pi/180},{170*math.pi/180},0)
    local j6_space=simOMPL.createStateSpace('j6_space',simOMPL.StateSpaceType.joint_position,jh[6],{-120*math.pi/180},{120*math.pi/180},0)
    local j7_space=simOMPL.createStateSpace('j7_space',simOMPL.StateSpaceType.joint_position,jh[7],{-170*math.pi/180},{170*math.pi/180},0)
    simOMPL.setStateSpace(task,{j1_space,j2_space,j3_space,j4_space,j5_space,j6_space,j7_space})
    simOMPL.setCollisionPairs(task,collisionPairs)
    simOMPL.setStartState(task,startConfig)
    simOMPL.setGoalState(task,goalConfigs[1])
    --这里又进行了add，因此得到的goalConfigs是一个二维列表
    for i=2,#goalConfigs,1 do
        simOMPL.addGoalState(task,goalConfigs[i])
    end
    local path=nil
    local l=999999999999
    forbidThreadSwitches(true)
    --返回距离最小的path
    for i=1,cnt,1 do
        local res,_path=simOMPL.compute(task,4,-1,300)
        if res and _path then
            local _l=getPathLength(_path)
            if _l<l then
                l=_l
                path=_path
            end
        end
    end
    forbidThreadSwitches(false)
    simOMPL.destroyTask(task)
    return path,l
end

findShortestPath=function(startConfig,goalConfigs,searchCntPerGoalConfig)
    -- This function will search for several paths between the specified start configuration,
    -- and several of the specified goal configurations. The shortest path will be returned
    forbidThreadSwitches(true)
    local thePath=findPath(startConfig,goalConfigs,searchCntPerGoalConfig)
    forbidThreadSwitches(false)
    return thePath
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)
    --将机械臂的target放到goalPose上
    sim.setObjectMatrix(ikTarget,-1,goalPose)
    --计算从机械臂当前臂型到其target位姿的最短路径
    local c=sim.generateIkPath(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

function sysCall_threadmain()
    -- Initialization phase:
   --获取机械臂关节等物体Handle
    jh={-1,-1,-1,-1,-1,-1,-1}
    for i=1,7,1 do
        jh[i]=sim.getObjectHandle('j'..i)
    end
    ikGroup=sim.getIkGroupHandle('ik')
    ikTarget=sim.getObjectHandle('target')
    collisionPairs={sim.getCollectionHandle('manipulator'),sim.getCollectionHandle('environment')}
    target1=sim.getObjectHandle('testTarget1')
    target2=sim.getObjectHandle('testTarget2')
    target3=sim.getObjectHandle('testTarget3')
    target4=sim.getObjectHandle('testTarget4')
    approachDirectionObstacle=sim.getObjectHandle('approachDirectionObstacle')

    --一些参数
    --判断两个臂型距离的权重
    metric={0.5,1,1,0.5,0.1,0.2,0.1}
    forbidLevel=0
    ikShift=0.1
    ikSteps=50

    -- Main loop:
    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
        --在四个target中循环
        local theTarget=allTargets[targetIndex]
        targetIndex=targetIndex+1
        if targetIndex>4 then
            targetIndex=1
        end

        --类似于sim.getObjectPosition
        --matrix描述了物体的位姿，与转换矩阵T一致，不过最后一行的 0 0 0 1 没有返回来
        local m=sim.getObjectMatrix(theTarget,-1)

        --沿物体本身的z轴平移-ikShift之后的矩阵
        --这个平移的0.1m是为了让机械臂先到目标前面一点，再沿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).
        --为位姿m搜索一些臂型，并按照这些臂型与当前臂型的距离排序（距离小更好）
        -- In following function we also check for collisions and whether the
        -- final IK approach is feasable:
        --下面的函数也做了碰撞检测，以及判断逆解是否可行
        --displayInfo这个函数在调试时非常有用。。直接弹出一个窗口，打印
        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.
        --搜索从当前臂型到目标臂型的路径，对于每一个目标臂型，搜索六次，选择最短的
        -- Do this for the first 3 configs returned by findCollisionFreeConfigs.
        --对findCollisionFreeConfigs函数返回的前三个臂型执行上述操作
        -- Since we do not want an approach along the negative Z axis, we place
        -- an artificial obstacle into the scene (the blue orthogon):
        --由于不想沿着-Z轴靠近的路径，把一个人工设置的障碍物（蓝色长方形）放置在场景中
        local initialApproachDirectionObstaclePose=sim.getObjectMatrix(approachDirectionObstacle,-1)
        --把我们的小蓝片放在目标位置沿Z轴向后0.1m，再向前0.01m
        --真正的目标点是向后0.1m，再稍微向前一点点，其实这个0.01可有可无，不影响效果，0.02也可以
        sim.setObjectPosition(approachDirectionObstacle,theTarget,{0,0,-ikShift+0.01})
        sim.setObjectOrientation(approachDirectionObstacle,theTarget,{0,0,0})
        sim.switchThread() -- in order see the change before next operation locks
        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)

        --路径已经找到，再把我们的小蓝片放回去
        sim.setObjectMatrix(approachDirectionObstacle,-1,initialApproachDirectionObstaclePose)

        --运动到目标的偏移
        -- Follow the path:
        followPath(path)
        -- For the final approach, the target is the original target pose:
        m=sim.getObjectMatrix(theTarget,-1)

        --计算直线路径，这段可以直接直线运动过去
        -- Compute a straight-line path from current config to pose m:
        path=generateIkPath(getConfig(),m,ikSteps)
        -- Follow the path:
        followPath(path)
        
        --怎么来的还要怎么回去
        -- Generate a reversed path in order to move back:
        path=getReversedPath(path)
        -- Follow the path:
        followPath(path)
    end
end

看似很长其实逻辑很简单，但是里面有个ikShift有点绕，为了达到让机械臂先到目标位姿z轴前面一点，再沿z轴直线运动过去的效果，添加了一个approachDirectionObstacle。以及，利用ikShift这样一个0.1m的偏移，生成了一个相对目标位姿z轴偏移了0.1m的新目标位姿，这里称之为预目标位姿。
下面整理一下程序逻辑：
首先用一个函数getMatrixShiftedAlongZ得到预目标位姿，然后用函数findSeveralCollisionFreeConfigsAndCheckApproach得到满足预目标位姿的机械臂构型（这里通过调用另一个同名的重载函数（在里面调用了generateIkPath（里面又调用了sim.generateIkPath）），保证了得到的满足预目标位姿的机械臂构型与目标位姿之间存在无碰的直线路径），然后将小蓝片放置在预目标位姿z轴后面一点点，保证机械臂不会从反方向运动到预目标位姿，这样有了机械臂的初始构型和预目标构型，调用findShortestPath（在里面调用findPath（task在这））得到机械臂路径，然后控制机械臂运动就好了。
值得一提的时，这次的程序中涉及到线程，使用了forbidThreadSwitches()、sim.switchThread()、sim.setThreadAutomaticSwitch()等函数，forbidThreadSwitches(true)和forbidThreadSwitches(false)成对使用，类似线程加锁解锁，很方便。
还涉及了简单的UI，displayInfo()，这个函数调试的时候很好用。
又到了惯例的上效果时间：

可以看到，通过添加多个目标位姿，手动设置障碍物等，可以灵活的实现机械臂避障的功能。