Exemplo n.º 1
0
 def goal_scored_for(self):
     if self.out_of_play():
         if self.ball.x > self.field.length / 2:
             ball_angle_to_goal_post_se = ray_angle(self.ball.x_just_now, self.ball.y_just_now,
                                                         self.field.markings_xy.ix['GoalPostSE'].x,
                                                         self.field.markings_xy.ix['GoalPostSE'].y)
             ball_angle_to_goal_post_ne = ray_angle(self.ball.x_just_now, self.ball.y_just_now,
                                                         self.field.markings_xy.ix['GoalPostNE'].x,
                                                         self.field.markings_xy.ix['GoalPostNE'].y)
             ball_angle_between_goal_posts_e = angular_difference(ball_angle_to_goal_post_se,
                                                                  ball_angle_to_goal_post_ne)
             ball_angle_between_goal_post_se_and_direction = angular_difference(ball_angle_to_goal_post_se,
                                                                                self.ball.angle)
             if within_range(ball_angle_between_goal_post_se_and_direction, 0., ball_angle_between_goal_posts_e):
                 return 'West'
         elif self.ball.x < - self.field.length / 2:
             ball_angle_to_goal_post_nw = ray_angle(self.ball.x_just_now, self.ball.y_just_now,
                                                         self.field.markings_xy.ix['GoalPostNW'].x,
                                                         self.field.markings_xy.ix['GoalPostNW'].y)
             ball_angle_to_goal_post_sw = ray_angle(self.ball.x_just_now, self.ball.y_just_now,
                                                         self.field.markings_xy.ix['GoalPostSW'].x,
                                                         self.field.markings_xy.ix['GoalPostSW'].y)
             ball_angle_between_goal_posts_w = angular_difference(ball_angle_to_goal_post_nw,
                                                                  ball_angle_to_goal_post_sw)
             ball_angle_between_goal_post_nw_and_direction = angular_difference(ball_angle_to_goal_post_nw,
                                                                                self.ball.angle)
             if within_range(ball_angle_between_goal_post_nw_and_direction, 0., ball_angle_between_goal_posts_w):
                 return 'East'
Exemplo n.º 2
0
 def goal_scored_for(self):
     if self.out_of_play():
         if self.ball.x > self.field.length / 2:
             ball_angle_to_goal_post_se = ray_angle(
                 self.ball.x_just_now, self.ball.y_just_now,
                 self.field.markings_xy.ix['GoalPostSE'].x,
                 self.field.markings_xy.ix['GoalPostSE'].y)
             ball_angle_to_goal_post_ne = ray_angle(
                 self.ball.x_just_now, self.ball.y_just_now,
                 self.field.markings_xy.ix['GoalPostNE'].x,
                 self.field.markings_xy.ix['GoalPostNE'].y)
             ball_angle_between_goal_posts_e = angular_difference(
                 ball_angle_to_goal_post_se, ball_angle_to_goal_post_ne)
             ball_angle_between_goal_post_se_and_direction = angular_difference(
                 ball_angle_to_goal_post_se, self.ball.angle)
             if within_range(ball_angle_between_goal_post_se_and_direction,
                             0., ball_angle_between_goal_posts_e):
                 return 'West'
         elif self.ball.x < -self.field.length / 2:
             ball_angle_to_goal_post_nw = ray_angle(
                 self.ball.x_just_now, self.ball.y_just_now,
                 self.field.markings_xy.ix['GoalPostNW'].x,
                 self.field.markings_xy.ix['GoalPostNW'].y)
             ball_angle_to_goal_post_sw = ray_angle(
                 self.ball.x_just_now, self.ball.y_just_now,
                 self.field.markings_xy.ix['GoalPostSW'].x,
                 self.field.markings_xy.ix['GoalPostSW'].y)
             ball_angle_between_goal_posts_w = angular_difference(
                 ball_angle_to_goal_post_nw, ball_angle_to_goal_post_sw)
             ball_angle_between_goal_post_nw_and_direction = angular_difference(
                 ball_angle_to_goal_post_nw, self.ball.angle)
             if within_range(ball_angle_between_goal_post_nw_and_direction,
                             0., ball_angle_between_goal_posts_w):
                 return 'East'
