Robotic Arm System with Suction Cups and Pioneer Robots: Lua Script for Object Transportation

This is a Lua script for a robotic arm system that uses 'suction cups' to pick up objects and move them to a designated location. The system also interacts with Pioneer robots to transport the objects to their final destination. The script includes functions to turn the 'suction cups' on and off, and to enable and disable inverse kinematics. The script uses RML vectors to control the velocity, acceleration, and jerk of the arm movements. The system also reads data from a vision sensor and uses proximity sensors to determine object size and location. The script waits for signals to initiate and complete tasks, and uses loops to iterate through multiple objects and Pioneer robots.

-- ON OFF 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

-- TURE FALSE 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

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

ikGroupHandle=sim.getIkGroupHandle('UR51')
ikTip=sim.getObjectHandle('UR5_ik1')
ikTarget=sim.getObjectHandle('UR5_target1')
cam=sim.getObjectHandle('Vision_sensor0#0')
sensor=sim.getObjectHandle('Beltsensor#1')
--Pioneer=sim.getObjectHandle('Pioneer_p3dx#0')
waitHandle = sim.getObjectHandle('wait#0')


-- 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={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
targetVel={0,0,0,0,0,0}

--ikMaxVel={0.6,0.6,0.6,1.8}
--ikMaxAccel={0.8,0.8,0.8,0.9}
ikMaxVel={1.2,1.2,1.2,3.6}
ikMaxAccel={1.6,1.6,1.6,1.8}
ikMaxVel2={12,12,12,36}
ikMaxAccel2={16,16,16,18}
ikMaxJerk={0.6,0.6,0.6,0.8}

startPos = sim.getObjectPosition(waitHandle,-1)
workQuat = sim.getObjectQuaternion(waitHandle,-1)
stertQuat = sim.getObjectQuaternion(ikTip,-1)

enableIk(true)
while true do
    if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then
        sim.waitForSignal('getdata')
        sim.waitForSignal('arrive2')
        if(sim.getIntegerSignal('arrive2')==1) then
            Pioneer=sim.getObjectHandle('P1')
        end
        if(sim.getIntegerSignal('arrive2')==2) then
            Pioneer=sim.getObjectHandle('P2')
        end
        if(sim.getIntegerSignal('arrive2')==3) then
            Pioneer=sim.getObjectHandle('P3')
        end
        if(sim.getIntegerSignal('arrive2')==4) then
            Pioneer=sim.getObjectHandle('P4')
        end
        sim.clearIntegerSignal('arrive2')
        local data={}
        data = sim.unpackTable(sim.readCustomDataBlock(cam, 'data3'))
        PioneerPosition= sim.getObjectPosition(Pioneer,-1)
        PioneerPositionMatrix={{PioneerPosition[1]+0.075,PioneerPosition[2]+0.075},{PioneerPosition[1],PioneerPosition[2]+0.075},{PioneerPosition[1]-0.075,PioneerPosition[2]+0.075},
                                {PioneerPosition[1]+0.075,PioneerPosition[2]},{PioneerPosition[1]-0.075,PioneerPosition[2]},{PioneerPosition[1]+0.075,PioneerPosition[2]-0.075},
                                {PioneerPosition[1],PioneerPosition[2]-0.075},{PioneerPosition[1]-0.075,PioneerPosition[2]-0.075},{PioneerPosition[1],PioneerPosition[2]}}
        
        local PioneerPositionOccupy={}
        for i=1,9,1 do
            PioneerPositionOccupy[i]=0
        end
        if(#data~=0) then
        for i=4,1,-1 do
            local size=0
            sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.27},workQuat,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.155},workQuat,nil)
            else
                sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{data[i][1],data[i][2],0.145},workQuat,nil)
            end

            suctionCupCmd('on')
            sim.wait(0.05) --S
            sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{data[i][1],data[i][2],0.3},workQuat,nil)
            for k=5,8,1 do
                if(PioneerPositionOccupy[k]==0) then
                    sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.3},workQuat,nil)
                    --sim.wait(0.1)
                    if(size==1) then
                        sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.29},workQuat,nil)
                        --sim.wait(0.1)
                    else
                        sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.28},workQuat,nil)
                        --sim.wait(0.1)
                    end
                    suctionCupCmd('off')
                    sim.wait(0.02)
                    sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel2,ikMaxAccel2,ikMaxJerk,{PioneerPositionMatrix[k][1],PioneerPositionMatrix[k][2],0.3},workQuat,nil)
                    --sim.wait(0.1)
                    PioneerPositionOccupy[k]=1
                    break
                end
            end
        end
        else
            sim.wait(8)
        end
        sim.setIntegerSignal('finish2',1)
        sim.setIntegerSignal('done',1)
    end
end
enableIk(false)

end