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