Exemplo n.º 3
0
    def observe(self, markings, min_distance_over_distance_sigma_ratio=6.):
        observations___vector = array([[]]).T
        marking_indices = []
        for marking in markings:
            if marking.id in self.SLAM:
                d = euclidean_distance(self.x, self.y, marking.x, marking.y)
                if d >= min_distance_over_distance_sigma_ratio * self.distance_sigma:
                    observed_distance = d + normal(
                        scale=self.distance_sigma /
                        2)  # to avoid over-confidence
                    observed_angle = ray_angle(self.x, self.y, marking.x, marking.y) +\
                        normal(scale=self.angle_sigma / 2)   # to avoid over-confidence
                    observations___vector = vstack(
                        (observations___vector, observed_distance,
                         observed_angle))
                    marking_indices += [self.SLAM[marking.id]]
        if marking_indices:
            current_means = deepcopy(self.EKF.means)
            self.EKF.update(observations___vector,
                            marking_indices=marking_indices)
            if euclidean_distance(
                    current_means[0, 0], current_means[1, 0],
                    self.EKF.means[0, 0],
                    self.EKF.means[1, 0]) > self.inconsistency_threshold:
                self.lost = True

        for marking in markings:
            if marking.id not in self.SLAM:
                self.augment_map(marking)
Exemplo n.º 4
0
 def observe_marking_in_front(self, field, min_distance_over_distance_sigma_ratio=6.):
     min_angle = pi
     marking_in_front = None
     for marking in field.markings:
         a = abs(angular_difference(self.angle, ray_angle(self.x, self.y, marking.x, marking.y)))
         d = euclidean_distance(self.x, self.y, marking.x, marking.y)
         if (a < min_angle) and (d >= min_distance_over_distance_sigma_ratio * self.distance_sigma):
             min_angle = a
             marking_in_front = marking
     self.observe((marking_in_front,))
Exemplo n.º 5
0
 def observation_means(self, xy___vector, marking_indices=tuple()):
     n_markings = len(marking_indices)
     conditional_observation_means___vector = zeros((2 * n_markings, 1))
     self_x, self_y = xy___vector[0:2, 0]
     for i in range(n_markings):
         k = 2 * i
         k_marking = 2 * marking_indices[i]
         marking_x, marking_y = xy___vector[k_marking:(k_marking + 2), 0]
         conditional_observation_means___vector[k, 0] = euclidean_distance(self_x, self_y, marking_x, marking_y)
         conditional_observation_means___vector[k + 1, 0] = ray_angle(self_x, self_y, marking_x, marking_y)
     return conditional_observation_means___vector
Exemplo n.º 6
0
 def observation_means(self, xy___vector, marking_indices=tuple()):
     n_markings = len(marking_indices)
     conditional_observation_means___vector = zeros((2 * n_markings, 1))
     self_x, self_y = xy___vector[0:2, 0]
     for i in range(n_markings):
         k = 2 * i
         k_marking = 2 * marking_indices[i]
         marking_x, marking_y = xy___vector[k_marking:(k_marking + 2), 0]
         conditional_observation_means___vector[k, 0] = euclidean_distance(
             self_x, self_y, marking_x, marking_y)
         conditional_observation_means___vector[k + 1, 0] = ray_angle(
             self_x, self_y, marking_x, marking_y)
     return conditional_observation_means___vector
