def getAngles(self, pos):
if pos[0] == 0:
pos[0] = 0.01
if pos[1] == 0:
pos[1] = 0.01
if pos[2] == 0:
pos[2] = 0.01
pos[0] = myutils.map(pos[0], 0, 100, self.xMin, self.xMax)
pos[1] = myutils.map(pos[1], 0, 100, self.yMin, self.yMax)
pos[2] = myutils.map(pos[2], 0, 100, self.zMin, self.zMax)
anglesRad = self.chain.inverse_kinematics(pos)
angles = anglesRad * self.radToDegreeFactor
calcPos = self.chain.forward_kinematics(anglesRad)[:3, 3]
calcPosPerc = self.getPosition(angles, True)
newAngles = []
for i in range(1, self.numberOfLinks - 1):
# newAngles.append(math.floor(angles[i]))
newAngles.append(angles[i])
outOfRange = False
for i in range(len(calcPosPerc)):
if abs(calcPosPerc[i] - pos[i]) > 2:
outOfRange = True
break
log("{} calcPosPerc {}".format(outOfRange, calcPosPerc), "OK")
print("Angles are: {}".format(newAngles))
return newAngles, outOfRange
def getPosition(self, angles, allLinks=False):
anglesRad = []
if not allLinks:
anglesRad.append(0)
for angle in angles:
anglesRad.append(angle * self.degreeToRadFactor)
if not allLinks:
anglesRad.append(0)
# print("anglesRad: {}".format(anglesRad))
calcPos = self.chain.forward_kinematics(anglesRad)[:3, 3]
calcPosPerc = []
calcPosPerc.append(
myutils.map(calcPos[0], self.xMin, self.xMax, -100, 100))
calcPosPerc.append(
myutils.map(calcPos[1], self.yMin, self.yMax, -100, 100))
calcPosPerc.append(
myutils.map(calcPos[2], self.zMin, self.zMax, 0, 100))
return calcPosPerc
def getPosition(self):
center = None
frame = self.videoStream.read()
# log("Frame read")
if frame is None:
log("no frame available", "WARN")
return self.position
# resize the frame, blur it, and convert it to the HSV
# color space
frame = imutils.resize(frame, width=600)
blurred = cv2.GaussianBlur(frame, (11, 11), 0)
hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
# construct a mask for the color, then perform
# a series of dilations and erosions to remove any small
# blobs left in the mask
mask = cv2.inRange(hsv, self.lowerColor, self.upperColor)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
# find contours in the mask
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
# only proceed if at least one contour was found
if len(cnts) > 0:
# log("found contour!", "OK")
# find the largest contour in the mask
c = max(cnts, key=cv2.contourArea)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
x = myutils.map(center[0], 0, 600, 0, 100)
y = myutils.map(center[1], 0, 600, 0, 100)
self.position = (x, y)
cv2.drawContours(frame, [c], 0, (0, 255, 0), 3)
else:
log("no contour found", "WARN")
cv2.imshow("Frame", frame)
log("Position is {}".format(self.position))
return self.position
def getPositionFromFrame(self, frame):
center = None
if frame is None:
log("no frame available", "WARN")
return self.position
# blur the frame, and convert it to the HSV color space
# frame = imutils.resize(frame, width=600)
blurred = cv2.GaussianBlur(frame, (11, 11), 0)
hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
# construct a mask for the color, then perform
# a series of dilations and erosions to remove any small
# blobs left in the mask
mask = cv2.inRange(hsv, self.lowerColor, self.upperColor)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
# find contours in the mask
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
# only proceed if at least one contour was found
if len(cnts) > 0:
# log("found contour!", "OK")
# find the largest contour in the mask
c = max(cnts, key=cv2.contourArea)
((xc, yc), radius) = cv2.minEnclosingCircle(c)
if radius > 10:
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
x = myutils.map(center[0], 0, 600, -100, 100)
x *= -0.7
y = myutils.map(center[1], 50, 300, 0, 80)
y = 80 - y
self.position = (x, y)
self.contour = c
else:
log("no contour found", "WARN")
# log("Position is {}".format(self.position))
return self.position
def getCorrectedAngle(self, angle):
if self.inverted:
angle = -angle
angle += self.offset
if angle > self.bounds[1]:
angle = self.bounds[1]
log(
"angle of servo {} out of bounds. angle is {}".format(
self.pin, angle), "ERR")
if angle < self.bounds[0]:
angle = self.bounds[0]
log(
"angle of servo {} out of bounds. angle is {}".format(
self.pin, angle), "ERR")
newAngle = myutils.map(angle, self.bounds[0], self.bounds[1], 0, 180)
return newAngle
def getAngle(self):
return myutils.map(self.servo.angle, 0, 180, self.bounds[0],
self.bounds[1])