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