vision sensor error

[NandanUmrikar]

I'm working on a visual servoing application. I'm trying to use a blobTo3dPosition sensor to get the depth information of objects in the scene using the function depthV=sim.getVisionSensorDepth(sensor). When I try to use this value later, the code gives error saying 'attempt to perform arithmetics on a nil value' which traces back to the formula. How to solve this problem? adding the code below.

Code: Select all

function blob3DPoints()
    local detectionCount,aux1,aux2
    local numBlobs,blobsize,u,v,U,V
    local depthV,depth
    local s={}
    local colors={}
    detectionCount,aux1,aux2=sim.readVisionSensor(sensor)
    numBlobs=aux2[1]
    blobSize=aux2[2]
    for i=1,numBlobs,1 do
        u=aux2[(i-1)*blobSize+5]
        v=aux2[(i-1)*blobSize+6]
        U=1+math.floor(u*(resX-0.99))
        V=1+math.floor(v*(resY-0.99))
        depthV=sim.getVisionSensorDepth(sensor)
        print (depthV)
        color=sim.getVisionSensorImg(sensor)
        colors[i]=color
        depth=nearClip+(farClip-nearClip)*depthV[1] (traceback of the error to this function)
        s[i]={depth*(0.5-u)/fX,depth*(v-0.5)/fY,depth}
    end
     -- Print the values of s
    print("Values of s:")
    for i, point in ipairs(s) do
        print(string.format("Point %d: (%.4f, %.4f, %.4f)", i, point[1], point[2], point[3]))
    end

    -- Print the values of colors
    print("\nValues of colors:")
    for i, color in ipairs(colors) do
        print(string.format("Color %d: %d", i, color))
    end
    return colors
end

Also, I'm unable to see the output in the console window.

[fferri]

the code gives error saying 'attempt to perform arithmetics on a nil value' which traces back to

Code: Select all

        depth=nearClip+(farClip-nearClip)*depthV[1] (traceback of the error to this function)

it means one of those values (either nearClip , farClip or depthV[1]) is nil.

Also, I'm unable to see the output in the console window.

that's normal, as execution stops at the first error.

[NandanUmrikar]

How do i overcome the nil value problem?
Regarding the console window, even before the error showed up, I tried printing the output of the 'depthV' variable but got no output anywhere. There was no error, but the output didn't show.

[fferri]

So, does print not work at all?

Try in a simple script, e.g.:

Code: Select all

function sysCall_init()
    print('Hello')
end

if it works, then you have some other kind of issue in your code.
But you posted incomplete code which is not possible to debug for us.

Btw, in your code, if numBlobs is 0, that print (depthV) line is never reached.
Also, variables nearClip and farClip are not defined anywhere in the piece of code you posted here.

[NandanUmrikar]

Following is the complete code associated with the child script for the sensor.

Code: Select all

--lua

sim=require'sim'
simVision=require'simVision'

function sysCall_init()
    sensor=sim.getObject('.')
    sphereContainer=sim.addDrawingObject(sim.drawing_spherepts,0.03,0,-1,9999,{1,0,1})
    xAngle=sim.getObjectFloatParam(sensor,sim.visionfloatparam_perspective_angle)
    resX=sim.getObjectInt32Param(sensor,sim.visionintparam_resolution_x)
    resY=sim.getObjectInt32Param(sensor,sim.visionintparam_resolution_y)
    perspAngle=sim.getObjectFloatParam(sensor,sim.visionfloatparam_perspective_angle)
    nearClip=sim.getObjectFloatParam(sensor,sim.visionfloatparam_near_clipping)
    farClip=sim.getObjectFloatParam(sensor,sim.visionfloatparam_far_clipping)
    ratio=resX/resY
    if (ratio>1) then
        fX=0.5/math.tan(perspAngle/2)
        fY=0.5/math.tan(perspAngle/2)/ratio
    else
        fX=0.5/math.tan(perspAngle/2)*ratio
        fY=0.5/math.tan(perspAngle/2)
    end
    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

end

