Tutorial 02 - Pick And Place

Main Script Lua Code

--Function for moving robot arm to a given location
function MoveArm(target, pos)
    vel = 0.3
    accel = 10
    jerk = 10
    -- Put experimental values for above three parameters
    currentVel = {0,0,0,0}
    currentAccel = {0,0,0,0}
    maxVel = {vel,vel,vel,vel}
    maxAccel = {accel, accel, accel, accel}
    maxJerk = {jerk, jerk, jerk, jerk}
    targetVel = {0,0,0,0}
    quaternion = {0,0,0,0}

    sim.rmlMoveToPosition(target, -1, -1, currentVel, currentAccel, maxVel, maxAccel, maxJerk, pos, quaternion, targetVel)
end

--Function for Pick and Place
function PickAndPlace(target, suctionPad_name, pickup, destination)
    --Pickup coordinate
    MoveArm(target, pickup)
    sim.wait(1)

    --Pickup the Box
    sim.setIntegerSignal(suctionPad_name..'activate', 1)
    sim.wait(1)

    --Destination coordinate
    MoveArm(target, destination)
    sim.wait(1)

    --Drop the Box
    sim.setIntegerSignal(suctionPad_name..'activate', 0)
    sim.wait(1)
end

function sysCall_threadmain()

    base = sim.getObjectHandle('IRB4600')
    IK_Target = sim.getObjectHandle('Target')

    pickup = {1.225, 0.725, 0.115} --For z coordinate you have to use an experimental value with your box size
    destination = {0.85, -1.25, 0.2} -- Use any destination coordinate
    PickAndPlace(IK_Target, 'suctionPad', pickup, destination)

end

function sysCall_cleanup()
    -- Put some clean-up code here
end

Modified Child Script for Suction Pad

--This is the inbuilt code for suction pad provided by V-REP after performing few modifications.
function sysCall_init() 
    s=sim.getObjectHandle('suctionPadSensor')
    l=sim.getObjectHandle('suctionPadLoopClosureDummy1')
    l2=sim.getObjectHandle('suctionPadLoopClosureDummy2')
    b=sim.getObjectHandle('suctionPad')
    objectName=sim.getObjectName(b)
    suctionPadLink=sim.getObjectHandle('suctionPadLink')

    infiniteStrength=sim.getScriptSimulationParameter(sim.handle_self,'infiniteStrength')
    maxPullForce=sim.getScriptSimulationParameter(sim.handle_self,'maxPullForce')
    maxShearForce=sim.getScriptSimulationParameter(sim.handle_self,'maxShearForce')
    maxPeelTorque=sim.getScriptSimulationParameter(sim.handle_self,'maxPeelTorque')

    sim.setLinkDummy(l,-1)
    sim.setObjectParent(l,b,true)
    m=sim.getObjectMatrix(l2,-1)
    sim.setObjectMatrix(l,-1,m)
end

function sysCall_cleanup() 
    sim.setLinkDummy(l,-1)
    sim.setObjectParent(l,b,true)
    m=sim.getObjectMatrix(l2,-1)
    sim.setObjectMatrix(l,-1,m)
end 

function sysCall_sensing() 
    parent=sim.getObjectParent(l)
    activate=sim.getIntegerSignal(objectName..'activate')
    if (activate~=1) then
        if (parent~=b) then
            sim.setLinkDummy(l,-1)
            sim.setObjectParent(l,b,true)
            m=sim.getObjectMatrix(l2,-1)
            sim.setObjectMatrix(l,-1,m)
        end
    else
        if (parent==b) then
            -- Here we want to detect a respondable shape, and then connect to it with a force sensor (via a loop closure dummy dummy link)
            -- However most respondable shapes are set to "non-detectable", so "sim.readProximitySensor" or similar will not work.
            -- But "sim.checkProximitySensor" or similar will work (they don't check the "detectable" flags), but we have to go through all shape objects!
            index=0
            while true do
                shape=sim.getObjects(index,sim.object_shape_type)
                if (shape==-1) then
                    break
                end
                if (shape~=b) and (sim.getObjectInt32Parameter(shape,sim.shapeintparam_respondable)~=0) and (sim.checkProximitySensor(s,shape)==1) then
                    -- Ok, we found a respondable shape that was detected
                    -- We connect to that shape:
                    -- Make sure the two dummies are initially coincident:
                    sim.setObjectParent(l,b,true)
                    m=sim.getObjectMatrix(l2,-1)
                    sim.setObjectMatrix(l,-1,m)
                    -- Do the connection:
                    sim.setObjectParent(l,shape,true)
                    sim.setLinkDummy(l,l2)
                    break
                end
                index=index+1
            end
        else
            -- Here we have an object attached
            if (infiniteStrength==false) then
                -- We might have to conditionally beak it apart!
                result,force,torque=sim.readForceSensor(suctionPadLink) -- Here we read the median value out of 5 values (check the force sensor prop. dialog)
                if (result>0) then
                    breakIt=false
                    if (force[3]>maxPullForce) then breakIt=true end
                    sf=math.sqrt(force[1]*force[1]+force[2]*force[2])
                    if (sf>maxShearForce) then breakIt=true end
                    if (torque[1]>maxPeelTorque) then breakIt=true end
                    if (torque[2]>maxPeelTorque) then breakIt=true end
                    if (breakIt) then
                        -- We break the link:
                        sim.setLinkDummy(l,-1)
                        sim.setObjectParent(l,b,true)
                        m=sim.getObjectMatrix(l2,-1)
                        sim.setObjectMatrix(l,-1,m)
                    end
                end
            end
        end
    end
end