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

首先看一下Scene Hierarchy

除了常規的逆解結構，包括target和tip，還在tip的相同位置和姿態下多了一個ReferenceFrame，其作用同樣放在代碼中解釋:

有機械臂和環境兩個
配置基本上是這樣了，下面正餐來了，開始看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()，這個函數調試的時候很好用。
又到了慣例的上效果時間：

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