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)
Beispiel #3
0
    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)
Beispiel #8
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(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)
Beispiel #10
0
    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
Beispiel #12
0
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
Beispiel #17
0
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)
Beispiel #34
0
 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))