Exemplo n.º 1
0
    def run(self):
        if len(self.errors): return False

        tracked = self.vision.searchTrackedHistory(trackable=self.trackable)


        # If the object was not seen, exit
        if tracked is None: return False


        inCount = 0
        for coord in tracked.quad:
            if rv.pointInPolygon(coord, self.quad):
                inCount += 1


        # Check if the appropriate part of the object is within the boundaries
        ret = False
        if self.parameters["part"] == "any":
            ret =  inCount > 0

        if self.parameters["part"] == "all":
            ret =  inCount == 4

        if self.parameters["part"] == "center":
            center = sum(tracked.quad) / 4
            ret =  rv.pointInPolygon(center, self.quad)



        # If 'not', flip the return value
        if self.parameters['not']: ret = not ret


        return ret
Exemplo n.º 2
0
    def run(self):
        if len(self.errors): return False

        tracked = self.vision.searchTrackedHistory(trackable=self.trackable)

        # If the object was not seen, exit
        if tracked is None: return False

        inCount = 0
        for coord in tracked.quad:
            if rv.pointInPolygon(coord, self.quad):
                inCount += 1

        # Check if the appropriate part of the object is within the boundaries
        ret = False
        if self.parameters["part"] == "any":
            ret = inCount > 0

        if self.parameters["part"] == "all":
            ret = inCount == 4

        if self.parameters["part"] == "center":
            center = sum(tracked.quad) / 4
            ret = rv.pointInPolygon(center, self.quad)

        # If 'not', flip the return value
        if self.parameters['not']: ret = not ret

        return ret
Exemplo n.º 3
0
    def run(self):
        if len(self.errors): return

        # Before doing any tracking, evaluate the "Relative" number to make sure its valid
        relativeAngle, success = self.interpreter.evaluateExpression(self.parameters["angle"])
        if not success:
            printf("Could not determine the relative angle for the wrist. Canceling command. ")
            return False


        # Find the object using vision
        _, tracked = self.vision.getObjectLatestRecognition(self.trackable)
        if tracked is None:
            printf("Could not find ", self.trackable.name, " in order to set wrist relative")
            return False


        # This is the rotation of the object in degrees, derived from the camera
        targetAngle = math.degrees(tracked.rotation[2])




        cntr = tracked.center
        if self.relToBase:
            # If the angle is relative to the base angle, do the math here and add it to target angle.

            TOCC = cntr   # Tracked Object Camera Coordinates
            ROCC = rv.getPositionTransform((0, 0, 0), "toCam", self.ptPairs)  # Robot Origin Camera Coordinates
            baseAngle = 90 - math.degrees(math.atan( (ROCC[1] - TOCC[1]) / (ROCC[0] - TOCC[0])))
            targetAngle += baseAngle - 90
        else:
            # If the angle is relative to x Axis, do different math and add it to target angle
            a = rv.getPositionTransform((0, 0, 0), "toCam", self.ptPairs)
            b = rv.getPositionTransform((10, 0, 0), "toCam", self.ptPairs)

            # TORC    = rv.getPositionTransform(cntr, "toRob", self.ptPairs)  # Tracked Object Robot Coordinates
            # RXCC    = rv.getPositionTransform((0, cntr[1], cntr[2]), "toCam", self.ptPairs)  # Robot XAxis Camera Coordinates
            xOffset =  math.degrees(math.atan( (a[1] - b[1]) / (a[0] - b[0])))
            targetAngle += 90 - xOffset
            print("XOffset: ", xOffset)
            print("Target: ", targetAngle)

        # Add the "Relative" angle to the wrist
        targetAngle += relativeAngle

        # Normalize the value so that it's between 0 and 180
        while targetAngle < 0: targetAngle += 180
        while targetAngle > 180: targetAngle -= 180



        # Set the robots wrist to the new value
        self.robot.setServoAngles(servo3 = targetAngle)


        return True
Exemplo n.º 4
0
    def run(self):
        if len(self.errors): return

        # Evaluate the "Speed" variable
        newSpeed, success = self.interpreter.evaluateExpression(self.parameters['speed'])

        if not success or newSpeed <= 0:
            printf("Commands| ERROR: In evaluating 'speed' parameter for motionpath")
            return False


        # Send the path to the "path player"
        printf("Commands| Playing motionPath ", self.parameters["objectID"])
        rv.playMotionPath(self.motionPath, self.robot, self.exitFunc, speedMultiplier=newSpeed, reverse=self.parameters["reversed"])
        return True
Exemplo n.º 5
0
    def run(self):
        if len(self.errors) > 0: return False

        ret = rv.pickupObject(self.trackable, self.rbMarker, self.robot,
                              self.vision, self.transform)

        return ret
