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 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 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 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 angleToBallToOwnGoal(absCoord): ball = blackboard.localisation.ballPos ballRR = blackboard.localisation.ballPosRR goalDir = angleToOwnGoal(ball) return MathUtil.normalisedTheta(goalDir - (absCoord.theta + ballRR.heading))
def angleToGoalRight(absCoord): phi = Vector2D.angleBetween(ENEMY_GOAL_INNER_RIGHT, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def angleToPenalty(absCoord): phi = Vector2D.angleBetween(ENEMY_PENALTY_CENTER, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def ballAngleToPoint(point): ballPos = blackboard.localisation.ballPos return MathUtil.normalisedTheta(math.atan2(point[1] - ballPos.y, point[0] - ballPos.x))
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 myAngleToGoalShot(commitToKick=Kick.MIDDLE): goalAngle = angleToGoalShot(Global.ballWorldPos(), Global.myPose(), blackboard.localisation.robotObstacles, commitToKick) return (MathUtil.normalisedTheta(goalAngle[0] - Global.myHeading()), goalAngle[2])
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 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 angleToOwnGoal(absCoord): phi = Vector2D.angleBetween(OWN_GOAL_BEHIND_CENTER, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def angleToBallToOwnGoal(absCoord): ball = blackboard.localisation.ballPos ballRR = blackboard.localisation.ballPosRR goalDir = angleToOwnGoal(ball) return MathUtil.normalisedTheta(goalDir - (absCoord.theta + ballRR.heading))
def ballAngleToPoint(point): ballPos = blackboard.localisation.ballPos return MathUtil.normalisedTheta( math.atan2(point[1] - ballPos.y, point[0] - ballPos.x))
def angleToPenalty(absCoord): phi = Vector2D.angleBetween(ENEMY_PENALTY_CENTER, 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)
def angleToPoint(point, absCoord): phi = Vector2D.angleBetween(point, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def angleToPoint(point, absCoord): phi = Vector2D.angleBetween(point, 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))