def _tick(self,
x,
y,
theta,
urgency=0,
keepFacing=False,
relative=False,
useAvoidance=True,
extraObstacles=[]):
# LedOverride.override(LedOverride.leftEye, Constants.LEDColour.off)
# LedOverride.override(LedOverride.rightEye, Constants.LEDColour.off)
self.keepFacing = keepFacing
# Save destination for walkingTo
self.world.b_request.walkingToX = int(x)
self.world.b_request.walkingToY = int(y)
if relative:
new = FieldGeometry.addRrToRobot(Global.myPose(), x, y)
self.world.b_request.walkingToX = int(new[0])
self.world.b_request.walkingToY = int(new[1])
# Convert everything to relative.
if relative:
vectorToTarget = Vector2D.Vector2D(x, y)
facingTurn = theta
else:
for i in range(0, len(extraObstacles)):
extraObstacles[
i] = FieldGeometry.globalPointToRobotRelativePoint(
extraObstacles[i])
vectorToTarget, facingTurn = FieldGeometry.globalPoseToRobotRelativePose(
Vector2D.Vector2D(x, y), theta)
# always keep heading the same after avoidance
facingTurn = MathUtil.normalisedTheta(facingTurn)
targetHeading = MathUtil.normalisedTheta(vectorToTarget.heading())
if useAvoidance:
vectorToTarget = self.calculateDestinationViaObstacles(
vectorToTarget, extraObstacles)
# avoidance doesn't change which way robot's facing
self.currentTarget = vectorToTarget
forward = vectorToTarget.x
left = vectorToTarget.y
self.currentState.urgency = urgency
# myPose = Global.myPose()
# print "Pos: (%5.0f, %5.0f < %2.2f) - CurrTarget: (%5.0f, %5.0f < %2.2f) fac?%s, rel?%s, flt: (%4.0f, %4.0f < T%2.2f, F%2.2f)" % (
# myPose.x, myPose.y, degrees(myPose.theta), x, y, degrees(theta), "Y" if keepFacing else "N", "Y" if relative else "N", forward, left, degrees(targetHeading), degrees(facingTurn)
# )
self.currentState.tick(forward, left, targetHeading, facingTurn)
def getGoalPostsFromVision(self):
"""Add Goalposts From Vision.
This takes vision goalposts and marks them as obstacles, so that hopefully we have one more point where we avoid
them (beyond just sonar). The main issue is that these are unfiltered.
"""
# If you see a goalpost -> Trigger a hysteresis item.
# Else decay it.
# If there is some hysteresis from a goalpost.
# If you see it: Avoid the vision goalpost.
# If you don't: Avoid the localisation one.
posts = Global.getVisionPosts()
posts_with_distance = []
obstacles = []
for post in posts:
if not 0 < post.rr.distance < (Constants.GOAL_POST_ABS_Y * 2):
# Sometimes posts are -1 in distance, I think when we don't trust our calculation. Don't try to avoid these.
# Also don't include posts we see across the field / far away, byt ignoring any over 1 goal width away.
continue
dist = max(0, post.rr.distance - Constants.GOAL_POST_DIAMETER / 2)
posts_with_distance.append(
Vector2D.makeVector2DFromDistHeading(dist, post.rr.heading))
if posts_with_distance:
self.visionPostHys.resetMax()
else:
self.visionPostHys.down()
#print "HysVal: %3d, PostsLen: %2d, DistPostsLen: %2d" % (self.visionPostHys.value, len(posts), len(posts_with_distance))
if self.visionPostHys.value > 0:
if posts_with_distance:
# If we can see the posts, avoid the posts we're seeing.
#print "Avoiding from vision"
obstacles.extend(posts_with_distance)
# print "adding posts from vision"
else:
# If we can't see a goalpost in vision, avoid the one in localisation (as an approximation so we don't
# stop avoiding as we turn our head).
robotPose = Global.myPose()
for (obs_x, obs_y, obs_diam) in WalkToPointV2.FIXED_OBSTACLES:
# Calculate distance to the obstacle.
distance, heading = MathUtil.absToRr(
(robotPose.x, robotPose.y, robotPose.theta),
(obs_x, obs_y))
# print "Adding post from localisation: (%5d < %4.0f)" % (distance, degrees(heading))
distance = max(0, distance - obs_diam / 2)
obstacles.append(
Vector2D.makeVector2DFromDistHeading(
distance, heading))
#print "Avoiding from localisation"
#return obstacles
return []
def calculateTimeToReachPose(myPos, myHeading, targetPos, targetHeading=None):
toTarget = targetPos.minus(myPos)
toTargetHeading = math.atan2(toTarget.y, toTarget.x)
# How far we need to turn to point at the targetPos
toTargetTurn = abs(MathUtil.normalisedTheta(toTargetHeading - myHeading))
# The straightline distance to walk to the targetPos
toTargetDistance = toTarget.length()
# How far we need to turn once we get to the targetPos so that we are facing the targetHeading
if targetHeading is None:
toTargetHeadingTurn = 0.0
else:
toTargetHeadingTurn = abs(MathUtil.normalisedTheta(toTargetHeading - targetHeading))
# approximate time it takes to avoid robots on the way
avoidTime = 0
if toTargetDistance > 400 :
robots = Global.robotObstaclesList()
for robot in robots:
robotPos = Vector2D.makeVector2DCopy(robot.pos)
toRobot = robotPos.minus(myPos)
dist = toRobot.length()
heading = abs(MathUtil.normalisedTheta(toRobot.heading() - toTargetHeading))
# further away robots are less relevant
distValue = min(1, dist / toTargetDistance)
# robots aren't in the way are less relevant
headingValue = min(1, heading / (math.pi/2))
# heading is more relevant than distance, has enough weighting to revert striker bonus time
combinedValue = (1 - distValue ** 4)*(1 - headingValue ** 2) * 3
if combinedValue > avoidTime :
avoidTime = combinedValue
return toTargetTurn/TURN_RATE + toTargetDistance/WALK_RATE + toTargetHeadingTurn/CIRCLE_STRAFE_RATE + avoidTime
def getBallIntersectionWithRobot(maintainCanSeeBall=True):
intervalInSeconds = 1
numSecondsForward = 1.0 # Estimate the ball position up to 1 second away
numIterations = int(round(numSecondsForward / intervalInSeconds))
FRICTION = 0.9 # friction per second
FRICTION_PER_ITERATION = FRICTION**intervalInSeconds
ballVel = Global.ballWorldVelHighConfidence()
ballPos = Global.ballWorldPos()
myHeading = Global.myHeading()
# If he ball is moving slowly, just chase the ball directly
if ballVel.isShorterThan(10.0):
return ballPos
# Dont bother chasing a moving ball if its quite close.
if Global.ballDistance() < 600.0:
return ballPos
ballVel.scale(intervalInSeconds)
robotPos = Global.myPos()
interceptPoint = ballPos
bestChasePoint = ballPos.clone()
seconds = 0.0
for i in xrange(0, numIterations):
seconds += intervalInSeconds
interceptPoint.add(ballVel)
ballVel.scale(FRICTION_PER_ITERATION)
toIntercept = interceptPoint.minus(robotPos)
toInterceptHeading = math.atan2(toIntercept.y, toIntercept.x)
# How far we need to turn to point at the interceptPoint
toInterceptTurn = abs(
MathUtil.normalisedTheta(toInterceptHeading - myHeading))
timeToTurn = toInterceptTurn / TURN_RATE
timeToWalk = toIntercept.length() / WALK_RATE
canReach = (timeToTurn + timeToWalk) <= seconds
# Calculate difference in heading to the current ball position and the intersect position
# to make sure we don't turn too far and lose sight of the ball
v1 = interceptPoint.minus(robotPos).normalised()
v2 = ballPos.minus(robotPos).normalised()
heading = v1.absThetaTo(v2)
if maintainCanSeeBall and heading > math.radians(75):
return bestChasePoint
if canReach:
return bestChasePoint
else:
bestChasePoint = Vector2D.makeVector2DCopy(interceptPoint)
return bestChasePoint
def getBallIntersectionWithRobot(maintainCanSeeBall = True):
intervalInSeconds = 1
numSecondsForward = 1.0 # Estimate the ball position up to 1 second away
numIterations = int(round(numSecondsForward / intervalInSeconds))
FRICTION = 0.9 # friction per second
FRICTION_PER_ITERATION = FRICTION ** intervalInSeconds
ballVel = Global.ballWorldVelHighConfidence()
ballPos = Global.ballWorldPos()
myHeading = Global.myHeading()
# If he ball is moving slowly, just chase the ball directly
if ballVel.isShorterThan(10.0):
return ballPos
# Dont bother chasing a moving ball if its quite close.
if Global.ballDistance() < 600.0:
return ballPos
ballVel.scale(intervalInSeconds)
robotPos = Global.myPos()
interceptPoint = ballPos
bestChasePoint = ballPos.clone()
seconds = 0.0
for i in xrange(0, numIterations):
seconds += intervalInSeconds
interceptPoint.add(ballVel)
ballVel.scale(FRICTION_PER_ITERATION)
toIntercept = interceptPoint.minus(robotPos)
toInterceptHeading = math.atan2(toIntercept.y, toIntercept.x)
# How far we need to turn to point at the interceptPoint
toInterceptTurn = abs(MathUtil.normalisedTheta(toInterceptHeading - myHeading))
timeToTurn = toInterceptTurn / TURN_RATE
timeToWalk = toIntercept.length() / WALK_RATE
canReach = (timeToTurn + timeToWalk) <= seconds
# Calculate difference in heading to the current ball position and the intersect position
# to make sure we don't turn too far and lose sight of the ball
v1 = interceptPoint.minus(robotPos).normalised()
v2 = ballPos.minus(robotPos).normalised()
heading = v1.absThetaTo(v2)
if maintainCanSeeBall and heading > math.radians(75):
return bestChasePoint
if canReach:
return bestChasePoint
else:
bestChasePoint = Vector2D.makeVector2DCopy(interceptPoint)
return bestChasePoint
def init(self, keepFacing=False, relative=False, useAvoidance=True):
self.keepFacing = keepFacing
self.relativeTarget = relative
self.currentState = Initial(self)
self.currentTarget = Vector2D.Vector2D(0, 0)
self.target = None
self.movingAvoidanceHistory = []
self.avoidanceHys = Hysteresis(-10, 10)
self.visionPostHys = Hysteresis(0, 10)
def getSonarObstacles():
obstacles = []
LEFT, RIGHT = 0, 1
nearbyObject = [
Sonar.hasNearbySonarObject(LEFT),
Sonar.hasNearbySonarObject(RIGHT)
]
sonarDebug = ""
if nearbyObject[LEFT]:
sonarDebug += " left"
obstacles.append(
Vector2D.makeVector2DFromDistHeading(Constants.ROBOT_DIAM,
radians(30)))
if nearbyObject[RIGHT]:
sonarDebug += " right"
obstacles.append(
Vector2D.makeVector2DFromDistHeading(Constants.ROBOT_DIAM,
-radians(30)))
if sonarDebug:
robot.say("sonar" + sonarDebug)
return obstacles
def _tick(self,
x,
y,
theta,
urgency=0,
keepFacing=False,
relative=False,
useAvoidance=True,
useOnlySonarAvoid=False):
urgency = min(1.0, urgency)
self.keepFacing = keepFacing
# Convert everything to relative.
if relative:
vectorToTarget = Vector2D.Vector2D(x, y)
facingTurn = theta
else:
vectorToTarget, facingTurn = FieldGeometry.globalPoseToRobotRelativePose(
Vector2D.Vector2D(x, y), theta)
facingTurn = MathUtil.normalisedTheta(facingTurn)
self.currentTarget = vectorToTarget
targetHeading = MathUtil.normalisedTheta(vectorToTarget.heading())
# forward/left are used for final adjustments, rotate to mean pos after the turn is made
if vectorToTarget.isShorterThan(400) or keepFacing:
forward = vectorToTarget.x * cos(
-facingTurn) - vectorToTarget.y * sin(-facingTurn)
left = vectorToTarget.x * sin(
-facingTurn) + vectorToTarget.y * cos(-facingTurn)
else:
forward = vectorToTarget.x
left = vectorToTarget.y
self.currentState.urgency = urgency
self.currentState.tick(forward, left, targetHeading, facingTurn)
def getSharedKickoffTarget():
side_of_field = 1
upfielder = getFirstOfRole(Constants.ROLE_UPFIELDER)
if upfielder >= 0:
upfielder_pos, _ = getTeammatePose(upfielder)
side_of_field = math.copysign(1, upfielder_pos.y)
else:
midfielder = getFirstOfRole(Constants.ROLE_MIDFIELDER)
if midfielder >= 0:
midfielder_pos, _ = getTeammatePose(midfielder)
side_of_field = math.copysign(1, midfielder_pos.y)
# Now we have a side of field to kick to.
return Vector2D.Vector2D(2500, 1200 * side_of_field)
def calculateTimeToReachPose(myPos, myHeading, targetPos, targetHeading=None):
toTarget = targetPos.minus(myPos)
toTargetHeading = math.atan2(toTarget.y, toTarget.x)
# How far we need to turn to point at the targetPos
toTargetTurn = abs(normalisedTheta(toTargetHeading - myHeading))
# The straightline distance to walk to the targetPos
toTargetDistance = toTarget.length()
# How far we need to turn once we get to the targetPos so
# that we are facing the targetHeading
if targetHeading is None:
toTargetHeadingTurn = 0.0
else:
toTargetHeadingTurn = abs(
normalisedTheta(toTargetHeading - targetHeading))
# approximate time it takes to avoid robots on the way
avoidTime = 0
if toTargetDistance > 400:
robots = Global.robotObstaclesList()
for _robot in robots:
robotPos = Vector2D.makeVector2DCopy(_robot.pos)
toRobot = robotPos.minus(myPos)
dist_squared = toRobot.length2()
heading = abs(normalisedTheta(toRobot.heading() - toTargetHeading))
# further away robots are less relevant
distValue = min(1, dist_squared / toTargetDistance**2)
# robots aren't in the way are less relevant
headingValue = min(1, heading / (math.pi / 2))
# heading is more relevant than distance,
# has enough weighting to revert striker bonus time
combinedValue = (1 - distValue**2) * (1 - headingValue**2) * 3
if combinedValue > avoidTime:
avoidTime = combinedValue
return (toTargetTurn / TURN_RATE + toTargetDistance / WALK_RATE +
toTargetHeadingTurn / CIRCLE_STRAFE_RATE + avoidTime)
def get_bot_position(bot, traceable_object, tracker, samples=3, debug=False):
xSamples = []
ySamples = []
for sample in range(samples):
image = tracker.get_video_frame()
timestamp = time.time()
if sample > 0: # ignore the first sample
x, y = tracker._find_traceable_in_image(
image, traceable_object) # side effect: adds mask to tracker
if x:
xSamples.append(x)
if y:
ySamples.append(y)
traceable_object.add_tracking(Vector2D(x, y), timestamp)
if debug:
display_current_view(tracker)
print "{} | {} ".format(xSamples, ySamples)
return (sum(xSamples) / len(xSamples)), (sum(ySamples) / len(ySamples))
def angleToGoalRight(absCoord):
phi = Vector2D.angleBetween(ENEMY_GOAL_INNER_RIGHT,
Vector2D.makeVector2DCopy(absCoord))
return MathUtil.normalisedTheta(phi)
def angleToOwnGoal(absCoord):
phi = Vector2D.angleBetween(OWN_GOAL_BEHIND_CENTER,
Vector2D.makeVector2DCopy(absCoord))
return MathUtil.normalisedTheta(phi)
def angleToPenalty(absCoord):
phi = Vector2D.angleBetween(ENEMY_PENALTY_CENTER,
Vector2D.makeVector2DCopy(absCoord))
return MathUtil.normalisedTheta(phi)
def angleToPoint(point, absCoord):
phi = Vector2D.angleBetween(point, Vector2D.makeVector2DCopy(absCoord))
return MathUtil.normalisedTheta(phi)
def angleBetweenBallAndGoalCenter():
ballPos = blackboard.localisation.ballPos
ballVec = Vector2D.Vector2D(ballPos.x, ballPos.y)
goalCenter = OWN_GOAL_CENTER
phi = Vector2D.angleBetween(ballVec, goalCenter)
return MathUtil.normalisedTheta(phi)
import math
import Global
import Constants
blackboard = None
class Kick(object):
NONE = -1
MIDDLE = 0
LEFT = 1
RIGHT = 2
# Enemy goal vectors.
ENEMY_GOAL_CENTER = Vector2D.Vector2D(Constants.FIELD_LENGTH / 2.0, 0)
ENEMY_GOAL_BEHIND_CENTER = Vector2D.Vector2D(
Constants.FIELD_LENGTH / 2.0 + 100,
0) # +100 offset so angles aren't too sharp near goals
ENEMY_GOAL_INNER_LEFT = Vector2D.Vector2D(
Constants.FIELD_LENGTH / 2.0,
Constants.GOAL_POST_ABS_Y - (Constants.GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_INNER_RIGHT = Vector2D.Vector2D(
Constants.FIELD_LENGTH / 2.0,
-Constants.GOAL_POST_ABS_Y + (Constants.GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_OUTER_LEFT = Vector2D.Vector2D(
Constants.FIELD_LENGTH / 2.0,
Constants.GOAL_POST_ABS_Y + (Constants.GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_OUTER_RIGHT = Vector2D.Vector2D(
Constants.FIELD_LENGTH / 2.0,
-Constants.GOAL_POST_ABS_Y - (Constants.GOAL_POST_DIAMETER / 2))
def angleToPenalty(absCoord): phi = Vector2D.angleBetween(ENEMY_PENALTY_CENTER, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
from util import TeamStatus
import math
import Global
from Constants import (
FIELD_LENGTH,
FIELD_WIDTH,
GOAL_POST_ABS_Y,
GOAL_POST_DIAMETER,
MARKER_CENTER_X,
)
blackboard = None
# Enemy goal vectors.
OFFSET_REDUCE_SHARPNESS = 150 # so angles aren't too sharp near goals
ENEMY_GOAL_CENTER = Vector2D.Vector2D(FIELD_LENGTH / 2.0, 0)
ENEMY_GOAL_BEHIND_CENTER = Vector2D.Vector2D(
FIELD_LENGTH / 2.0 + OFFSET_REDUCE_SHARPNESS, 0)
ENEMY_GOAL_INNER_LEFT = Vector2D.Vector2D(
FIELD_LENGTH / 2.0, GOAL_POST_ABS_Y - (GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_INNER_RIGHT = Vector2D.Vector2D(
FIELD_LENGTH / 2.0, -GOAL_POST_ABS_Y + (GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_OUTER_LEFT = Vector2D.Vector2D(
FIELD_LENGTH / 2.0, GOAL_POST_ABS_Y + (GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_OUTER_RIGHT = Vector2D.Vector2D(
FIELD_LENGTH / 2.0, -GOAL_POST_ABS_Y - (GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_CORNER_LEFT = Vector2D.Vector2D(FIELD_LENGTH / 2.0, FIELD_WIDTH / 2)
ENEMY_GOAL_CORNER_RIGHT = Vector2D.Vector2D(FIELD_LENGTH / 2.0,
-FIELD_WIDTH / 2)
ENEMY_PENALTY_CENTER = Vector2D.Vector2D(MARKER_CENTER_X, 0)
def angleToOwnGoal(absCoord): phi = Vector2D.angleBetween(OWN_GOAL_BEHIND_CENTER, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def angleToGoalRight(absCoord): phi = Vector2D.angleBetween(ENEMY_GOAL_INNER_RIGHT, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def angleIntoField():
toCentre = globalPointToRobotRelativePoint(Vector2D.Vector2D(0, 0))
return toCentre.heading()
def angleToGoal(absCoord):
phi = Vector2D.angleBetween(ENEMY_GOAL_BEHIND_CENTER,
Vector2D.makeVector2DCopy(absCoord))
return normalisedTheta(phi)
def calculateDestinationViaObstacles(self, vectorToTarget, extraObstacles):
isSupporter = len(extraObstacles) > 0
"attractive field of target"
Uatt = PotentialField.getAttractiveField(vectorToTarget)
# let x axis be path
targetHeading = vectorToTarget.heading()
rotatedVectorToTarget = vectorToTarget.rotated(-targetHeading)
# dont avoid obstacles within 100mm to target
rotatedVectorToTarget.x -= copysign(100, rotatedVectorToTarget.x)
"repulsive field of each obstacle"
Urep = Vector2D.Vector2D(0, 0)
obstacles = Global.robotObstaclesList()
for i in range(0, len(obstacles)):
# LedOverride.override(LedOverride.leftEye, Constants.LEDColour.magenta)
logObstacle(obstacles[i].rr.distance, obstacles[i].rr.heading)
obsPos = Vector2D.makeVector2DFromDistHeading(
obstacles[i].rr.distance, obstacles[i].rr.heading)
if isSupporter or isNearPathToTarget(
targetHeading, rotatedVectorToTarget, obsPos):
Urep = Urep.plus(PotentialField.getRepulsiveField(obsPos))
# add sonar and goal posts
obstacles = self.getSonarObstacles()
obstacles.extend(self.getGoalPostsFromVision())
obstacles.extend(extraObstacles)
for i in range(0, len(obstacles)):
logObstacle(obstacles[i].length(), obstacles[i].heading())
if isSupporter or isNearPathToTarget(
targetHeading, rotatedVectorToTarget, obstacles[i]):
Urep = Urep.plus(PotentialField.getRepulsiveField(
obstacles[i]))
# add closest points to field borders
myPos = Global.myPos()
myHeading = Global.myHeading()
BORDER_PADDING = 800
obstacles = [
Vector2D.Vector2D(
Constants.FIELD_LENGTH / 2 + BORDER_PADDING - myPos.x, 0),
Vector2D.Vector2D(
-(Constants.FIELD_LENGTH / 2 + BORDER_PADDING) - myPos.x, 0),
Vector2D.Vector2D(
0, Constants.FIELD_WIDTH / 2 + BORDER_PADDING - myPos.y),
Vector2D.Vector2D(
0, -(Constants.FIELD_WIDTH / 2 + BORDER_PADDING) - myPos.y)
]
for i in range(0, len(obstacles)):
obstacle = obstacles[i].rotate(-myHeading)
logObstacle(obstacle.length(), obstacle.heading())
if isSupporter or isNearPathToTarget(
targetHeading, rotatedVectorToTarget, obstacle):
Urep = Urep.plus(PotentialField.getRepulsiveField(obstacle))
#sum the fields, we only need the heading change
U = Uatt.plus(Urep)
# avoid losing too much forward momentum
headingDiff = clamp(U.heading() - vectorToTarget.heading(),
radians(-70), radians(70))
vectorToTarget.rotate(headingDiff)
logTarget(vectorToTarget, headingDiff)
return vectorToTarget
def angleBetweenBallAndGoalCenter(): ballPos = blackboard.localisation.ballPos ballVec = Vector2D.Vector2D(ballPos.x, ballPos.y) goalCenter = OWN_GOAL_CENTER phi = Vector2D.angleBetween(ballVec, goalCenter) return MathUtil.normalisedTheta(phi)
def angleToPoint(point, absCoord): phi = Vector2D.angleBetween(point, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def getTeammatePose(index):
pose = blackboard.receiver.data[index].robotPos
return (Vector2D.Vector2D(pose.x, pose.y), pose.theta)
