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])