機器人系統設計--coppeliasim程序解讀：UR5碼垛

博客園不支持lua語言，所以對於關鍵字和函數沒有顏色突出，將就着看吧。

使用的是coppeliasim自帶的場景。

（寫的很不完善，不過最近有新的理解，后面有時間再更新）

function sysCall_init()  --  syscall_init是不可選的初始化函數，用來創造一個協同例程（協程：coroutine），作為主腳本調用的接口（回調函數），只執行一次
    corout=coroutine.create(coroutineMain)
end

function sysCall_actuation()  --  syscall_actuation是可選的驅動函數，負責處理仿真的所有驅動功能，每次仿真都得執行一次，沒有它子腳本無法執行 if coroutine.status(corout)~='dead' then  -- 如果協程處在死掉的狀態
        local ok,errorMsg=coroutine.resume(corout)  --  將局部變量errorMsg賦值為協程恢復 if errorMsg then  --  errorMsg有值就為1
            error(debug.traceback(corout,errorMsg),2)  --  回溯錯誤並顯示在運行窗口
        end
    end
end

setGripperData=function(open,velocity,force)  --  設置夾爪的數據
    if not velocity then  --  設置初始速度
        velocity=0.11
    end
    if not force then  --  設置初始力
        force=20
    end
    if not open then  --  夾爪打開的狀態確定
        velocity=-velocity
    end
    
    local dat={}  --  局部變量dat
    dat.velocity=velocity
    dat.force=force
    sim.writeCustomDataBlock(gripperHandle,'activity',sim.packTable(dat))  --  寫數據塊
end

function moveToPoseCallback(q,velocity,accel,auxData)  --  對初始和目標位置進行插值（四個參數：q、速度、加速度、auxData）
    sim.setObjectPose(auxData.target,-1,q)  --  設置對象的位姿
    simIK.applyIkEnvironmentToScene(auxData.ikEnv,auxData.ikGroup)  --  添加一個IK元素到IK組，使用IK的方法進行逆運動學求解（會有奇異點（即雅克比矩陣不滿秩），fk的方法更好）
end
  --  auxData:將被轉發到回調函數的隨機數據。
  --  callback:回調函數:每一個移動步驟都會調用的回調函數。提供給回調函數的參數是:currentPose/currentMatrix, currentVel, currentAccel, auxData。
  --  targetPose/targetMatrix:目標姿態，通過一個姿態(x,y,z,qx,qy,qz,qw)或變換矩陣(省略4x4矩陣的最后一行)指定。

function moveToPose_viaIK(maxVelocity,maxAcceleration,maxJerk,targetQ,auxData)  --  執行運動函數進行運動，設置最大速度、最大加速度、最大加加速度、目標Q和auxData)
    local currentQ=sim.getObjectPose(auxData.tip,-1)  --  局部變量currentQ，獲得當前位姿
    return sim.moveToPose(-1,currentQ,maxVelocity,maxAcceleration,maxJerk,targetQ,moveToPoseCallback,auxData,nil)  --  返回值
end

function moveToConfigCallback(config,velocity,accel,auxData)
    for i=1,#auxData.joints,1 do
        local jh=auxData.joints[i]
        if sim.getJointMode(jh)==sim.jointmode_force and sim.isDynamicallyEnabled(jh) then  --  檢索關節操作模式
            sim.setJointTargetPosition(jh,config[i])  --  設置目標位置的auxData（感覺應該是一個矩陣，后面會對其進行調整設置）
        else    
            sim.setJointPosition(jh,config[i])
        end
    end
end

function moveToConfig_viaFK(maxVelocity,maxAcceleration,maxJerk,goalConfig,auxData)
    local startConfig={}
    for i=1,#auxData.joints,1 do
        startConfig[i]=sim.getJointPosition(auxData.joints[i])
    end
    sim.moveToConfig(-1,startConfig,nil,nil,maxVelocity,maxAcceleration,maxJerk,goalConfig,nil,moveToConfigCallback,auxData,nil)
end

