UR5 机械臂 Lua 脚本：目标物体识别与抓取

function sysCall_init()\ncorout=coroutine.create(coroutineMain)\nend\n\nfunction sysCall_actuation()\n\tif coroutine.status(corout)~='dead' then\n\t\tlocal ok,errorMsg=coroutine.resume(corout)\n\t\tif errorMsg then\n\t\t\terror(debug.traceback(corout,errorMsg),2)\n\t\tend\n\tend\nend\n\nfunction suctionCupCmd(command)\n\tmodelBase=sim.getObjectHandle(sim.handle_self)\n\tmodelName=sim.getObjectName(modelBase)\n\tif(command=='on') then\n\t\tsim.setInt32Signal(modelName .."_suctionCup",1)\n\telse if(command=='off') then\n\t\tsim.setInt32Signal(modelName .."_suctionCup",0)\n\t\tend\n\tend\nend\n\nenableIk=function(enable)\n\tif enable then\n\t\tsim.setObjectMatrix(ikTarget,-1,sim.getObjectMatrix(ikTip,-1))\n\t\tfor i=1,#jointHandles,1 do\n\t\t\tsim.setJointMode(jointHandles[i],sim.jointmode_ik,1)\n\t\tend\n\t\n\t\tsim.setExplicitHandling(ikGroupHandle,0)\n\telse\n\t\tsim.setExplicitHandling(ikGroupHandle,1)\n\t\tfor i=1,#jointHandles,1 do\n\t\t\tsim.setJointMode(jointHandles[i],sim.jointmode_force,0)\n\t\tend\n\tend\nend\n\nfunction coroutineMain()\n\n\tjointHandles={-1,-1,-1,-1,-1,-1}\n\tfor i=1,6,1 do\n\t\tjointHandles[i]=sim.getObjectHandle('UR5_joint'..i..'#2')\n\tend\n\t\n\tikGroupHandle=sim.getIkGroupHandle('UR5#2')\n\tikTip=sim.getObjectHandle('UR5_ik#2')\n\tikTarget=sim.getObjectHandle('UR5_target#2')\n\twaitHandle = sim.getObjectHandle('wait#2')\n\tcam = sim.getObjectHandle('Vision_sensor0#2')\n\tsensor=sim.getObjectHandle('Beltsensor#2')\n\n\t-- Set-up some of the RML vectors:\n\tvel=180\n\taccel=40\n\tjerk=80\n\tcurrentVel={0,0,0,0,0,0,0}\n\tcurrentAccel={0,0,0,0,0,0,0}\n\tmaxVel={velmath.pi/180,velmath.pi/180,velmath.pi/180,velmath.pi/180,velmath.pi/180,velmath.pi/180}\n\tmaxAccel={accelmath.pi/180,accelmath.pi/180,accelmath.pi/180,accelmath.pi/180,accelmath.pi/180,accelmath.pi/180}\n\tmaxJerk={jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180}\n\ttargetVel={0,0,0,0,0,0}\n\n\tikMaxVel0={0.6,0.6,0.6,1.8}\n\tikMaxAccel0={0.8,0.8,0.8,0.9}\n\tikMaxVel={0.9,0.9,0.9,2.7}\n\tikMaxAccel={1.2,1.2,1.2,1.35}\n\tikMaxVel2={18,18,18,48}\n\tikMaxAccel2={24,24,24,27}\n\tikMaxJerk={0.6,0.6,0.6,0.8}\n\t\n\tposition = {{-0.1,1.6},{-0.1,1.675},{-0.1,1.75},\n\t\t\t\t\t\t{-0.025,1.6},{-0.025,1.675},{-0.025,1.75},\n\t\t\t\t\t\t{0.05,1.6},{0.05,1.675},{0.05,1.75}}\n\ttempposition = {0.25,1.6}\n\toccupy={}\n\tfor i=1,9,1 do\n\t\toccupy[i]=0\n\tend\n\t--sim.addStatusbarMessage(string.format("**"))\n\ttempoccupy=0\n\t\n\tlocal waitPos = sim.getObjectPosition(waitHandle,-1)\n\twaitQuat = sim.getObjectQuaternion(waitHandle,-1)\n\tenableIk(true)\n\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)\n\t--sim.wait(0.1)\n\t--size={0,1,1,0,1,1,1,0}\n\twhile true do\n\t\tif true then\n\t\t\tsim.waitForSignal('arrive6')\n\t\t\tlocal data = {}\n\t\t\tdata = sim.unpackTable(sim.readCustomDataBlock(cam, 'data2'))\n\t\t\tif(#data~=0) then\n\t\t\tfor i=1,#data,1 do\n\t\t\t\tlocal size=0\n\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.34},waitQuat,nil)\n\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel0,ikMaxAccel0,ikMaxJerk,{data[i][1],data[i][2],0.30},waitQuat,nil)\n\t\t\t\tsim.wait(0.1)\n\t\t\t\tif sim.readProximitySensor(sensor)>0 then\n\t\t\t\t\tsize=1\n\t\t\t\tend\n\t\t\t\tif(size==1) then\n\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{data[i][1],data[i][2],0.29},waitQuat,nil)\n\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\telse\n\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{data[i][1],data[i][2],0.28},waitQuat,nil)\n\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\tend\n\t\t\t\tsuctionCupCmd('on')\n\t\t\t\tsim.wait(0.06)\n\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.38},waitQuat,nil)\n\t\t\t\t--sim.wait(0.1)\n\t\t\t\tif(i==#data) then\n\t\t\t\t\tsim.setInt32Signal('finish6',1)\n\t\t\t\t\tsim.clearInt32Signal('arrive6')\n\t\t\t\tend\n\t\t\t\tsim.setObjectOrientation(waitHandle,-1, {0,0,-data[i][3]})\n\t\t\t\tlocal temp = sim.getObjectQuaternion(waitHandle,-1)\n\t\t\t\tlocal targetQuat = {temp[1], temp[2], temp[3], temp[4]}\n\t\t\t\tsim.setObjectOrientation(waitHandle,-1, {0,0,0})\n\t\t\t\tif(size==0) then\n\t\t\t\t\tlocal n=1\n\t\t\t\t\tfor j=1,9,1 do\n\t\t\t\t\t\tif(occupy[j]~=2) then\n\t\t\t\t\t\t\tn=n+1\n\t\t\t\t\t\telse\n\t\t\t\t\t\t\tbreak\n\t\t\t\t\t\tend\n\t\t\t\t\tend\n\t\t\t\t\tif(n==10) then\n\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],0.35},targetQuat,nil)\n\t\t\t\t\t\tif(tempoccupy==0) then\n\t\t\t\t\t\t\theight=0.04\n\t\t\t\t\t\telse\n\t\t\t\t\t\t\theight=0.04tempoccupy+0.04\n\t\t\t\t\t\tend\n\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],height+0.1},targetQuat,nil)\n\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{tempposition[1],tempposition[2],height+0.008},targetQuat,nil)\n\t\t\t\t\t\tsim.wait(0.05)\n\t\t\t\t\t\tsuctionCupCmd('off')\n\t\t\t\t\t\tsim.wait(0.05)\n\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],0.33},targetQuat,nil)\n\t\t\t\t\t\ttempoccupy=tempoccupy+1\n\t\t\t\t\t\telse\n\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.36},targetQuat,nil)\n\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.22},targetQuat,nil)\n\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{position[n][1],position[n][2],0.14+0.005},targetQuat,nil)\n\t\t\t\t\t\tsim.wait(0.05)\n\t\t\t\t\t\tsuctionCupCmd('off')\n\t\t\t\t\t\tsim.wait(0.05)\n\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.33},targetQuat,nil)\n\t\t\t\t\t\toccupy[n]=3\n\t\t\t\t\tend\n\t\t\t\telse\n\t\t\t\t\tlocal m=1\n\t\t\t\t\tfor j=1,9,1 do\n\t\t\t\t\t\tif(occupy[j]>=2) then\n\t\t\t\t\t\t\tm=m+1\n\t\t\t\t\t\telse\n\t\t\t\t\t\t\tbreak\n\t\t\t\t\t\tend\n\t\t\t\t\tend\n\t\t\t\t\t--sim.addStatusbarMessage(string.format(m))\n\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[m][1],position[m][2],0.35},targetQuat,nil)\n\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[m][1],position[m][2],0.22},targetQuat,nil)\n\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\t\tif(occupy[m]==0) then\n\t\t\t\t\t\theight=0.05\n\t\t\t\t\telse\n\t\t\t\t\t\theight=0.05occupy[m]+0.05\n\t\t\t\t\tend\n\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{position[m][1],position[m][2],height+0.005},targetQuat,nil)\n\t\t\t\t\tsim.wait(0.05)\n\t\t\t\t\tsuctionCupCmd('off')\n\t\t\t\t\tsim.wait(0.05)\n\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[m][1],position[m][2],0.33},targetQuat,nil)\n\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\t\toccupy[m]=occupy[m]+1\n\t\t\t\tend\n\t\t\t\tend\n\t\t\telse\n\t\t\t\tsim.setInt32Signal('finish6',1)\n\t\t\t\tsim.clearInt32Signal('arrive6')\n\t\t\tend\n\t\t\tif(tempoccupy~=0) then\n\t\t\t\tlocal p=tempoccupy\n\t\t\t\tfor i=1,p,1 do\n\t\t\t\t\tlocal n=1\n\t\t\t\t\tfor j=1,9,1 do\n\t\t\t\t\t\tif(occupy[j]~=2) then\n\t\t\t\t\t\t\tn=n+1\n\t\t\t\t\t\telse\n\t\t\t\t\t\t\tbreak\n\t\t\t\t\t\tend\n\t\t\t\t\tend\n\t\t\t\t\tif(n==10) then\n\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)\n\t\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\t\t\tbreak\n\t\t\t\t\telse\n\t\t\t\t\t\thigh=0.04*tempoccupy\n\t\t\t\t\t\tif(high>0.3) then\n\t\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],high+0.01},waitQuat,nil)\n\t\t\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\t\t\telse\n\t\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],high+0.1},waitQuat,nil)\n\t\t\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{tempposition[1],tempposition[2],high+0.005},waitQuat,nil)\n\t\t\t\t\t\t\tsim.wait(0.05)\n\t\t\t\t\t\t\tsuctionCupCmd('on')\n\t\t\t\t\t\t\tsim.wait(0.05)\n\t\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{tempposition[1],tempposition[2],0.3},waitQuat,nil)\n\t\t\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\t\t\t\t--sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.3},waitQuat,nil)\n\t\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.22},waitQuat,nil)\n\t\t\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{position[n][1],position[n][2],0.14+0.005},waitQuat,nil)\n\t\t\t\t\t\t\tsim.wait(0.05)\n\t\t\t\t\t\t\tsuctionCupCmd('off')\n\t\t\t\t\t\t\tsim.wait(0.05)\n\t\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position[n][1],position[n][2],0.33},waitQuat,nil)\n\t\t\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\t\t\t\ttempoccupy=tempoccupy-1\n\t\t\t\t\t\t\toccupy[n]=3\n\t\t\t\t\t\tend\n\t\t\t\t\tend\n\t\t\t\tend\n\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)\n\t\t\t--sim.wait(0.1)\n\t\t\telse\n\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)\n\t\t\t\t--sim.wait(0.1)\n\t\t\tend\n\t\tend\n\tend\n\tenableIk(false)\nend