第三篇，終于來到了激動人心的機械臂路徑規(guī)劃環(huán)節(jié)
motionPlanningDemo1.ttt
這個scene相比于前面兩個就稍微復雜一點了，除了機械臂，障礙物，還多了approachDirectionObstacle，而且涉及到機械臂逆解和碰撞檢測的相關內(nèi)容，我在文章里會稍微介紹一下，但是最好還是先看一下User Manual里面Calculation modules這一節(jié)

首先看一下Scene Hierarchy

除了常規(guī)的逆解結(jié)構(gòu)，包括target和tip，還在tip的相同位置和姿態(tài)下多了一個ReferenceFrame，其作用同樣放在代碼中解釋:

有機械臂和環(huán)境兩個
配置基本上是這樣了，下面正餐來了，開始看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)
    --用于尋找滿足預目標位姿的機械臂構(gòu)型，generateIkPath函數(shù)用于判斷預目標位姿和目標位姿之間能否直接通過
    -- 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位姿的機械臂構(gòu)型，參數(shù)：
    --ikHandle
    --機械臂關節(jié) Handle
    --thresholdDist，與精確解的距離閾值，小于這個算滿足的逆解
    --maxTimeInMs:最大計算時間
    --距離計算方式
    --collisionPairs，默認二范數(shù)
    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)
    --獲取當前機械臂關節(jié)角
    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:
   --獲取機械臂關節(jié)等物體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')

    --一些參數(shù)
    --判斷兩個臂型距離的權(quán)重
    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中循環(huán)
        local theTarget=allTargets[targetIndex]
        targetIndex=targetIndex+1
        if targetIndex>4 then
            targetIndex=1
        end

        --類似于sim.getObjectPosition
        --matrix描述了物體的位姿，與轉(zhuǎn)換矩陣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:
        --下面的函數(shù)也做了碰撞檢測，以及判斷逆解是否可行
        --displayInfo這個函數(shù)在調(diào)試時非常有用。。直接彈出一個窗口，打印
        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函數(shù)返回的前三個臂型執(zhí)行上述操作
        -- 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)

        --路徑已經(jīng)找到，再把我們的小藍片放回去
        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的新目標位姿，這里稱之為預目標位姿。
下面整理一下程序邏輯：
首先用一個函數(shù)getMatrixShiftedAlongZ得到預目標位姿，然后用函數(shù)findSeveralCollisionFreeConfigsAndCheckApproach得到滿足預目標位姿的機械臂構(gòu)型（這里通過調(diào)用另一個同名的重載函數(shù)（在里面調(diào)用了generateIkPath（里面又調(diào)用了sim.generateIkPath）），保證了得到的滿足預目標位姿的機械臂構(gòu)型與目標位姿之間存在無碰的直線路徑），然后將小藍片放置在預目標位姿z軸后面一點點，保證機械臂不會從反方向運動到預目標位姿，這樣有了機械臂的初始構(gòu)型和預目標構(gòu)型，調(diào)用findShortestPath（在里面調(diào)用findPath（task在這））得到機械臂路徑，然后控制機械臂運動就好了。
值得一提的時，這次的程序中涉及到線程，使用了forbidThreadSwitches()、sim.switchThread()、sim.setThreadAutomaticSwitch()等函數(shù)，forbidThreadSwitches(true)和forbidThreadSwitches(false)成對使用，類似線程加鎖解鎖，很方便。
還涉及了簡單的UI，displayInfo()，這個函數(shù)調(diào)試的時候很好用。
又到了慣例的上效果時間：

可以看到，通過添加多個目標位姿，手動設置障礙物等，可以靈活的實現(xiàn)機械臂避障的功能。