UR5 Robotic Arm Control with Suction Cups - Lua Script for Object Picking

{/'suctionCupCmd/': /'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/',/'enableIk/': /'function enableIk(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/',/'sysCall_threadmain/': /'function 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}}//n occupy1={}//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/