Exemplo n.º 7
0
    def play_per_second(self):
        self.time += 1
        index_player_having_ball = None
        min_distance_to_ball = inf
        for i in range(self.num_players):
            self.players[i].orient_toward_ball(self.ball)
            self.players[i].accelerate(
                normal(scale=self.players[i].acceleration_sigma))
            self.players[i].run()
            self.players[i].observe_marking_in_front(self.field)
            d = self.players[i].distance_to_ball(self.ball)
            if d <= min(min_distance_to_ball, self.players[i].velocity):
                index_player_having_ball = i
                min_distance_to_ball = d

        if index_player_having_ball is not None:
            player = self.players[index_player_having_ball]
            if player.know_goal():
                estimated_goal_x = 0.
                estimated_goal_y = 0.
                known_goal_posts = set(player.target_goal) & set(player.SLAM)
                num_known_goal_posts = len(known_goal_posts)
                for goal_post in known_goal_posts:
                    k = 2 * player.SLAM[goal_post]
                    estimated_goal_x += player.EKF.means[k, 0]
                    estimated_goal_y += player.EKF.means[k + 1, 0]
                estimated_goal_x /= num_known_goal_posts
                estimated_goal_y /= num_known_goal_posts
                estimated_goal_angle = ray_angle(self.ball.x, self.ball.y, estimated_goal_x, estimated_goal_y) +\
                    normal(scale=player.angle_sigma / 2)   # to avoid over-confidence
                self.ball.kicked(self.ball_kick_velocity, estimated_goal_angle)
            elif player.team == 'West':
                self.ball.kicked(self.ball_kick_velocity,
                                 normal(scale=player.angle_sigma /
                                        2))  # avoid over-confidence
            elif player.team == 'East':
                self.ball.kicked(
                    self.ball_kick_velocity, pi +
                    normal(scale=player.angle_sigma / 2))  # avoid over-confi

        self.ball.roll()
        if self.out_of_play():
            goal_scored_for = self.goal_scored_for()
            if goal_scored_for:
                self.score[goal_scored_for] += 1
            self.ball = Ball(x=uniform(-self.field.length / 2,
                                       self.field.length / 2),
                             y=uniform(-self.field.width / 2,
                                       self.field.width / 2),
                             slow_down=self.ball_slow_down)
Exemplo n.º 8
0
 def observe_marking_in_front(self,
                              field,
                              min_distance_over_distance_sigma_ratio=6.):
     min_angle = pi
     marking_in_front = None
     for marking in field.markings:
         a = abs(
             angular_difference(
                 self.angle, ray_angle(self.x, self.y, marking.x,
                                       marking.y)))
         d = euclidean_distance(self.x, self.y, marking.x, marking.y)
         if (a < min_angle) and (d >= min_distance_over_distance_sigma_ratio
                                 * self.distance_sigma):
             min_angle = a
             marking_in_front = marking
     self.observe((marking_in_front, ))
Exemplo n.º 9
0
    def play_per_second(self):
        self.time += 1
        index_player_having_ball = None
        min_distance_to_ball = inf
        for i in range(self.num_players):
            self.players[i].orient_toward_ball(self.ball)
            self.players[i].accelerate(normal(scale=self.players[i].acceleration_sigma))
            self.players[i].run()
            self.players[i].observe_marking_in_front(self.field)
            d = self.players[i].distance_to_ball(self.ball)
            if d <= min(min_distance_to_ball, self.players[i].velocity):
                index_player_having_ball = i
                min_distance_to_ball = d

        if index_player_having_ball is not None:
            player = self.players[index_player_having_ball]
            if player.know_goal():
                estimated_goal_x = 0.
                estimated_goal_y = 0.
                known_goal_posts = set(player.target_goal) & set(player.SLAM)
                num_known_goal_posts = len(known_goal_posts)
                for goal_post in known_goal_posts:
                    k = 2 * player.SLAM[goal_post]
                    estimated_goal_x += player.EKF.means[k, 0]
                    estimated_goal_y += player.EKF.means[k + 1, 0]
                estimated_goal_x /= num_known_goal_posts
                estimated_goal_y /= num_known_goal_posts
                estimated_goal_angle = ray_angle(self.ball.x, self.ball.y, estimated_goal_x, estimated_goal_y) +\
                    normal(scale=player.angle_sigma / 2)   # to avoid over-confidence
                self.ball.kicked(self.ball_kick_velocity, estimated_goal_angle)
            elif player.team == 'West':
                self.ball.kicked(self.ball_kick_velocity, normal(scale=player.angle_sigma / 2))  # avoid over-confidence
            elif player.team == 'East':
                self.ball.kicked(self.ball_kick_velocity, pi + normal(scale=player.angle_sigma / 2))  # avoid over-confi

        self.ball.roll()
        if self.out_of_play():
            goal_scored_for = self.goal_scored_for()
            if goal_scored_for:
                self.score[goal_scored_for] += 1
            self.ball = Ball(x=uniform(-self.field.length / 2, self.field.length / 2),
                             y=uniform(-self.field.width / 2, self.field.width / 2),
                             slow_down=self.ball_slow_down)