function sysCall_vision(inData)
    local retVal={}
    retVal.trigger=false
    retVal.packedPackets={}
    simVision.sensorImgToWorkImg(inData.handle)
    local trig,packedPacket=simVision.blobDetectionOnWorkImg(inData.handle,0.100000,0.000000,true) if trig then retVal.trigger=true end if packedPacket then retVal.packedPackets[#retVal.packedPackets+1]=packedPacket end
    simVision.workImgToSensorImg(inData.handle)
    return retVal
end

dq={}

function sysCall_sensing()
    local m=sim.getObjectMatrix(sensor)
    sim.addDrawingObjectItem(sphereContainer,nil)
    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 depth=sim.getVisionSensorDepth(sensor,1,{1+math.floor(blobPositionX*(resX-0.99)),1+math.floor(blobPositionY*(resY-0.99))},{1,1})
            depth=sim.unpackFloatTable(depth)
            local coord={0,0,depth[1]}
            local x=0.5-blobPositionX
            local y=blobPositionY-0.5
            coord[1]=depth[1]*math.tan(xAngle*0.5)*(0.5-blobPositionX)/0.5
            coord[2]=depth[1]*math.tan(yAngle*0.5)*(blobPositionY-0.5)/0.5
            coord=sim.multiplyVector(m,coord)
            -- coord now contains the position of the blob, in world coordinates
            sim.addDrawingObjectItem(sphereContainer,coord)
        end
    end

function blob3DPoints()
    local detectionCount,aux1,aux2
    local numBlobs,blobsize,u,v,U,V
    local depthV,depth
    local s={}
    local colors={}
    detectionCount,aux1,aux2=sim.readVisionSensor(sensor)
    numBlobs=aux2[1]
    blobSize=aux2[2]
    for i=1,numBlobs,1 do
        u=aux2[(i-1)*blobSize+5]
        v=aux2[(i-1)*blobSize+6]
        U=1+math.floor(u*(resX-0.99))
        V=1+math.floor(v*(resY-0.99))
        depthV=sim.getVisionSensorDepth(sensor)
        print (depthV)
        color=sim.getVisionSensorImg(sensor)
        colors[i]=color
        depth=nearClip+(farClip-nearClip)--*deptcolorsRefhV[1]
        --depth=0.1
        s[i]={depth*(0.5-u)/fX,depth*(v-0.5)/fY,depth}
    end
     -- Print the values of s
    --print("Values of s:")
    --for i, point in ipairs(s) do
        --print(string.format("Point %d: (%.4f, %.4f, %.4f)", i, point[1], point[2], point[3]))
    --end

    -- Print the values of colors
    --print("\nValues of colors:")
    --for i, color in ipairs(colors) do
        --print(string.format("Color %d: %d", i, color))
    --end
    return s,colors
end
    s,colors=blob3DPoints()
    colorsRef=Matrix(4,3,{1,0.09,0.09,0.09,1,0.09,0.09,0.09,1,1,1,1})
    c=colorCorrespondence(colors,colorsRef)
    A0n=getAn(ikEnv,ikJoints,DH_params)
    J=getJacob(ikEnv,ikGroup_undamped,ikJoints,DH_params)
    Tn0=Matrix(6,6)
    Tn0=Tn0:assign(1,1,Matrix4x4:torotation(A0n):t())
    Tn0=Tn0:assign(4,4,Matrix4x4:torotation(A0n):t())
    if (#s==#sref) then
        e=computeFeatureError(s,sref,c)
        Ls=getInteractionMatrix(s,sref,c)
        Jc=Ls*Tcn*Tn0*J
        print(e:norm())
        dq=-lambda*(Jc:pinv(1e-9)*e)
    end
end

function colorCorrespondance(colors,colorsRef)
    local jmin
    local red,green,blue
    local c={}
    for i=1,#colors,1 do
        dmin=math.sqrt(3)
        for j=1,#colorsRef,1 do
            red=colors[i][1]-colorsRef[j][1]
            green=colors[i][2]-colorsRef[j][2]
            blue=colors[i][3]-colorsRef[j][3]
            d=math.sqrt(red^2+green^2+blue^2)
            if (d<=dmin) then
                dmin=d
                jmin=j
            end
        end
        c[i]=jmin
    end
    return c
end

function computeFeaturesError(s,sref,c)
    local e
    e=Matrix(2*#s,1)
    for i=1,#s,1 do
        e[(i-1)*2+1]=s[i][1]-sref[c[i]][1]
        e[i*2]=s[i][2]-sref[c[i]][2]
    end
    return e
end

function sysCall_cleanup()
    sim.removeDrawingObject(sphereContainer)
end

For sRef and colorsRef, a simple function was mentioned in the tutorial I am following, which is as follows:

Code: Select all

sRef,colorsRef=blob3DPoints()

But I'm a bit confused about where to put is so that it won't generate an error.
The separate print command worked fine. Not sure why can't I get output for this code.