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