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
Exemplo n.º 2
0
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
Exemplo n.º 4
0
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)
Exemplo n.º 5
0
def angleToGoalRight(absCoord):
    phi = Vector2D.angleBetween(ENEMY_GOAL_INNER_RIGHT,
                                Vector2D.makeVector2DCopy(absCoord))
    return MathUtil.normalisedTheta(phi)
Exemplo n.º 6
0
def angleToOwnGoal(absCoord):
    phi = Vector2D.angleBetween(OWN_GOAL_BEHIND_CENTER,
                                Vector2D.makeVector2DCopy(absCoord))
    return MathUtil.normalisedTheta(phi)
Exemplo n.º 7
0
def angleToPenalty(absCoord):
    phi = Vector2D.angleBetween(ENEMY_PENALTY_CENTER,
                                Vector2D.makeVector2DCopy(absCoord))
    return MathUtil.normalisedTheta(phi)
Exemplo n.º 8
0
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 angleToPoint(point, absCoord):
   phi = Vector2D.angleBetween(point, 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 angleToOwnGoal(absCoord):
   phi = Vector2D.angleBetween(OWN_GOAL_BEHIND_CENTER, Vector2D.makeVector2DCopy(absCoord))
   return MathUtil.normalisedTheta(phi)
Exemplo n.º 13
0
def angleToGoal(absCoord):
    phi = Vector2D.angleBetween(ENEMY_GOAL_BEHIND_CENTER,
                                Vector2D.makeVector2DCopy(absCoord))
    return normalisedTheta(phi)