Exemplo n.º 10
0
    def observe(self, markings, min_distance_over_distance_sigma_ratio=6.):
        observations___vector = array([[]]).T
        marking_indices = []
        for marking in markings:
            if marking.id in self.SLAM:
                d = euclidean_distance(self.x, self.y, marking.x, marking.y)
                if d >= min_distance_over_distance_sigma_ratio * self.distance_sigma:
                    observed_distance = d + normal(scale=self.distance_sigma / 2)  # to avoid over-confidence
                    observed_angle = ray_angle(self.x, self.y, marking.x, marking.y) +\
                        normal(scale=self.angle_sigma / 2)   # to avoid over-confidence
                    observations___vector = vstack((observations___vector,
                                                    observed_distance,
                                                    observed_angle))
                    marking_indices += [self.SLAM[marking.id]]
        if marking_indices:
            current_means = deepcopy(self.EKF.means)
            self.EKF.update(observations___vector, marking_indices=marking_indices)
            if euclidean_distance(current_means[0, 0], current_means[1, 0],
                                  self.EKF.means[0, 0], self.EKF.means[1, 0]) > self.inconsistency_threshold:
                self.lost = True

        for marking in markings:
            if marking.id not in self.SLAM:
                self.augment_map(marking)
Exemplo n.º 11
0
    def augment_map(self, marking):

        mapping_jacobi = zeros((2, 2 * (self.num_mapped_markings + 1)))
        mapping_jacobi[0, 0] = 1.
        mapping_jacobi[1, 1] = 1.

        self.num_mapped_markings += 1
        self.SLAM[marking.id] = self.num_mapped_markings

        observed_distance = euclidean_distance(self.x, self.y, marking.x, marking.y) +\
            normal(scale=self.distance_sigma / 2)  # to avoid over-confidence
        distance_minus_1sd = observed_distance - self.distance_sigma
        distance_plus_1sd = observed_distance + self.distance_sigma

        observed_angle = ray_angle(self.x, self.y, marking.x, marking.y) +\
            normal(scale=self.angle_sigma / 2)  # to avoid over-confidence
        c = cos(observed_angle)
        s = sin(observed_angle)
        angle_minus_1sd = observed_angle - self.angle_sigma
        c_minus = cos(angle_minus_1sd)
        s_minus = sin(angle_minus_1sd)
        angle_plus_1sd = observed_angle + self.angle_sigma
        c_plus = cos(angle_plus_1sd)
        s_plus = sin(angle_plus_1sd)

        self_x_mean = self.EKF.means[0, 0]
        self_y_mean = self.EKF.means[1, 0]
        new_marking_x_mean = self_x_mean + c * observed_distance
        new_marking_y_mean = self_y_mean + s * observed_distance
        self.EKF.means = vstack(
            (self.EKF.means, new_marking_x_mean, new_marking_y_mean))

        x_minus_distance_1sd_minus_angle_1sd = self_x_mean + c_minus * distance_minus_1sd
        y_minus_distance_1sd_minus_angle_1sd = self_y_mean + s_minus * distance_minus_1sd
        x_minus_distance_1sd_same_angle = self_x_mean + c * distance_minus_1sd
        y_minus_distance_1sd_same_angle = self_y_mean + s * distance_minus_1sd
        x_minus_distance_1sd_plus_angle_1sd = self_x_mean + c_plus * distance_minus_1sd
        y_minus_distance_1sd_plus_angle_1sd = self_y_mean + s_plus * distance_minus_1sd

        x_same_distance_minus_angle_1sd = self_x_mean + c_minus * observed_distance
        y_same_distance_minus_angle_1sd = self_y_mean + s_minus * observed_distance
        x_same_distance_plus_angle_1sd = self_x_mean + c_plus * observed_distance
        y_same_distance_plus_angle_1sd = self_y_mean + s_plus * observed_distance

        x_plus_distance_1sd_minus_angle_1sd = self_x_mean + c_minus * distance_plus_1sd
        y_plus_distance_1sd_minus_angle_1sd = self_y_mean + s_minus * distance_plus_1sd
        x_plus_distance_1sd_same_angle = self_x_mean + c * distance_plus_1sd
        y_plus_distance_1sd_same_angle = self_y_mean + s * distance_plus_1sd
        x_plus_distance_1sd_plus_angle_1sd = self_x_mean + c_plus * distance_plus_1sd
        y_plus_distance_1sd_plus_angle_1sd = self_y_mean + s_plus * distance_plus_1sd

        x_1sd = array(
            (x_minus_distance_1sd_minus_angle_1sd,
             x_minus_distance_1sd_same_angle,
             x_minus_distance_1sd_plus_angle_1sd,
             x_same_distance_minus_angle_1sd, x_same_distance_plus_angle_1sd,
             x_plus_distance_1sd_minus_angle_1sd,
             x_plus_distance_1sd_same_angle,
             x_plus_distance_1sd_plus_angle_1sd))
        y_1sd = array(
            (y_minus_distance_1sd_minus_angle_1sd,
             y_minus_distance_1sd_same_angle,
             y_minus_distance_1sd_plus_angle_1sd,
             y_same_distance_minus_angle_1sd, y_same_distance_plus_angle_1sd,
             y_plus_distance_1sd_minus_angle_1sd,
             y_plus_distance_1sd_same_angle,
             y_plus_distance_1sd_plus_angle_1sd))

        new_marking_x_own_standard_deviation = max(
            abs(x_1sd - new_marking_x_mean))
        new_marking_x_own_variance = new_marking_x_own_standard_deviation**2
        new_marking_y_own_standard_deviation = max(
            abs(y_1sd - new_marking_y_mean))
        new_marking_y_own_variance = new_marking_y_own_standard_deviation**2
        new_marking_xy_own_covariance = c * s * (self.distance_sigma**2)
        new_marking_xy_covariance_matrix = mapping_jacobi.dot(self.EKF.covariances).dot(mapping_jacobi.T) +\
            array([[new_marking_x_own_variance, new_marking_xy_own_covariance],
                   [new_marking_xy_own_covariance, new_marking_y_own_variance]])

        new_marking_xy_covariance_with_known_xy = mapping_jacobi.dot(
            self.EKF.covariances)

        self.EKF.covariances =\
            vstack((hstack((self.EKF.covariances, new_marking_xy_covariance_with_known_xy.T)),
                    hstack((new_marking_xy_covariance_with_known_xy, new_marking_xy_covariance_matrix))))