function coroutineMain()
    -- Initialize some values:
    local simJoints={}
    for i=1,6,1 do
        simJoints[i]=sim.getObject('./joint',{index=i-1})  --  讀取關節句柄，如果是joint1的格式應該使用‘joint’..1的格式
    end
    local simTip=sim.getObject('./ikTip')
    local simTarget=sim.getObject('./ikTarget')  --  ikTip和ikTarget在同一個位置，用於結束后使機械臂回到初始位置
    local modelBase=sim.getObject('.')
    gripperHandle=sim.getObject('./RG2')
    
    ikEnv=simIK.createEnvironment()

    -- Prepare the ik group, using the convenience function 'simIK.addIkElementFromScene':
    ikGroup=simIK.createIkGroup(ikEnv)
    simIK.addIkElementFromScene(ikEnv,ikGroup,modelBase,simTip,simTarget,simIK.constraint_pose)

    -- FK movement data:  --  ik是逆向運動學（已知目標位置），FK是正向運動學（對關節直接控制）
    local initConf={0,0,0,0,0,0}
    local vel=180
    local accel=40
    local jerk=80
    local maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
    local maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
    local maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}

    -- IK movement data:
    local ikMaxVel={0.4,0.4,0.4,1.8}
    local ikMaxAccel={0.8,0.8,0.8,0.9}
    local ikMaxJerk={0.6,0.6,0.6,0.8}

    local pickConfig={-70.1*math.pi/180,18.85*math.pi/180,93.18*math.pi/180,68.02*math.pi/180,109.9*math.pi/180,90*math.pi/180}  --  夾爪抓取的位置
    local dropConfig1={-183.34*math.pi/180,14.76*math.pi/180,78.26*math.pi/180,-2.98*math.pi/180,-90.02*math.pi/180,86.63*math.pi/180}  --  夾爪釋放的第一個位置
    local dropConfig2={-197.6*math.pi/180,14.76*math.pi/180,78.26*math.pi/180,-2.98*math.pi/180,-90.02*math.pi/180,72.38*math.pi/180}  --  夾爪釋放的第二個位置
    local dropConfig3={-192.1*math.pi/180,3.76*math.pi/180,91.16*math.pi/180,-4.9*math.pi/180,-90.02*math.pi/180,-12.13*math.pi/180}  --  夾爪釋放的第三個位置
    local dropConfig4={-189.38*math.pi/180,24.94*math.pi/180,64.36*math.pi/180,0.75*math.pi/180,-90.02*math.pi/180,-9.41*math.pi/180}  --  夾爪釋放的第四個位置

    local dropConfigs={dropConfig1,dropConfig2,dropConfig3,dropConfig4}
    local dropConfigIndex=1
    local droppedPartsCnt=0

    setGripperData(true)
    sim.setInt32Param(sim.intparam_current_page,0)

    local data={}
    data.ikEnv=ikEnv
    data.ikGroup=ikGroup
    data.tip=simTip
    data.target=simTarget
    data.joints=simJoints
    
    while droppedPartsCnt<6 do
        moveToConfig_viaFK(maxVel,maxAccel,maxJerk,pickConfig,data)
        sim.setInt32Param(sim.intparam_current_page,1)

        local pose=sim.getObjectPose(simTip,-1)
        pose[1]=pose[1]+0.105
        moveToPose_viaIK(ikMaxVel,ikMaxAccel,ikMaxJerk,pose,data)

        setGripperData(false)
        sim.wait(0.5)

        pose[2]=pose[2]-0.2
        pose[3]=pose[3]+0.2
        moveToPose_viaIK(ikMaxVel,ikMaxAccel,ikMaxJerk,pose,data)

        sim.setInt32Param(sim.intparam_current_page,0)

        moveToConfig_viaFK(maxVel,maxAccel,maxJerk,dropConfigs[dropConfigIndex],data)

        sim.setInt32Param(sim.intparam_current_page,2)
        local pose=sim.getObjectPose(simTip,-1)
        local pose2=sim.copyTable(pose)
        pose[3]=0.025+0.05*math.floor(0.1+droppedPartsCnt/2)
        moveToPose_viaIK(ikMaxVel,ikMaxAccel,ikMaxJerk,pose,data)

        setGripperData(true)
        sim.wait(0.5)

        moveToPose_viaIK(ikMaxVel,ikMaxAccel,ikMaxJerk,pose2,data)


        sim.setInt32Param(sim.intparam_current_page,0)

        dropConfigIndex=dropConfigIndex+1
        if dropConfigIndex>4 then
            dropConfigIndex=1
        end

        droppedPartsCnt=droppedPartsCnt+1
    end

    moveToConfig_viaFK(maxVel,maxAccel,maxJerk,initConf,data)
    sim.stopSimulation()
end