Hello coppelia,
I need to track an object (Quadcopter) with vision senor which is related to 2 joints yaw and pitch,
i create a path to make the quadcoter move and also i made the 2 joints and i fix their proprieties but i had a problem to code the vision senor child script(lua) and to make it track the quadcopter and make a bounding box on it.
I'm new with coppelia community and i need your help.
Cordially thanks.
Visual tracking object
-
- Posts: 4
- Joined: 08 Mar 2023, 12:05
Re: Visual tracking object
hello again , i wish can you replay to my post,
so i made a code to track a quadcopter with a vision senor and i need to extarct informations from the vision sensor to work with it (machine learning) so first of all i need to solve my problem
this is the code can you correct it for me
thanks
function sysCall_init()
yawJoint = sim.getObject("/yaw")
pitchJoint = sim.getObject("/pitch")
quadcopter = sim.getObject("/Quadcopter")
camera = sim.getObject(".")
sphereContainer = sim.addDrawingObject(sim.drawing_spherepoints, 0.02, 0, -1, 1000, {1, 0, 0})
Kp = 2
resX = 128
resY = 128
xAngle = math.rad(45)
yAngle = math.rad(45)
end
function sysCall_actuation()
local pos = sim.getObjectPosition(quadcopter, -1)
local camPos = sim.getObjectPosition(camera, -1)
local dx = pos[1] - camPos[1]
local dy = pos[2] - camPos[2]
local dz = pos[3] - camPos[3]
local distance = math.sqrt(dx*dx + dy*dy + dz*dz)
-- Calculer l'angle de rotation en fonction de la position du "quadcopter"
local pitch = math.atan2(dz, math.sqrt(dx*dx + dy*dy))
local yaw = math.atan2(dy, dx) + math.pi
-- Ajouter une correction proportionnelle en fonction de la distance
local pitchCorrection = Kp * (pos[3] - camPos[3])
pitch = pitch + pitchCorrection
sim.setJointPosition(yawJoint, yaw)
sim.setJointPosition(pitchJoint, pitch)
end
-- Variables pour stocker les coordonn?es de la quadcopter
local quadcopterCoords = {0, 0, 0}
function sysCall_sensing()
-- R?cup?rer la matrice de l'objet de la vision sensor
local m=sim.getObjectMatrix(camera,sim.handle_world)
-- Ajouter un objet de dessin pour chaque blob d?tect?
sim.addDrawingObjectItem(sphereContainer,nil)
-- Obtenir les donn?es de la vision sensor
local res,packet1,packet2=sim.handleVisionSensor(camera)
if res>=0 then
local blobCnt=packet2[1]
local valCnt=packet2[2]
if blobCnt~= nil then
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]
-- Obtenir la profondeur a partir de la vision sensor
local depth=sim.getVisionSensorDepth(camera,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]}
-- Calculer les coordonnees de la quadcopter a partir de la profondeur et de l'angle de vue
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)
-- Mettre a jour les coordonnees de la quadcopter
quadcopterCoords[1] = coord[1]
quadcopterCoords[2] = coord[2]
quadcopterCoords[3] = coord[3]
sim.auxiliaryConsolePrint(res,string.format("Coordonnees de la quadcopter dans la cam?ra : x=%.2f, y=%.2f, z=%.2f", quadcopterCoords[1], quadcopterCoords[2], quadcopterCoords[3]))
-- Ajouter un objet de dessin pour le blob
sim.addDrawingObjectItem(sphereContainer,coord)
end
end
end
end
so i made a code to track a quadcopter with a vision senor and i need to extarct informations from the vision sensor to work with it (machine learning) so first of all i need to solve my problem
this is the code can you correct it for me
thanks
function sysCall_init()
yawJoint = sim.getObject("/yaw")
pitchJoint = sim.getObject("/pitch")
quadcopter = sim.getObject("/Quadcopter")
camera = sim.getObject(".")
sphereContainer = sim.addDrawingObject(sim.drawing_spherepoints, 0.02, 0, -1, 1000, {1, 0, 0})
Kp = 2
resX = 128
resY = 128
xAngle = math.rad(45)
yAngle = math.rad(45)
end
function sysCall_actuation()
local pos = sim.getObjectPosition(quadcopter, -1)
local camPos = sim.getObjectPosition(camera, -1)
local dx = pos[1] - camPos[1]
local dy = pos[2] - camPos[2]
local dz = pos[3] - camPos[3]
local distance = math.sqrt(dx*dx + dy*dy + dz*dz)
-- Calculer l'angle de rotation en fonction de la position du "quadcopter"
local pitch = math.atan2(dz, math.sqrt(dx*dx + dy*dy))
local yaw = math.atan2(dy, dx) + math.pi
-- Ajouter une correction proportionnelle en fonction de la distance
local pitchCorrection = Kp * (pos[3] - camPos[3])
pitch = pitch + pitchCorrection
sim.setJointPosition(yawJoint, yaw)
sim.setJointPosition(pitchJoint, pitch)
end
-- Variables pour stocker les coordonn?es de la quadcopter
local quadcopterCoords = {0, 0, 0}
function sysCall_sensing()
-- R?cup?rer la matrice de l'objet de la vision sensor
local m=sim.getObjectMatrix(camera,sim.handle_world)
-- Ajouter un objet de dessin pour chaque blob d?tect?
sim.addDrawingObjectItem(sphereContainer,nil)
-- Obtenir les donn?es de la vision sensor
local res,packet1,packet2=sim.handleVisionSensor(camera)
if res>=0 then
local blobCnt=packet2[1]
local valCnt=packet2[2]
if blobCnt~= nil then
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]
-- Obtenir la profondeur a partir de la vision sensor
local depth=sim.getVisionSensorDepth(camera,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]}
-- Calculer les coordonnees de la quadcopter a partir de la profondeur et de l'angle de vue
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)
-- Mettre a jour les coordonnees de la quadcopter
quadcopterCoords[1] = coord[1]
quadcopterCoords[2] = coord[2]
quadcopterCoords[3] = coord[3]
sim.auxiliaryConsolePrint(res,string.format("Coordonnees de la quadcopter dans la cam?ra : x=%.2f, y=%.2f, z=%.2f", quadcopterCoords[1], quadcopterCoords[2], quadcopterCoords[3]))
-- Ajouter un objet de dessin pour le blob
sim.addDrawingObjectItem(sphereContainer,coord)
end
end
end
end
-
- Posts: 4
- Joined: 08 Mar 2023, 12:05
Re: Visual tracking object
pleaaaase help me coppelia :(
Re: Visual tracking object
Hello,
visual tracking is not that easy, and we cannot debug your code. Best is to start from something that is working, and to expand from there.
In Models/components/sensors/, you can find a few relevant models, e.g.:
Cheers
visual tracking is not that easy, and we cannot debug your code. Best is to start from something that is working, and to expand from there.
In Models/components/sensors/, you can find a few relevant models, e.g.:
- Blob to 3D position.ttm
- Camera pixels to 3D positions.ttm
Cheers
-
- Posts: 4
- Joined: 08 Mar 2023, 12:05
Re: Visual tracking object
Hi again,
I made this code and it is debugging, but the problem here is i need to detect the object first of all and then make the vision sensor track it, so i need to extrat the cordinate of the blob and track it and make the object in the center of the cam.
And can you help me how to make a bounding box for the object tracked.
I made this code and it is debugging, but the problem here is i need to detect the object first of all and then make the vision sensor track it, so i need to extrat the cordinate of the blob and track it and make the object in the center of the cam.
And can you help me how to make a bounding box for the object tracked.
Code: Select all
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)
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
yawJoint = sim.getObject("/yaw")
pitchJoint = sim.getObject("/pitch")
quadcopter = sim.getObject("/Quadcopter")
Kp=1
graph=sim.getObject('/Graph')
joint1Vel=sim.addGraphStream(graph,'Yaw velocity','deg/s',0,{1,0,0})
joint2Vel=sim.addGraphStream(graph,'Pitch velocity','deg/s',0,{0,1,0})
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
function sysCall_sensing()
local m=sim.getObjectMatrix(sensor,sim.handle_world)
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)
-- Calculate the 2D coordinates of the blob in the vision sensor's image
local xCoord = math.floor(blobPositionX * resX)
local yCoord = math.floor(blobPositionY * resY)
-- Print the 2D coordinates of the blob
print('Blob ' .. i .. ' coordinates: (' .. xCoord .. ', ' .. yCoord .. ')')
end
end
sim.setGraphStreamValue(graph,joint1Vel,180*sim.getJointVelocity(yawJoint)/math.pi)
sim.setGraphStreamValue(graph,joint2Vel,180*sim.getJointVelocity(pitchJoint)/math.pi)
end
function sysCall_actuation()
local pos = sim.getObjectPosition(quadcopter, -1)
local camPos = sim.getObjectPosition(sensor, -1)
local dx = pos[1] - camPos[1]
local dy = pos[2] - camPos[2]
local dz = pos[3] - camPos[3]
--local distance = math.sqrt(dx*dx + dy*dy + dz*dz)
-- Calculer l'angle de rotation en fonction de la position du "objet"
local pitch = math.atan2(dz, math.sqrt(dx*dx + dy*dy))
local yaw = math.atan2(dy, dx) + math.pi
-- Ajouter une correction proportionnelle en fonction de la distance
local pitchCorrection = Kp * (0.5 - camPos[3])
local yawCorrection = Kp * (0.5 - camPos[2])
pitch = pitch + pitchCorrection
yaw = yaw + yawCorrection
sim.setJointPosition(yawJoint, yaw)
sim.setJointPosition(pitchJoint, pitch)
end
function sysCall_cleanup()
sim.removeDrawingObject(sphereContainer)
end