Exemplo n.º 6
0
    def run(self):
        if len(self.errors) > 0: return False

        ret = rv.pickupObject(self.trackable, self.rbMarker, self.ptPairs, self.grndHeight,
                              self.robot, self.vision, self.exitFunc)

        return ret
Exemplo n.º 7
0
    def run(self):
        if len(self.errors): return False

        # printf("Testing if ", self.parameters["objectID"], " is at location ", self.parameters["location"])

        tracked = self.vision.searchTrackedHistory(trackable=self.trackable)



        # If the object was not seen, exit
        if tracked is None: return False

        # print("Center: ", tracked.center, "Quad", tracked.quad)


        inCount = 0
        for coord in tracked.quad:
            if rv.pointInPolygon(coord, self.quad):
                inCount += 1




        # Check if the appropriate part of the object is within the boundaries
        ret = False
        if self.parameters["part"] == "any":
            ret =  inCount > 0

        if self.parameters["part"] == "all":
            ret =  inCount == 4

        if self.parameters["part"] == "center":
            center = sum(tracked.quad) / 4
            ret =  rv.pointInPolygon(center, self.quad)




        if self.parameters['not']: ret = not ret

        if ret:
            printf("Tested ", self.parameters["objectID"], " location. ", inCount, " points were in location. ")
        # If 'not', flip the return value
        return ret
Exemplo n.º 8
0
    def run(self):
        if len(self.errors): return

        # Before doing any tracking, evaluate the "Relative" number to make sure its valid
        relativeAngle, success = self.interpreter.evaluateExpression(
            self.parameters["angle"])
        if not success:
            printf(
                "Commands| ERROR: Could not determine the relative angle for the wrist. Canceling command. "
            )
            return False

        # Find the object using vision
        _, tracked = self.vision.getObjectLatestRecognition(self.trackable)
        if tracked is None:
            printf("Commands| ERROR: Could not find ", self.trackable.name,
                   " in order to set wrist relative")
            return False

        cntr = tracked.center
        if self.relToBase:
            # If the angle is relative to the base angle, do the math here and add it to target angle.
            targetAngle = math.degrees(tracked.rotation[2])

            TOCC = cntr  # Tracked Object Camera Coordinates
            ROCC = self.transform.robotToCamera(
                (0, 0, 0))  # Robot Origin Camera Coordinates
            baseAngle = math.degrees(
                math.atan((ROCC[1] - TOCC[1]) / (ROCC[0] - TOCC[0]))) + 90
            targetAngle -= baseAngle
        else:
            # If the angle is relative to x Axis, do different math and add it to target angle
            targetAngle = self.transform.cameraToRobotRotation(
                tracked.rotation[2])

            # TODO: Come up with a system for adjusting the direction of a wrist with robots of higher axis
            directionOfWrist = -1
            targetAngle *= directionOfWrist

        # Add the "Relative" angle to the wrist
        targetAngle += relativeAngle
        targetAngle = rv.normalizeAngle(targetAngle)

        # Normalize the value so that it's between 0 and 180
        while targetAngle < 0:
            targetAngle += 180
        while targetAngle > 180:
            targetAngle -= 180

        # Set the robots wrist to the new value
        self.robot.setServoAngles(servo3=targetAngle)

        return True
Exemplo n.º 9
0
    def run(self):
        if len(self.errors): return

        # Evaluate the "Speed" variable
        newSpeed, success = self.interpreter.evaluateExpression(
            self.parameters['speed'])

        if not success or newSpeed <= 0:
            printf(
                "Commands| ERROR: In evaluating 'speed' parameter for motionpath"
            )
            return False

        # Send the path to the "path player"
        printf("Commands| Playing motionPath ", self.parameters["objectID"])
        rv.playMotionPath(self.motionPath,
                          self.robot,
                          self.exitFunc,
                          speedMultiplier=newSpeed,
                          reverse=self.parameters["reversed"])
        return True
