UR5机器人吸盘控制和逆运动学Lua脚本

function suctionCupCmd(command)\n modelBase=sim.getObjectAssociatedWithScript(sim.handle_self)\n modelName=sim.getObjectName(modelBase)\n if(command=='on') then\n sim.setIntegerSignal(modelName .."_suctionCup",1)\n else if(command=='off') then\n sim.setIntegerSignal(modelName .."_suctionCup",0)\n end\n end\n end\n\nenableIk=function(enable)\n if enable then\n sim.setObjectMatrix(ikTarget,-1,sim.getObjectMatrix(ikTip,-1))\n for i=1,#jointHandles,1 do\n sim.setJointMode(jointHandles[i],sim.jointmode_ik,1)\n end\n\n sim.setExplicitHandling(ikGroupHandle,0)\n else\n sim.setExplicitHandling(ikGroupHandle,1)\n for i=1,#jointHandles,1 do\n sim.setJointMode(jointHandles[i],sim.jointmode_force,0)\n end\n end\n end\n\nfunction sysCall_threadmain()\n\n jointHandles={-1,-1,-1,-1,-1,-1}\n for i=1,6,1 do\n jointHandles[i]=sim.getObjectHandle('UR5_joint'..i)\n end\n\n ikGroupHandle=sim.getIkGroupHandle('UR5')\n ikTip=sim.getObjectHandle('UR5_ik')\n ikTarget=sim.getObjectHandle('UR5_target')\n waitHandle = sim.getObjectHandle('wait#2')\n cam1 = sim.getObjectHandle('Vision_sensor0')\n cam2 = sim.getObjectHandle('Vision_sensor0#2')\n sensor=sim.getObjectHandle('Beltsensor')\n\n -- Set-up some of the RML vectors:\n vel=180\n accel=40\n jerk=80\n currentVel={0,0,0,0,0,0,0}\n currentAccel={0,0,0,0,0,0,0}\n maxVel={velmath.pi/180,velmath.pi/180,velmath.pi/180,velmath.pi/180,velmath.pi/180,velmath.pi/180}\n maxAccel={accelmath.pi/180,accelmath.pi/180,accelmath.pi/180,accelmath.pi/180,accelmath.pi/180,accelmath.pi/180}\n maxJerk={jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180,jerkmath.pi/180}\n targetVel={0,0,0,0,0,0}\n\n ikMaxVel0={0.6,0.6,0.6,1.8}\n ikMaxAccel0={0.8,0.8,0.8,0.9}\n ikMaxVel={0.9,0.9,0.9,2.7}\n ikMaxAccel={1.2,1.2,1.2,1.35}\n ikMaxVel2={18,18,18,48}\n ikMaxAccel2={24,24,24,27}\n ikMaxJerk={0.6,0.6,0.6,0.8}\n \n position1 = {{-0.7,1.6},{-0.7,1.675},{-0.7,1.75},\n {-0.625,1.6},{-0.625,1.675},{-0.625,1.75},\n {-0.55,1.6},{-0.55,1.675},{-0.55,1.75}\n }\n\n \n position2 = {{0.25,1.6},{0.25,1.675},{0.25,1.75},\n {0.175,1.6},{0.175,1.675},{0.175,1.75},\n {0.1,1.6},{0.1,1.675},{0.1,1.75}}\noccupy1={}\n for i=1,9,1 do\n occupy1[i]=0\n end\n occupy2={}\n for i=1,9,1 do\n occupy2[i]=0\n end\n tempoccupy=0\n \n local waitPos = sim.getObjectPosition(waitHandle,-1)\n waitQuat = sim.getObjectQuaternion(waitHandle,-1)\n enableIk(true)\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)\n while true do\n if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then\n sim.waitForSignal('arrive5')\n local data1 = {}\n data1 = sim.unpackTable(sim.readCustomDataBlock(cam1, 'data'))\n if(#data1~=0) then\n for i=1,#data1,1 do\n local size=0\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data1[i][1],data1[i][2],0.38},waitQuat,nil)\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel0,ikMaxAccel0,ikMaxJerk,{data1[i][1],data1[i][2],0.30},waitQuat,nil)\n sim.wait(0.1)\n if sim.readProximitySensor(sensor)>0 then\n size=1\n end\n if(size==1) then\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{data1[i][1],data1[i][2],0.29},waitQuat,nil)\n end\n suctionCupCmd('on')\n sim.wait(0.06)\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data1[i][1],data1[i][2],0.38},waitQuat,nil)\n if(i==#data1) then\n sim.setIntegerSignal('finish5',1)\n sim.clearIntegerSignal('arrive5')\n end\n sim.setObjectOrientation(waitHandle, -1, {0,0,-data1[i][3]})\n local temp1 = sim.getObjectQuaternion(waitHandle, -1)\n local targetQuat1 = {temp1[1], temp1[2], temp1[3], temp1[4]}\n sim.setObjectOrientation(waitHandle, -1, {0,0,0})\n if(size==1) then\n local m=1\n for j=1,9,1 do\n if(occupy1[j]==3) then\n m=m+1\n else\n break\n end\n end\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position1[m][1],position1[m][2],0.35},targetQuat1,nil)\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position1[m][1],position1[m][2],0.24},targetQuat1,nil)\n if(occupy1[m]==0) then\n height=0.05\n else\n height=0.05occupy1[m]+0.05\n end\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{position1[m][1],position1[m][2],height+0.005},targetQuat1,nil)\n sim.wait(0.05)\n suctionCupCmd('off')\n sim.wait(0.05)\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position1[m][1],position1[m][2],0.33},targetQuat1,nil)\n occupy1[m]=occupy1[m]+1\n end\n end\n else\n sim.setIntegerSignal('finish5',1)\n sim.clearIntegerSignal('arrive5')\n end\n end\n \n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,waitPos,waitQuat,nil)\n \n \n if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then\n sim.waitForSignal('arrive6')\n local data2 = {}\n data2 = sim.unpackTable(sim.readCustomDataBlock(cam2, 'data2'))\n if(#data2~=0) then\n for i=1,#data2,1 do\n local size=0\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data2[i][1],data2[i][2],0.38},waitQuat,nil)\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel0,ikMaxAccel0,ikMaxJerk,{data2[i][1],data2[i][2],0.30},waitQuat,nil)\n sim.wait(0.1)\n if sim.readProximitySensor(sensor)>0 then\n size=1\n end\n if(size==1) then\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{data2[i][1],data2[i][2],0.29},waitQuat,nil)\n else\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{data2[i][1],data2[i][2],0.28},waitQuat,nil)\n end\n suctionCupCmd('on')\n sim.wait(0.06)\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data2[i][1],data2[i][2],0.38},waitQuat,nil)\n if(i==#data2) then\n sim.setIntegerSignal('finish6',1)\n sim.clearIntegerSignal('arrive6')\n end\n sim.setObjectOrientation(waitHandle, -1, {0,0,-data2[i][3]})\n local temp2 = sim.getObjectQuaternion(waitHandle, -1)\n local targetQuat2 = {temp2[1], temp2[2], temp2[3], temp2[4]}\n sim.setObjectOrientation(waitHandle, -1, {0,0,0})\n if(size==1) then\n local n=1\n for j=1,9,1 do\n if(occupy2[j]==3) then\n n=n+1\n else\n break\n end\n end\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position2[n][1],position2[n][2],0.35},targetQuat2,nil)\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position2[n][1],position2[n][2],0.22},targetQuat2,nil)\n if(occupy2[n]==0) then\n height=0.05\n else\n height=0.05occupy2[n]+0.05\n end\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{position2[n][1],position2[n][2],height+0.005},targetQuat2,nil)\n sim.wait(0.05)\n suctionCupCmd('off')\n sim.wait(0.05)\n sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{position2[n][1],position2[n][2],0.33},targetQuat2,nil)\n occupy2[n]=occupy2[n]+1\n end\n end\n else\n sim.setIntegerSignal('finish6',1)\n sim.clearIntegerSignal('arrive6')\n end\n end\n end\n\n enableIk(false)\n end