机器人系统设计-coppeliasim仿真

一、用coppeliasim建立UR5机器人模型

1、首先获得机器人的模型，一般是stp格式的，这也是大多数三维建模软件都支持的网格文件，常用的三维建模软件有：犀牛、solidworks、3Dmax、proe等。

2、将模型分好部件后逐个导入，注意导入后部件在coppeliasim的位置默认是其他软件导出时的相对位置，所以三维软件建模时应该小心设置原点位置，一般将后面要设置关节的位置设为原点。另外注意三维建模软件中的模型单位和coppeliasim的单位的不同，需要进行转换。

3、将所有部件导入完成后，通过Menu bar --> Edit --> Decimate selected shape…命令简化模型。

4、给模型添加关节，主要是关节位置的设置和约束的设置。对于旋转关节，有一个方向的位置是无关紧要的。

5、设置部件的颜色，炸开部件后，逐个调整颜色。颜色调整为双击模型树前的图标，进入颜色界面，不足就是没有取色的功能，需要用笔记录自己设置的rgb值。

6、颜色设置完后组合部件，需要将同一个部件的所有部分都在模型树中选中，按ctrl多选。

7、调整层次结构，部件应该跟随上一个关节运动。对各部分进行重命名，双击，修改后enter确定。

8、双击模型树中的关节前的图标，进入关节初始角度和极限角度的设置。去除position is cyclic前的勾，设置关节的最小角度min和角度范围range（没有最大角度的设置）

9、添加脚本程序，主脚本一般不用修改，只用在最顶层的部件上加child script，可以选线程或非线程，线程和非现程的区别在于多个子脚本同时执行时的优先级问题，在这没有必要区别。具体的lua语言写机器人控制代码的可能要在比较后面的文章去介绍。

10、将所有部件设为一个整体，双击最顶层部件，在common属性中勾选object is model，就可以在你点最顶层部件的时候选中的是整个模型了。

11、将建好的模型导入到库中方便后续的使用。选中最顶层的部件，将该模型设为coppeliasim model，选着位置和编好名称后，建议从库中拖出该模型，仿真运行一下，避免脚本文件在下次使用时被删除。

二、UR5码垛代码

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

三、JAKA 码垛场景搭建

1、在JAKA机械臂的基础上加上力传感器，调整力传感器的位置和角度。

添加夹爪

先选夹爪，再选力传感器，再点对齐按钮。

易犯的错误：将力传感器和机械臂的末端对齐，而不是和力传感器对齐，这样会导致夹爪掉落。

2、建立ikTip和ikTarget，后者应该是前者的复制（同样的位姿），ikTip附属于力传感器，ikTarget附属于机械臂基座。

3、将UR5码垛场景下的机械臂的代码、夹爪的代码分别复制到JAKA机械臂和夹爪上。

4、调整机械臂子脚本中的名称和抓取和放置的位置。（抓取的位置不好调，可以调整传送带的位置以匹配）

精细的摆放需要不断地微调放置的位置（六个参数分别对应从基座到夹爪的六个关节的转动角度）

参考链接：

https://space.bilibili.com/272305415/?spm_id_from=333.999.0.0

https://www.bilibili.com/video/BV1v4411H7ez?p=1

https://blog.csdn.net/uuuuur/article/details/108028900

https://www.cnblogs.com/hsw-home/