Tutorial 03 - Blob Detection

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 for blob detection
function capture(sensor)
    local res
    res,nearClip=sim.getObjectFloatParameter(sensor,sim.visionfloatparam_near_clipping)
    res,farClip=sim.getObjectFloatParameter(sensor,sim.visionfloatparam_far_clipping)
    res,xAngle=sim.getObjectFloatParameter(sensor,sim.visionfloatparam_perspective_angle)
    res,resX=sim.getObjectInt32Parameter(sensor,sim.visionintparam_resolution_x)
    res,resY=sim.getObjectInt32Parameter(sensor,sim.visionintparam_resolution_y)
    yAngle=xAngle
    local ratio=resX/resY
    if resX>resY then
        yAngle=2*math.atan(math.tan(xAngle/2)/ratio)
    else
        xAngle=2*math.atan(math.tan(yAngle/2)/ratio)
    end

    local retFloat = {} -- Float array to include coordinates of blob centers (x,y)

    local m=sim.getObjectMatrix(sensor,-1)
    local res,packet1,packet2=sim.handleVisionSensor(sensor)
    if res>=0 then
        local blobCnt=packet2[1]
        local valCnt=packet2[2]
        for i=0,blobCnt-1,1 do
            local blobSize=packet2[2+valCnt*i+1]
            local blobOrientation=packet2[2+valCnt*i+2]
            local blobPositionX=packet2[2+valCnt*i+3]
            local blobPositionY=packet2[2+valCnt*i+4]
            local blobWidth=packet2[2+valCnt*i+5]
            local blobHeight=packet2[2+valCnt*i+6]
            local depthV=sim.getVisionSensorDepthBuffer(sensor,1+math.floor(blobPositionX*(resX-0.99)),1+math.floor(blobPositionY*(resY-0.99)),1,1)
            local depth=nearClip+(farClip-nearClip)*depthV[1]
            local coord={0,0,depth}  
            coord[1]=depth*math.tan(xAngle*0.5)*(0.5-blobPositionX)/0.5
            coord[2]=depth*math.tan(yAngle*0.5)*(blobPositionY-0.5)/0.5
            table.insert(retFloat,-coord[1])-- x-coordinate of the blob center (minus because of vision sensor x-axis is inverted with world x-axis)
            table.insert(retFloat,coord[2]) -- y-coordinate of the blob center
        end
        return retFloat
    end
end

--Function for getting world coordinates of the blob centers
function getRandomBoxCoordinates(vision_sensor_handle,retFloats)
    coord = sim.getObjectPosition(vision_sensor_handle,-1)
    
    vs_x = coord[1] -- Vision sensor x coordinate (in world coordinate system)
    vs_y = coord[2] -- Vision sensor y coordinate (in world coordinate system)

    Coords_x = {}
    Coords_y = {}

    for i = 1,#retFloats-1,2 do
        table.insert(Coords_x,(retFloats[i] + vs_x))  --Calculating x coordinate of the i th blob (in world coordinate system)
        table.insert(Coords_y,(retFloats[i+1] + vs_y))--Calculating y coordinate of the i th blob (in world coordinate system)
    end
    return Coords_x,Coords_y
end

function sysCall_threadmain()

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

    scanPos = {1.225, 0.725, 0.75}  --Position where vision sensor captures the boxes for blob detection
    MoveArm(IK_Target, scanPos)     --Move robot arm to capture position
    Cordinates = capture(vs)        --Capture the boxes

    Coords_x, Coords_y = getRandomBoxCoordinates(vs, Cordinates)    --Getting world coordinates of the blob centers
    
    --Move to each blob center and perform pick and place
    for i=1, #Coords_x, 1 do
        pickup =  {Coords_x[i],Coords_y[i],0.115}
        destination = {0.85, -1.25, 0.2} -- Use any destination coordinate
        PickAndPlace(IK_Target, 'suctionPad', pickup, destination)
    end

end

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