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
, a simple function was mentioned in the tutorial I am following, which is as follows:
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.