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 _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 _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 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 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))
return toTargetTurn/TURN_RATE + toTargetDistance/WALK_RATE + toTargetHeadingTurn/CIRCLE_STRAFE_RATE
def calculateTimeToReachBall():
opponentGoal = ENEMY_GOAL_CENTER
interceptPoint = getBallIntersectionWithRobot(maintainCanSeeBall=False)
interceptToGoal = opponentGoal.minus(interceptPoint)
interceptToGoalHeading = MathUtil.normalisedTheta(
math.atan2(interceptToGoal.y, interceptToGoal.x))
return calculateTimeToReachPose(Global.myPos(), Global.myHeading(),
interceptPoint, interceptToGoalHeading)
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))
return toTargetTurn / TURN_RATE + toTargetDistance / WALK_RATE + toTargetHeadingTurn / CIRCLE_STRAFE_RATE
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 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 angleToGoalShot(ballPosAbs, robotPos, robotObstacles, commitToKick):
goalRight = angleToGoalRight(ballPosAbs)
goalLeft = angleToGoalLeft(ballPosAbs)
pointsOfAim = [[angleToGoal(ballPosAbs), -1.0, Kick.MIDDLE],
[goalLeft - math.radians(10), -1.0, Kick.LEFT],
[goalRight + math.radians(10), -1.0, Kick.RIGHT]]
# If we've committed to a kick already
if Kick.MIDDLE <= commitToKick <= Kick.RIGHT:
return pointsOfAim[commitToKick]
# If goal angle is so fine that we can only kick for the middle anyway
if pointsOfAim[Kick.LEFT][0] < pointsOfAim[Kick.MIDDLE][0] or pointsOfAim[
Kick.RIGHT][0] > pointsOfAim[Kick.MIDDLE][0]:
return pointsOfAim[Kick.MIDDLE]
nearestDist = 10000
result = pointsOfAim[Kick.MIDDLE]
for i in range(0, len(robotObstacles)):
# Find position of the robots in ball relative terms
ball = (ballPosAbs.x, ballPosAbs.y, 0.0)
rr = MathUtil.absToRr(ball,
MathUtil.rrToAbs(robotPos, robotObstacles[i].rr))
dist = rr[0]
heading = rr[1]
if pointsOfAim[Kick.LEFT][0] > heading > pointsOfAim[Kick.RIGHT][0]:
if dist < nearestDist:
nearestDist = dist
if heading > pointsOfAim[Kick.MIDDLE][0]:
# Robot is to the left
#print "Aiming right"
result = pointsOfAim[Kick.RIGHT]
else:
#print "Aiming left"
# Robot is to the right
result = pointsOfAim[Kick.LEFT]
return result
def _assign_data_to_clusters(data, centroids):
cluster_count = len(centroids)
clusters = [[] for i in range(cluster_count)]
for d in data:
distances = np.zeros(cluster_count)
for i in range(0, cluster_count):
distances[i] = MathUtil.euclidean_distance(d, centroids[i])
i_min = np.argmin(distances)
clusters[i_min].append(d)
return clusters
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 angleToGoalShot(ballPosAbs, robotPos, robotObstacles, commitToKick):
goalRight = angleToGoalRight(ballPosAbs)
goalLeft = angleToGoalLeft(ballPosAbs)
pointsOfAim = [[angleToGoal(ballPosAbs), -1.0, Kick.MIDDLE],
[goalLeft - math.radians(10), -1.0, Kick.LEFT],
[goalRight + math.radians(10), -1.0, Kick.RIGHT]]
# If we've committed to a kick already
if Kick.MIDDLE <= commitToKick <= Kick.RIGHT:
return pointsOfAim[commitToKick]
# If goal angle is so fine that we can only kick for the middle anyway
if pointsOfAim[Kick.LEFT][0] < pointsOfAim[Kick.MIDDLE][0] or pointsOfAim[Kick.RIGHT][0] > pointsOfAim[Kick.MIDDLE][0]:
return pointsOfAim[Kick.MIDDLE]
nearestDist = 10000
result = pointsOfAim[Kick.MIDDLE]
for i in range(0,len(robotObstacles)):
# Find position of the robots in ball relative terms
ball = (ballPosAbs.x, ballPosAbs.y, 0.0)
rr = MathUtil.absToRr(ball, MathUtil.rrToAbs(robotPos, robotObstacles[i].rr))
dist = rr[0]
heading = rr[1]
if pointsOfAim[Kick.LEFT][0] > heading > pointsOfAim[Kick.RIGHT][0]:
if dist < nearestDist:
nearestDist = dist
if heading > pointsOfAim[Kick.MIDDLE][0]:
# Robot is to the left
#print "Aiming right"
result = pointsOfAim[Kick.RIGHT]
else:
#print "Aiming left"
# Robot is to the right
result = pointsOfAim[Kick.LEFT]
return result
def teammateMinWeightedDistToBall():
minDist = robot.FULL_FIELD_LENGTH
ballPosAbs = blackboard.localisation.ballPos
lostCount = blackboard.localisation.ballLostCount
robotPos = blackboard.localisation.robotPos
if lostCount < 100:
robotObs = blackboard.localisation.robotObstacles
for i in range(0,len(robotObs)):
isFriend = isTeamMate(robotObs[i])
# Find position of the robots in absolute terms and discard if near the end of field (false positive)
robotObsAbs = MathUtil.rrToAbs(robotPos, robotObs[i].rr)
if robotObsAbs[0] > 2800:
isFriend = False
if isFriend:
dist = math.hypot(ballPosAbs.x - robotObsAbs[0], ballPosAbs.y - robotObsAbs[1])
if dist < minDist:
minDist = dist
return minDist
def teammateMinWeightedDistToBall():
minDist = robot.FULL_FIELD_LENGTH
ballPosAbs = blackboard.localisation.ballPos
lostCount = blackboard.localisation.ballLostCount
robotPos = blackboard.localisation.robotPos
if lostCount < 100:
robotObs = blackboard.localisation.robotObstacles
for i in range(0, len(robotObs)):
isFriend = isTeamMate(robotObs[i])
# Find position of the robots in absolute terms and discard if near the end of field (false positive)
robotObsAbs = MathUtil.rrToAbs(robotPos, robotObs[i].rr)
if robotObsAbs[0] > 2800:
isFriend = False
if isFriend:
dist = math.hypot(ballPosAbs.x - robotObsAbs[0],
ballPosAbs.y - robotObsAbs[1])
if dist < minDist:
minDist = dist
return minDist
def On(trigger, do=None, to=None):
if isinstance(do, (OutputEvent, basestring)):
do = [do]
elif do == None:
do = []
else:
pass
if isinstance(trigger, basestring):
if MathUtil.is_single_symbol(trigger):
return DoOnEvent(input_event=trigger, do=do, to=to)
else:
return DoOnCondition(condition=trigger, do=do, to=to)
elif isinstance(trigger, OnCondition):
return DoOnCondition(condition=trigger, do=do, to=to)
else:
err = "Unexpected Type for On() trigger: %s %s" % (type(trigger),
str(trigger))
raise NineMLRuntimeError(err)
def myAngleToGoalShot(commitToKick=Kick.MIDDLE):
goalAngle = angleToGoalShot(Global.ballWorldPos(), Global.myPose(),
blackboard.localisation.robotObstacles,
commitToKick)
return (MathUtil.normalisedTheta(goalAngle[0] - Global.myHeading()),
goalAngle[2])
def angleSubtendedByPoints(from_coord, to_coord_a, to_coord_b):
aVector = to_coord_a.minus(from_coord)
bVector = to_coord_b.minus(from_coord)
return MathUtil.angleDiff(aVector.heading(), bVector.heading())
def angleToGoalRight(absCoord): phi = Vector2D.angleBetween(ENEMY_GOAL_INNER_RIGHT, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def calculateTimeToReachBall(): opponentGoal = ENEMY_GOAL_CENTER interceptPoint = getBallIntersectionWithRobot(maintainCanSeeBall = False) interceptToGoal = opponentGoal.minus(interceptPoint) interceptToGoalHeading = MathUtil.normalisedTheta(math.atan2(interceptToGoal.y, interceptToGoal.x)) return calculateTimeToReachPose(Global.myPos(), Global.myHeading(), interceptPoint, interceptToGoalHeading)
def angleToGoalRight(absCoord):
phi = Vector2D.angleBetween(ENEMY_GOAL_INNER_RIGHT,
Vector2D.makeVector2DCopy(absCoord))
return MathUtil.normalisedTheta(phi)
def angleToTeamBallToGoal(teamBallPos, robotPos): heading = MathUtil.normalisedTheta(math.atan2(teamBallPos.y - robotPos.y, teamBallPos.x - robotPos.x)) goalDir = angleToGoal(teamBallPos) return MathUtil.normalisedTheta(goalDir - (robotPos.theta + heading))
def angleToOwnGoal(absCoord): phi = Vector2D.angleBetween(OWN_GOAL_BEHIND_CENTER, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def angleToPoint(point, absCoord):
phi = Vector2D.angleBetween(point, Vector2D.makeVector2DCopy(absCoord))
return MathUtil.normalisedTheta(phi)
def angleToPenalty(absCoord):
phi = Vector2D.angleBetween(ENEMY_PENALTY_CENTER,
Vector2D.makeVector2DCopy(absCoord))
return MathUtil.normalisedTheta(phi)
def angleSubtendedByPoints(from_coord, to_coord_a, to_coord_b): aVector = to_coord_a.minus(from_coord) bVector = to_coord_b.minus(from_coord) return MathUtil.angleDiff(aVector.heading(), bVector.heading())
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 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 myAngleToGoalShot(commitToKick=Kick.MIDDLE): goalAngle = angleToGoalShot(Global.ballWorldPos(), Global.myPose(), blackboard.localisation.robotObstacles, commitToKick) return (MathUtil.normalisedTheta(goalAngle[0] - Global.myHeading()), goalAngle[2])
def ballAngleToPoint(point):
ballPos = blackboard.localisation.ballPos
return MathUtil.normalisedTheta(
math.atan2(point[1] - ballPos.y, point[0] - ballPos.x))
def angleToOwnGoal(absCoord):
phi = Vector2D.angleBetween(OWN_GOAL_BEHIND_CENTER,
Vector2D.makeVector2DCopy(absCoord))
return MathUtil.normalisedTheta(phi)
def update_distance(self, vector):
self.distance = util.euclidean_distance(self.weights, vector)
def angleToBallToOwnGoal(absCoord):
ball = blackboard.localisation.ballPos
ballRR = blackboard.localisation.ballPosRR
goalDir = angleToOwnGoal(ball)
return MathUtil.normalisedTheta(goalDir -
(absCoord.theta + ballRR.heading))
def angleToBallToOwnGoal(absCoord): ball = blackboard.localisation.ballPos ballRR = blackboard.localisation.ballPosRR goalDir = angleToOwnGoal(ball) return MathUtil.normalisedTheta(goalDir - (absCoord.theta + ballRR.heading))
def angleToTeamBallToGoal(teamBallPos, robotPos):
heading = MathUtil.normalisedTheta(
math.atan2(teamBallPos.y - robotPos.y, teamBallPos.x - robotPos.x))
goalDir = angleToGoal(teamBallPos)
return MathUtil.normalisedTheta(goalDir - (robotPos.theta + heading))
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 ballAngleToPoint(point): ballPos = blackboard.localisation.ballPos return MathUtil.normalisedTheta(math.atan2(point[1] - ballPos.y, point[0] - ballPos.x))