Exemplo n.º 12
0
    def augment_map(self, marking):

        mapping_jacobi = zeros((2, 2 * (self.num_mapped_markings + 1)))
        mapping_jacobi[0, 0] = 1.
        mapping_jacobi[1, 1] = 1.

        self.num_mapped_markings += 1
        self.SLAM[marking.id] = self.num_mapped_markings

        observed_distance = euclidean_distance(self.x, self.y, marking.x, marking.y) +\
            normal(scale=self.distance_sigma / 2)  # to avoid over-confidence
        distance_minus_1sd = observed_distance - self.distance_sigma
        distance_plus_1sd = observed_distance + self.distance_sigma

        observed_angle = ray_angle(self.x, self.y, marking.x, marking.y) +\
            normal(scale=self.angle_sigma / 2)  # to avoid over-confidence
        c = cos(observed_angle)
        s = sin(observed_angle)
        angle_minus_1sd = observed_angle - self.angle_sigma
        c_minus = cos(angle_minus_1sd)
        s_minus = sin(angle_minus_1sd)
        angle_plus_1sd = observed_angle + self.angle_sigma
        c_plus = cos(angle_plus_1sd)
        s_plus = sin(angle_plus_1sd)

        self_x_mean = self.EKF.means[0, 0]
        self_y_mean = self.EKF.means[1, 0]
        new_marking_x_mean = self_x_mean + c * observed_distance
        new_marking_y_mean = self_y_mean + s * observed_distance
        self.EKF.means = vstack((self.EKF.means,
                                 new_marking_x_mean,
                                 new_marking_y_mean))

        x_minus_distance_1sd_minus_angle_1sd = self_x_mean + c_minus * distance_minus_1sd
        y_minus_distance_1sd_minus_angle_1sd = self_y_mean + s_minus * distance_minus_1sd
        x_minus_distance_1sd_same_angle = self_x_mean + c * distance_minus_1sd
        y_minus_distance_1sd_same_angle = self_y_mean + s * distance_minus_1sd
        x_minus_distance_1sd_plus_angle_1sd = self_x_mean + c_plus * distance_minus_1sd
        y_minus_distance_1sd_plus_angle_1sd = self_y_mean + s_plus * distance_minus_1sd

        x_same_distance_minus_angle_1sd = self_x_mean + c_minus * observed_distance
        y_same_distance_minus_angle_1sd = self_y_mean + s_minus * observed_distance
        x_same_distance_plus_angle_1sd = self_x_mean + c_plus * observed_distance
        y_same_distance_plus_angle_1sd = self_y_mean + s_plus * observed_distance

        x_plus_distance_1sd_minus_angle_1sd = self_x_mean + c_minus * distance_plus_1sd
        y_plus_distance_1sd_minus_angle_1sd = self_y_mean + s_minus * distance_plus_1sd
        x_plus_distance_1sd_same_angle = self_x_mean + c * distance_plus_1sd
        y_plus_distance_1sd_same_angle = self_y_mean + s * distance_plus_1sd
        x_plus_distance_1sd_plus_angle_1sd = self_x_mean + c_plus * distance_plus_1sd
        y_plus_distance_1sd_plus_angle_1sd = self_y_mean + s_plus * distance_plus_1sd

        x_1sd = array((x_minus_distance_1sd_minus_angle_1sd, x_minus_distance_1sd_same_angle,
                       x_minus_distance_1sd_plus_angle_1sd, x_same_distance_minus_angle_1sd,
                       x_same_distance_plus_angle_1sd, x_plus_distance_1sd_minus_angle_1sd,
                       x_plus_distance_1sd_same_angle, x_plus_distance_1sd_plus_angle_1sd))
        y_1sd = array((y_minus_distance_1sd_minus_angle_1sd, y_minus_distance_1sd_same_angle,
                       y_minus_distance_1sd_plus_angle_1sd, y_same_distance_minus_angle_1sd,
                       y_same_distance_plus_angle_1sd, y_plus_distance_1sd_minus_angle_1sd,
                       y_plus_distance_1sd_same_angle, y_plus_distance_1sd_plus_angle_1sd))

        new_marking_x_own_standard_deviation = max(abs(x_1sd - new_marking_x_mean))
        new_marking_x_own_variance = new_marking_x_own_standard_deviation ** 2
        new_marking_y_own_standard_deviation = max(abs(y_1sd - new_marking_y_mean))
        new_marking_y_own_variance = new_marking_y_own_standard_deviation ** 2
        new_marking_xy_own_covariance = c * s * (self.distance_sigma ** 2)
        new_marking_xy_covariance_matrix = mapping_jacobi.dot(self.EKF.covariances).dot(mapping_jacobi.T) +\
            array([[new_marking_x_own_variance, new_marking_xy_own_covariance],
                   [new_marking_xy_own_covariance, new_marking_y_own_variance]])

        new_marking_xy_covariance_with_known_xy = mapping_jacobi.dot(self.EKF.covariances)

        self.EKF.covariances =\
            vstack((hstack((self.EKF.covariances, new_marking_xy_covariance_with_known_xy.T)),
                    hstack((new_marking_xy_covariance_with_known_xy, new_marking_xy_covariance_matrix))))