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'
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'
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)
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,))
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
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
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)
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, ))
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)
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)
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))))
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))))