UR5 机械臂自动抓取任务 Lua 脚本逐行分析

该脚本是一个 Lua 脚本，主要实现了 UR5 机械臂在 V-REP 仿真环境中的自动抓取任务。下面是逐行分析：

function suctionCupCmd(command) modelBase=sim.getObjectAssociatedWithScript(sim.handle_self) modelName=sim.getObjectName(modelBase) if(command=='on') then sim.setIntegerSignal(modelName ..'_suctionCup',1) else if(command=='off') then sim.setIntegerSignal(modelName ..'_suctionCup',0) end end end

该函数用于控制机械臂末端的吸盘开关，当 command 为 'on' 时，打开吸盘，设置信号为 1；当 command 为 'off' 时，关闭吸盘，设置信号为 0。

enableIk=function(enable) if enable then sim.setObjectMatrix(ikTarget,-1,sim.getObjectMatrix(ikTip,-1)) for i=1,#jointHandles,1 do sim.setJointMode(jointHandles[i],sim.jointmode_ik,1) end
```
 sim.setExplicitHandling(ikGroupHandle,0)
```
else sim.setExplicitHandling(ikGroupHandle,1) for i=1,#jointHandles,1 do sim.setJointMode(jointHandles[i],sim.jointmode_force,0) end end end

该函数用于控制机械臂的运动方式，当 enable 为 true 时，将机械臂的运动模式设置为 IK 模式；当 enable 为 false 时，将机械臂的运动模式设置为力控模式。

function sysCall_threadmain()

jointHandles={-1,-1,-1,-1,-1,-1} for i=1,6,1 do jointHandles[i]=sim.getObjectHandle('UR5_joint'..i) end

ikGroupHandle=sim.getIkGroupHandle('UR5') ikTip=sim.getObjectHandle('UR5_ik') ikTarget=sim.getObjectHandle('UR5_target') waitHandle = sim.getObjectHandle('wait') cam = sim.getObjectHandle('Vision_sensor0') sensor=sim.getObjectHandle('Beltsensor')

-- Set-up some of the RML vectors: vel=180 accel=40 jerk=80 currentVel={0,0,0,0,0,0,0} currentAccel={0,0,0,0,0,0,0} maxVel={velmath.pi/180,velmath.pi/180,velmath.pi/180,velmath.pi/180,velmath.pi/180,velmath.pi/180} maxAccel={accelmath.pi/180,accelmath.pi/180,accelmath.pi/180,accelmath.pi/180,accelmath.pi/180,accelmath.pi/180} maxJerk={jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180} targetVel={0,0,0,0,0,0}

ikMaxVel0={0.6,0.6,0.6,1.8} ikMaxAccel0={0.8,0.8,0.8,0.9} ikMaxVel={0.9,0.9,0.9,2.7} ikMaxAccel={1.2,1.2,1.2,1.35} ikMaxVel2={18,18,18,48} ikMaxAccel2={24,24,24,27} ikMaxJerk={0.6,0.6,0.6,0.8}

position = {{-0.35,1.6},{-0.35,1.675},{-0.35,1.75}, {-0.425,1.6},{-0.425,1.675},{-0.425,1.75}, {-0.5,1.6},{-0.5,1.675},{-0.5,1.75}} tempposition = {-0.7,1.6} occupy={} for i=1,9,1 do occupy[i]=0 end tempoccupy=0

local waitPos = sim.getObjectPosition(waitHandle,-1) waitQuat = sim.getObjectQuaternion(waitHandle,-1) enableIk(true) sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil) while true do if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then sim.waitForSignal('arrive5') local data = {} data = sim.unpackTable(sim.readCustomDataBlock(cam, 'data')) if(#data~=0) then for i=1,#data,1 do local size=0 sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.33},waitQuat,nil) sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel0,ikMaxAccel0,ikMaxJerk,{data[i][1],data[i][2],0.30},waitQuat,nil) sim.wait(0.1) if sim.readProximitySensor(sensor)>0 then size=1 end if(size==1) then sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{data[i][1],data[i][2],0.29},waitQuat,nil) else sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{data[i][1],data[i][2],0.28},waitQuat,nil) end suctionCupCmd('on') sim.wait(0.08) sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.38},waitQuat,nil) if(i==#data) then sim.setIntegerSignal('finish5',1) sim.clearIntegerSignal('arrive5') end sim.setObjectOrientation(waitHandle, -1, {0,0,-data[i][3]}) local temp = sim.getObjectQuaternion(waitHandle, -1) local targetQuat = {temp[1], temp[2], temp[3], temp[4]} sim.setObjectOrientation(waitHandle, -1, {0,0,0}) if(size==0) then local n=1 for j=1,9,1 do if(occupy[j]~=2) then n=n+1 else break end end if(n==10) then sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],0.35},targetQuat,nil) if(tempoccupy==0) then height=0.04 else height=0.04tempoccupy+0.04 end sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],height+0.1},targetQuat,nil) sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{tempposition[1],tempposition[2],height+0.01},targetQuat,nil) sim.wait(0.08) suctionCupCmd('off') sim.wait(0.05) sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],0.33},targetQuat,nil) tempoccupy=tempoccupy+1 else sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.35},targetQuat,nil) sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.22},targetQuat,nil) sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{position[n][1],position[n][2],0.14+0.005},targetQuat,nil) sim.wait(0.05) suctionCupCmd('off') sim.wait(0.05) sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.33},targetQuat,nil) occupy[n]=3 end else local m=1 for j=1,9,1 do if(occupy[j]>=2) then m=m+1 else break end end sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[m][1],position[m][2],0.35},targetQuat,nil) sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[m][1],position[m][2],0.22},targetQuat,nil) if(occupy[m]==0) then height=0.05 else height=0.05occupy[m]+0.05 end sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{position[m][1],position[m][2],height+0.005},targetQuat,nil) sim.wait(0.05) suctionCupCmd('off') sim.wait(0.05) sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[m][1],position[m][2],0.33},targetQuat,nil) occupy[m]=occupy[m]+1 end end else sim.setIntegerSignal('finish5',1) sim.clearIntegerSignal('arrive5') end
```
     if(tempoccupy~=0) then
         local p=tempoccupy
         for i=1,p,1 do
             local n=1
             for j=1,9,1 do
                 if(occupy[j]~=2) then
                     n=n+1
                 else
                     break
                 end
             end
             if(n==10) then
                 sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)
                 break
             else
                 high=0.04*tempoccupy
                 if(high>0.3) then
                     sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],high+0.1},waitQuat,nil)
                 else
                     --sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],0.3},waitQuat,nil)
                     sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],high+0.1},waitQuat,nil)
                     sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{tempposition[1],tempposition[2],high+0.005},waitQuat,nil)
                     sim.wait(0.05)
                     suctionCupCmd('on')
                     sim.wait(0.05)
                     sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],0.3},waitQuat,nil)
                     --sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.3},waitQuat,nil)
                     sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.22},waitQuat,nil)
                     sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{position[n][1],position[n][2],0.14+0.005},waitQuat,nil)
                     sim.wait(0.05)
                     suctionCupCmd('off')
                     sim.wait(0.05)
                     sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.33},waitQuat,nil)
                     tempoccupy=tempoccupy-1
                     occupy[n]=3
                 end
             end
         end
         sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)
     else
         sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)
     end
 end
```
end enableIk(false) end