Thank you for the link. It is working well now.
In the objectTracking example, they have used a mobile robot as the target. I want to use this moving target instead of the mobile robot. I tried simply copying the UR5 model from the tutorial (along with the script and all) into my scene and running the simulation, but it is not tracking the sphere. The mobile robot script does not seem to contribute to the tracking algorithm.
How do I make it work?
Adding below the Robot manipulator script and the sensor script.
UR5:
Code: Select all
--lua
function sysCall_init()
sim = require('sim')
simIK = require('simIK')
simBase = sim.getObject('.')
simTip = sim.getObject('./tip')
simTarget = sim.getObject('./target')
simJoints = {}
for i = 1, 6, 1 do
simJoints[i] = sim.getObject('./joint', {index = i - 1})
end
ikEnv = simIK.createEnvironment()
ikGroup = simIK.createGroup(ikEnv)
simIK.setGroupCalculation(ikEnv, ikGroup, simIK.method_damped_least_squares, 1.5, 1)
local ikElement = simIK.addElementFromScene(ikEnv, ikGroup, simBase, simTip, simTarget, simIK.constraint_position)
visionSensor = sim.getObject('./Vision_sensor')
centerX = 0
centerY = 0
end
function setBoxCenter(cx, cy)
centerX = cx
centerY = cy
end
function sysCall_actuation()
-- Get data computed by the Python image processing script:
k = 0.15
sim.setObjectPose(simTarget, {-k * centerX, k * centerY, 0, 0, 0, 0, 1}, simTip)
simIK.handleGroup(ikEnv, ikGroup, {syncWorlds = true, allowError = true})
end
function sysCall_cleanup()
simIK.eraseEnvironment(ikEnv)
end
Vision sensor:
Code: Select all
# python
import numpy as np
import cv2 as cv
def readFrame():
sim.handleVisionSensor(self.handle)
buf, res = sim.getVisionSensorImg(self.handle)
return np.frombuffer(buf, dtype=np.uint8).reshape(*res, 3)
def showImage(k, img):
sim.setVisionSensorImg(sim.getObject(f':/{k}'), img.tobytes())
def sysCall_init():
sim = require('sim')
self.handle = sim.getObject('.')
robot = sim.getObject(':')
script = sim.getScript(sim.scripttype_childscript, robot)
self.robotScriptFuncs = sim.getScriptFunctions(script)
def sysCall_thread():
while True:
frame = readFrame()
imgw, imgh, imgc = frame.shape
blurred = cv.GaussianBlur(frame, (13, 13), 0)
hsv = cv.cvtColor(blurred, cv.COLOR_RGB2HSV)
# construct a mask for the color "red", then perform
# a series of dilations and erosions to remove any small
# blobs left in the mask
mask = cv.inRange(hsv, (0, 200, 64), (80, 255, 200))
n = 5
mask = cv.dilate(mask, None, iterations=n)
mask = cv.erode(mask, None, iterations=n)
maskdbg = cv.cvtColor(mask, cv.COLOR_GRAY2RGB)
contours, hierarchy = cv.findContours(mask.copy(), cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
if contours:
c = max(contours, key = cv.contourArea)
x, y, w, h = cv.boundingRect(c)
cv.rectangle(maskdbg, (x, y), (x+w, y+h), (0, 255, 0), 2)
cv.arrowedLine(maskdbg, (x+w//2, y+h//2), (imgw//2, imgh//2), (255, 0, 0), 2)
# normalize rect in -1 ... +1:
normalize = lambda x: np.interp(x[0], (0, x[1]), (-1, 1))
x, y, w, h = map(normalize, ((x, imgw), (y, imgh), (w, imgw), (h, imgh)))
# box center:
self.robotScriptFuncs.setBoxCenter(x + w/2, y + h/2)
showImage('blurred', blurred)
showImage('hsv', hsv)
showImage('mask', maskdbg)
Please check and give me some solution.
Thank you!!!