Exemplo n.º 10
0
    def run(self):
        if len(self.errors): return

        # Before doing any tracking, evaluate the "Relative" number to make sure its valid
        relativeAngle, success = self.interpreter.evaluateExpression(self.parameters["angle"])
        if not success:
            printf("Commands| ERROR: Could not determine the relative angle for the wrist. Canceling command. ")
            return False


        # Find the object using vision
        _, tracked = self.vision.getObjectLatestRecognition(self.trackable)
        if tracked is None:
            printf("Commands| ERROR: Could not find ", self.trackable.name, " in order to set wrist relative")
            return False



        cntr = tracked.center
        if self.relToBase:
            # If the angle is relative to the base angle, do the math here and add it to target angle.
            targetAngle = math.degrees(tracked.rotation[2])

            TOCC = cntr   # Tracked Object Camera Coordinates
            ROCC = self.transform.robotToCamera((0, 0, 0))   # Robot Origin Camera Coordinates
            baseAngle = math.degrees(math.atan( (ROCC[1] - TOCC[1]) / (ROCC[0] - TOCC[0]))) + 90
            targetAngle -= baseAngle
        else:
            # If the angle is relative to x Axis, do different math and add it to target angle
            targetAngle = self.transform.cameraToRobotRotation(tracked.rotation[2])

            # TODO: Come up with a system for adjusting the direction of a wrist with robots of higher axis
            directionOfWrist = -1
            targetAngle *= directionOfWrist


        # Add the "Relative" angle to the wrist
        targetAngle += relativeAngle
        targetAngle = rv.normalizeAngle(targetAngle)

        # Normalize the value so that it's between 0 and 180
        while targetAngle < 0: targetAngle += 180
        while targetAngle > 180: targetAngle -= 180



        # Set the robots wrist to the new value
        self.robot.setServoAngles(servo3 = targetAngle)


        return True
Exemplo n.º 11
0
    def run(self):
        if len(self.errors) > 0: return False

        newX, successX = self.interpreter.evaluateExpression(self.parameters['x'])
        newY, successY = self.interpreter.evaluateExpression(self.parameters['y'])
        newZ, successZ = self.interpreter.evaluateExpression(self.parameters['z'])


        # Special case: If the parameter is "" set the new val to None and success to True
        if self.parameters['x'] == "": newX, successX = None, True
        if self.parameters['y'] == "": newY, successY = None, True
        if self.parameters['z'] == "": newZ, successZ = None, True



        # If X Y and Z could not be evaluated correctly, quit
        if not successX or not successY or not successZ:
            printf("ERROR in parsing either X Y or Z: ", successX, successY, successZ)
            return False


        printf("Moving robot to obj, relative XYZ is:  ", newX, " ", newY, " ", newZ)


        # Get a super recent frame of the object
        trackedObj = self.vision.getObjectBruteAccurate(self.trackable,
                                                        minPoints   = rv.MIN_POINTS_FOCUS,
                                                        maxAge= rv.MAX_FRAME_AGE_MOVE)
        if trackedObj is None: return False



        # Get the object position
        printf("Found object. Moving to XY Location now.")
        pos     = rv.getPositionTransform(trackedObj.center, direction="toRob", ptPairs=self.ptPairs)
        pos[2] += trackedObj.view.height



        # Create a function that will return "None" if new val is None, and otherwise it will sum pos and new val
        printf(newX is None, newX)
        noneSum = lambda objPos, newPos: None if newPos is None else objPos + newPos



        # Set the robots position. Any "None" values simply don't change anything
        self.robot.setPos(x=noneSum(pos[0], newX),
                          y=noneSum(pos[1], newY),
                          z=noneSum(pos[2], newZ))
        return True
Exemplo n.º 12
0
    def run(self):
        if len(self.errors): return

        # First, move to the position like the normal MoveXYZ command does
        success = super(VisionMoveXYZCommand, self).run()

        # Check if the parent couldn't run correctly. If not, exit
        if not success: return False


        # Get the new position that the robot should be at
        destRobCoord = self.robot.coord[:]


        # Get the robots new position
        currentCamCoord, avgMag, avgDir = self.vision.getObjectSpeedDirectionAvg(self.rbMarker)

        # If the robot couldn't be seen, exit
        if currentCamCoord is None:
            printf("Could not find robot marker after move. Exiting without Vision adjustment.")
            return False


        # Get the robots "True" position from the cameras point of view
        currentRobPos = rv.getPositionTransform(currentCamCoord, "toRob", self.ptPairs)


        # Find the move offset (aka, the error and how to correct it)
        offset        = np.asarray(destRobCoord) - np.asarray(currentRobPos)
        magnitude = np.linalg.norm(offset)


        # If there's too much error, something bad must have happened. Avoid doing any corrections
        if magnitude > 5:
            printf("Correction magnitude too high. Canceling move. Offset: ", offset, "Magnitude: ", magnitude)
            return False


        # Move the offset amount
        printf("Correcting move. Offset: ", offset, " Magnitude: ", magnitude)
        self.robot.setPos(coord=offset, relative=True)

        return True
Exemplo n.º 13
0
    def run(self):
        if len(self.errors) > 0: return False

        ret = rv.pickupObject(self.trackable, self.rbMarker, self.robot, self.vision, self.transform)

        return ret