V-REP仿真环境中机器人抓取操作Lua脚本

"function sysCall_init()\n\tcorout=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\n-- ON OFF\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\n-- TURE FALSE\nenableIk=function(enable)\n\tif enable then\n\t\tsim.setObjectMatrix(ikTarget,-1,sim.getObjectMatrix(ikTip,-1))\n\t\t\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\t\n\t\tfor i=1,#jointHandles,1 do\n\t\t\tsim.setJointMode(jointHandles[i],sim.jointmode_force,0)\n\t\tend\n\t\t\n\tend\nend\n\nfunction coroutineMain()\n\tjointHandles={-1,-1,-1,-1,-1,-1}\n\tfor i=1,6,1 do\n\t\tjointHandles[i]=sim.getObjectHandle('UR5_joint'..i..'#1')\n\tend\n\t\n\tikGroupHandle=sim.getIkGroupHandle('UR51')\n\tikTip=sim.getObjectHandle('UR5_ik1')\n\tikTarget=sim.getObjectHandle('UR5_target1')\n\tcam=sim.getObjectHandle('Vision_sensor0#0')\n\tsensor=sim.getObjectHandle('Beltsensor#1')\n\t--Pioneer=sim.getObjectHandle('Pioneer_p3dx#0')\n\twaitHandle = sim.getObjectHandle('wait#0')\n\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\t--ikMaxVel={0.6,0.6,0.6,1.8}\n\t--ikMaxAccel={0.8,0.8,0.8,0.9}\n\tikMaxVel={1.2,1.2,1.2,3.6}\n\tikMaxAccel={1.6,1.6,1.6,1.8}\n\tikMaxVel2={12,12,12,36}\n\tikMaxAccel2={16,16,16,18}\n\tikMaxJerk={0.6,0.6,0.6,0.8}\n\t\n\tstartPos = sim.getObjectPosition(waitHandle,-1)\n\tworkQuat = sim.getObjectQuaternion(waitHandle,-1)\n\tstertQuat = sim.getObjectQuaternion(ikTip,-1)\n\t\n\tenableIk(true)\n\twhile true do\n\t\tif true then\n\t\t\tsim.waitForSignal('getdata')\n\t\t\tsim.waitForSignal('arrive2')\n\t\t\tif(sim.getInt32Signal('arrive2')==1) then\n\t\t\t\tPioneer=sim.getObjectHandle('P1')\n\t\t\tend\n\t\t\tif(sim.getInt32Signal('arrive2')==2) then\n\t\t\t\tPioneer=sim.getObjectHandle('P2')\n\t\t\tend\n\t\t\tif(sim.getInt32Signal('arrive2')==3) then\n\t\t\t\tPioneer=sim.getObjectHandle('P3')\n\t\t\tend\n\t\t\tif(sim.getInt32Signal('arrive2')==4) then\n\t\t\t\tPioneer=sim.getObjectHandle('P4')\n\t\t\tend\n\t\t\tsim.clearInt32Signal('arrive2')\n\t\t\tlocal data={}\n\t\t\tdata = sim.unpackTable(sim.readCustomDataBlock(cam, 'data3'))\n\t\t\tPioneerPosition= sim.getObjectPosition(Pioneer,-1)\n\t\t\tPioneerPositionMatrix={{PioneerPosition[1]+0.075,PioneerPosition[2]+0.075},{PioneerPosition[1],PioneerPosition[2]+0.075},{PioneerPosition[1]-0.075,PioneerPosition[2]+0.075},\n\t\t\t\t\t\t\t\t\t{PioneerPosition[1]+0.075,PioneerPosition[2]},{PioneerPosition[1]-0.075,PioneerPosition[2]},{PioneerPosition[1]+0.075,PioneerPosition[2]-0.075},\n\t\t\t\t\t\t\t\t\t{PioneerPosition[1],PioneerPosition[2]-0.075},{PioneerPosition[1]-0.075,PioneerPosition[2]-0.075},{PioneerPosition[1],PioneerPosition[2]}}\n\t\t\t\n\t\t\tlocal PioneerPositionOccupy={}\n\t\t\tfor i=1,9,1 do\n\t\t\t\tPioneerPositionOccupy[i]=0\n\t\t\tend\n\t\t\tif(#data~=0) then\n\t\t\tfor i=4,1,-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.27},workQuat,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\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.155},workQuat,nil)\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.145},workQuat,nil)\n\t\t\t\tend\n\t\t\t\n\t\t\t\tsuctionCupCmd('on')\n\t\t\t\tsim.wait(0.05) --S\n\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.3},workQuat,nil)\n\t\t\t\tfor k=5,8,1 do\n\t\t\t\t\tif(PioneerPositionOccupy[k]==0) then\n\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.3},workQuat,nil)\n\t\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\t\t\tif(size==1) then\n\t\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.29},workQuat,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,ikMaxVel,ikMaxAccel,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.28},workQuat,nil)\n\t\t\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\t\t\tend\n\t\t\t\t\t\tsuctionCupCmd('off')\n\t\t\t\t\t\tsim.wait(0.02)\n\t\t\t\t\t\tsim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.3},workQuat,nil)\n\t\t\t\t\t\t--sim.wait(0.1)\n\t\t\t\t\t\tPioneerPositionOccupy[k]=1\n\t\t\t\t\t\tbreak\n\t\t\t\t\tend\n\t\t\t\tend\n\t\t\tend\n\t\t\telse\n\t\t\t\tsim.wait(8)\n\t\t\tend\n\t\t\tsim.setInt32Signal('finish2',1)\n\t\t\tsim.setInt32Signal('done',1)\n\t\tend\n\tend\n\tenableIk(false)\nend