机器人系统设计--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