def comeBehind(bot, tar, obj):
result_pst = Position()
aglTar2Obj = get_angle(tar, obj)
aglBot2Tar = get_angle(tar, bot)
diffAglBot2Obj = aglBot2Tar - aglTar2Obj
if get_distance(bot, tar) > 130:
if 0 <= diffAglBot2Obj < 1.57:
result_pst = stayOutsideCircle(tar, obj, get_distance(obj, tar) + 500)
result_pst.x = result_pst.x + 500 * math.cos(aglTar2Obj + 1.57)
result_pst.y = result_pst.y + 500 * math.sin(aglTar2Obj + 1.57)
elif -1.57 <= diffAglBot2Obj < 0:
result_pst = stayOutsideCircle(tar, obj, get_distance(obj, tar) + 500)
result_pst.x = result_pst.x + 500 * math.cos(aglTar2Obj - 1.57)
result_pst.y = result_pst.y + 500 * math.sin(aglTar2Obj - 1.57)
elif 1.57 <= diffAglBot2Obj < 2.9:
result_pst = stayOutsideCircle(tar, obj, get_distance(obj, tar) + 500)
elif -2.9 <= diffAglBot2Obj < -1.57:
result_pst = stayOutsideCircle(tar, obj, get_distance(obj, tar) + 500)
else:
result_pst = stayOutsideCircle(tar, obj, get_distance(tar, obj) + 90)
else:
result_pst = stayOutsideCircle(tar, obj, get_distance(tar, obj))
return result_pst
def calculate_path(self):
self.initial_position = self.game_state.get_player_position(self.player_id)
self.initial_orientation = self.game_state.get_player_pose(self.player_id).orientation
self.initial_distance = get_distance(self.origin, self.initial_position)
self.initial_angle = get_angle(self.origin, self.initial_position)
self.target_angle = get_angle(self.origin, self.target.position)
self.delta_angle = (self.target_angle + m.pi - self.initial_angle) % (2 * m.pi)
if self.delta_angle > m.pi:
self.delta_angle -= 2 * m.pi
if m.fabs(self.delta_angle) >= self.ANGLE_INCREMENT:
self.num_points = int(
m.fabs(self.delta_angle) // self.ANGLE_INCREMENT) # incrément d'environ pi/8 radians (22.5 degrés)
self.angle_increment = self.delta_angle / self.num_points
else:
self.num_points = 1
self.angle_increment = self.delta_angle
for i in range(self.num_points):
pos = rotate_point_around_origin(self.initial_position, self.origin, (i + 1) * self.angle_increment)
theta = self.initial_orientation + (i + 1) * self.angle_increment
self.pose_list.append(Pose(pos, theta))
self.next_state = self.rotate_around
return MoveToPosition(self.game_state, self.player_id, self.pose_list[self.index])
def calculate_path(self):
self.initial_position = self.player.pose.position
self.initial_orientation = self.player.pose.orientation
self.initial_distance = get_distance(self.origin, self.initial_position)
self.initial_angle = get_angle(self.origin, self.initial_position)
self.target_angle = get_angle(self.origin, self.target.position)
self.delta_angle = (self.target_angle + m.pi - self.initial_angle) % (2 * m.pi)
if self.delta_angle > m.pi:
self.delta_angle -= 2 * m.pi
if m.fabs(self.delta_angle) >= self.ANGLE_INCREMENT:
self.num_points = int(
m.fabs(self.delta_angle) // self.ANGLE_INCREMENT) # incrément d'environ pi/8 radians (22.5 degrés)
self.angle_increment = self.delta_angle / self.num_points
else:
self.num_points = 1
self.angle_increment = self.delta_angle
for i in range(self.num_points):
pos = rotate_point_around_origin(self.initial_position, self.origin, (i + 1) * self.angle_increment)
theta = self.initial_orientation + (i + 1) * self.angle_increment
self.pose_list.append(Pose(pos, theta))
self.next_state = self.rotate_around
return MoveToPosition(self.game_state, self.player, self.pose_list[self.index])
def _bougerPlusAim(self, joueur):
#TODO : ajuster la deadzone en fonction du type du goal
destination = self._convertirPosition(self.robot_goals[joueur])
cible = self._convertirPosition(self.robot_aim[joueur])
deadzone = 225 #225 est la deadzone nécessaire pour que le robot ne fonce pas dans la balle
player = self.team.players[joueur]
dist = geometry.get_distance(player.pose.position, destination)
angle = m.fabs(
geometry.get_angle(player.pose.position, cible) -
player.pose.orientation) #angle between the robot and the ball
if (0 < dist <= deadzone and 0 < angle <= 0.09
): #0.087 rad = 5 deg : marge d'erreur de l'orientation
self._succeed(joueur)
elif (dist > deadzone and 0 < angle <= 0.09):
command = Command.MoveTo(player, self.team, destination)
self._send_command(command)
elif (0 < dist <= deadzone and angle > 0.09):
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.Rotate(player, self.team, orientation)
self._send_command(command)
else:
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.MoveToAndRotate(
player, self.team, Pose(destination, orientation))
self._send_command(command)
"""
def kickBallAction(self, i):
bot_pst = self.team.players[i].pose.position
tar_pst = self.field.ball.position
goal_pst = Position(-4500, -150) if i else Position(-4500, 150)
next_pst = self.comeBehind(bot_pst, tar_pst, goal_pst)
agl = get_angle(bot_pst, goal_pst)
agl2 = get_angle(bot_pst, tar_pst)
if get_distance(tar_pst, bot_pst) < 130 and -0.045 < agl - agl2 < 0.045 and tar_pst.x < -1000:
self.kickAction(i, force=8)
else:
self._send_command(Command.MoveToAndRotate(self.team.players[i], self.team, Pose(next_pst, agl)))
def passBallAction(self, i):
bot_pst = self.team.players[i].pose.position
tar_pst = self.field.ball.position
goal_pst = self.team.players[not i].pose.position + Position(-50, 0)
next_pst = self.comeBehind(bot_pst, tar_pst, goal_pst)
agl = get_angle(bot_pst, goal_pst)
agl2 = get_angle(bot_pst, tar_pst)
if get_distance(tar_pst, bot_pst) < 130 and -0.045 < agl - agl2 < 0.045:
kick_f = get_distance(goal_pst, bot_pst) // 500
self.kickAction(i, force=kick_f)
else:
self._send_command(Command.MoveToAndRotate(self.team.players[i], self.team, Pose(next_pst, agl)))
def receptBallAction(self, i):
bot_pst = self.team.players[i].pose.position
ball_pst = self.field.ball.position
agl1 = get_angle(bot_pst, ball_pst)
agl2 = get_angle(bot_pst, Position(-4500, 0))
agl = agl2 - agl1
agl = agl1 + agl * 0.3
if i:
next_pst = stayInsideSquare(ball_pst, -1500, -1501, -4500, 4500)
else:
next_pst = stayInsideSquare(ball_pst, 1501, 1500, -4500, 4500)
self._send_command(Command.MoveToAndRotate(self.team.players[i], self.team, Pose(next_pst, agl)))
def get_behind_ball(self):
self.status_flag = Flags.WIP
dist = self._get_distance_from_ball()
angle = get_angle(self.game_state.get_player_pose(self.player_id).position, self.target.position)
player_x = self.game_state.game.friends.players[self.player_id].pose.position.x
player_y = self.game_state.game.friends.players[self.player_id].pose.position.y
ball_x = self.game_state.get_ball_position().x
ball_y = self.game_state.get_ball_position().y
target_x = self.target.position.x
target_y = self.target.position.y
vector_player_2_ball = np.array([ball_x - player_x, ball_y - player_y])
vector_target_2_ball = np.array([ball_x - target_x, ball_y - target_y])
angle_ball_2_target = np.arctan2(vector_target_2_ball[1], vector_target_2_ball[0])
vector_player_2_ball /= np.linalg.norm(vector_player_2_ball)
vector_target_2_ball /= np.linalg.norm(vector_target_2_ball)
if np.dot(vector_player_2_ball, vector_target_2_ball) < - 0.95:
if self.game_state.game.friends.players[self.player_id].velocity[2] < 0.01:
print(abs(angle_ball_2_target - self.game_state.game.friends.players[self.player_id].pose.orientation))
if abs(abs(angle_ball_2_target - self.game_state.game.friends.players[self.player_id].pose.orientation) - np.pi) < 0.1 or \
abs(angle_ball_2_target - self.game_state.game.friends.players[self.player_id].pose.orientation) < 0.1:
self.last_time = time.time()
self.next_state = self.start_dribbler
else:
# self.debug.add_log(4, "Distance from ball: {}".format(dist))
self.next_state = self.get_behind_ball
return GoBehind(self.game_state, self.player_id, self.game_state.get_ball_position()+Position(vector_player_2_ball[0]*70, vector_player_2_ball[1] * 70), self.target.position,
self.game_state.const["DISTANCE_BEHIND"])
def exec(self):
"""
Calcul la pose que doit prendre le gardien en fonction de la position de la balle.
:return: Un tuple (Pose, kick) où Pose est la destination du gardien et kick est nul (on ne botte pas)
"""
goalkeeper_position = self.game_state.get_player_pose(self.player_id).position
ball_position = self.game_state.get_ball_position()
goal_x = FIELD_X_RIGHT if self.is_right_goal else FIELD_X_LEFT
goal_position = Position(goal_x, 0)
# Calcul de la position d'interception entre la balle et le centre du but
destination_position = get_closest_point_on_line(goalkeeper_position, goal_position, ball_position)
# Vérification que destination_position respecte la distance minimale
destination_position = stayOutsideCircle(destination_position, goal_position, self.minimum_distance)
# Vérification que destination_position respecte la distance maximale
if self.maximum_distance is None:
destination_position = stayInsideGoalArea(destination_position, self.is_right_goal)
else:
destination_position = stayInsideCircle(destination_position, goal_position, self.maximum_distance)
# Calcul de l'orientation de la pose de destination
destination_orientation = get_angle(destination_position, ball_position)
destination_pose = Pose(destination_position, destination_orientation)
kick_strength = 0
return AICommand(destination_pose, kick_strength)
def exec(self):
"""
Calcul la pose que doit prendre le gardien en fonction de la position de la balle.
:return: Un tuple (Pose, kick) où Pose est la destination du gardien et kick est nul (on ne botte pas)
"""
goalkeeper_position = self.game_state.get_player_position(self.player_id)
ball_position = self.game_state.get_ball_position()
goal_x = self.game_state.const["FIELD_X_RIGHT"] if self.is_right_goal else self.game_state.const["FIELD_X_LEFT"]
goal_position = Position(goal_x, 0)
# Calcul de la position d'interception entre la balle et le centre du but
destination_position = get_closest_point_on_line(goalkeeper_position, goal_position, ball_position)
# Vérification que destination_position respecte la distance minimale
destination_position = stayOutsideCircle(destination_position, goal_position, self.minimum_distance)
# Vérification que destination_position respecte la distance maximale
if self.maximum_distance is None:
destination_position = self.game_state.game.field.stay_inside_goal_area(destination_position,
self.is_right_goal)
else:
destination_position = stayInsideCircle(destination_position, goal_position, self.maximum_distance)
# Calcul de l'orientation de la pose de destination
destination_orientation = get_angle(destination_position, ball_position)
destination_pose = Pose(destination_position, destination_orientation)
kick_strength = 0
return AICommand(self.player_id, AICommandType.MOVE, **{"pose_goal": destination_pose})
def stay_inside(self, point):
if self.is_outside(point):
agl = get_angle(self.centre, point)
x = self.centre.x + self.rayon * cos(agl)
y = self.centre.y + self.rayon * sin(agl)
return Position(x, y)
else:
return Position(point.x, point.y)
def stay_inside(self, point):
if self.is_outside(point):
agl = get_angle(self.centre, point)
x = self.centre.x + self.rayon * cos(agl)
y = self.centre.y + self.rayon * sin(agl)
return Position(x, y)
else:
return Position(point.x, point.y)
def exec(self):
"""
Place le robot afin qu'il prenne le contrôle de la balle
:return: Un tuple (Pose, kick) où Pose est la destination du joueur et kick est nul (on ne botte pas)
"""
ball_position = self.game_state.get_ball_position()
destination_orientation = get_angle(self.game_state.get_player_pose(self.player_id).position, ball_position)
destination_pose = {"pose_goal": Pose(ball_position, destination_orientation)}
return AICommand(self.player_id, AICommandType.MOVE, **destination_pose)
def get_destination(self):
"""
Calcule le point situé à x pixels derrière la position 1 par rapport à la position 2
:return: Un tuple (Pose, kick) où Pose est la destination du joueur et kick est nul (on ne botte pas)
"""
delta_x = self.position2.x - self.position1.x
delta_y = self.position2.y - self.position1.y
theta = math.atan2(delta_y, delta_x)
x = self.position1.x - self.distance_behind * math.cos(theta)
y = self.position1.y - self.distance_behind * math.sin(theta)
player_x = self.p_game_state.game.friends.players[self.player_id].pose.position.x
player_y = self.p_game_state.game.friends.players[self.player_id].pose.position.y
norm_player_2_position2 = math.sqrt((player_x - self.position2.x) ** 2+(player_y - self.position2.y) ** 2)
norm_position1_2_position2 = math.sqrt((self.position1.x - self.position2.x) ** 2 + (self.position1.y - self.position2.y) ** 2)
if norm_player_2_position2 < norm_position1_2_position2:
print(norm_player_2_position2)
print(norm_position1_2_position2)
#on doit contourner l'objectif
vecteur_position1_2_position2 = np.array([self.position1.x - self.position2.x,
self.position1.y - self.position2.y, 0])
vecteur_vertical = np.array([0, 0, 1])
vecteur_player_2_position1 = np.array([self.position1.x - player_x,
self.position1.y - player_y, 0])
vecteur_perp = np.cross(vecteur_position1_2_position2, vecteur_vertical)
vecteur_perp /= np.linalg.norm(vecteur_perp)
if np.dot(vecteur_perp, vecteur_player_2_position1) > 0:
vecteur_perp = -vecteur_perp
position_intermediaire_x = x + vecteur_perp[0] * self.rayon_avoid
position_intermediaire_y = y + vecteur_perp[1] * self.rayon_avoid
if math.sqrt((player_x-position_intermediaire_x)**2+(player_y-position_intermediaire_y)**2) < 50:
position_intermediaire_x += vecteur_perp[0] * self.rayon_avoid * 2
position_intermediaire_y += vecteur_perp[1] * self.rayon_avoid * 2
destination_position = Position(position_intermediaire_x, position_intermediaire_y)
else:
if math.sqrt((player_x-x)**2+(player_y-y)**2) < 50:
x -= math.cos(theta) * 2
y -= math.sin(theta) * 2
destination_position = Position(x, y)
# Calcul de l'orientation de la pose de destination
destination_orientation = get_angle(destination_position, self.position1)
destination_pose = Pose(destination_position, destination_orientation)
DebugInterface().add_log(1, "orientation go behind {}".format(destination_orientation))
return destination_pose
def exec(self):
"""
Place le robot afin qu'il prenne le contrôle de la balle
:return: Un tuple (Pose, kick) où Pose est la destination du joueur et kick est nul (on ne botte pas)
"""
ball_position = self.game_state.get_ball_position()
destination_orientation = get_angle(self.game_state.get_player_pose(self.player_id).position, ball_position)
destination_pose = Pose(ball_position, destination_orientation)
kick_strength = 0
return AICommand(destination_pose, kick_strength)
def _bougerPlusAim(self, joueur, deadzone=None):
destination = self._convertirPosition(self.robot_goals[joueur])
cible = self._convertirPosition(self.robot_aim[joueur])
if not deadzone:
deadzone = self._getDeadZone(self.robot_goals[joueur])
player = self.team.players[joueur]
dist = geometry.get_distance(player.pose.position, destination)
angle = m.fabs(geometry.get_angle(player.pose.position, cible) - player.pose.orientation) #angle between the robot and the ball
if(dist <= deadzone and angle <= 0.01): #0.087 rad = 5 deg : marge d'erreur de l'orientation
self._succeed(joueur)
elif(dist > deadzone and angle <= 0.01):
command = Command.MoveTo(player, self.team, destination)
self._send_command(command)
elif(dist <= deadzone and angle > 0.01):
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.Rotate(player, self.team, orientation)
self._send_command(command)
else:
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.MoveToAndRotate(player, self.team, Pose(destination, orientation))
self._send_command(command)
def _bougerPlusAim(self, joueur, deadzone=None):
destination = self._convertirPosition(self.robot_goals[joueur])
cible = self._convertirPosition(self.robot_aim[joueur])
if not deadzone:
deadzone = self._getDeadZone(self.robot_goals[joueur])
player = self.team.players[joueur]
dist = geometry.get_distance(player.pose.position, destination)
angle = m.fabs(geometry.get_angle(player.pose.position, cible) - player.pose.orientation) #angle between the robot and the ball
if(dist <= deadzone and angle <= 0.01): #0.087 rad = 5 deg : marge d'erreur de l'orientation
self._succeed(joueur)
elif(dist > deadzone and angle <= 0.01):
command = Command.MoveTo(player, self.team, destination)
self._send_command(command)
elif(dist <= deadzone and angle > 0.01):
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.Rotate(player, self.team, orientation)
self._send_command(command)
else:
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.MoveToAndRotate(player, self.team, Pose(destination, orientation))
self._send_command(command)
def exec(self):
"""
Place le robot afin qu'il prenne le contrôle de la balle
:return: Un tuple (Pose, kick) où Pose est la destination du joueur et kick est nul (on ne botte pas)
"""
ball_position = self.game_state.get_ball_position()
destination_orientation = get_angle(self.player.pose.position,
ball_position)
destination_pose = {
"pose_goal": Pose(ball_position, destination_orientation)
}
return AICommand(self.player, AICommandType.MOVE, **destination_pose)
def exec(self):
"""
Exécute le déplacement en tenant compte de la possession de la balle. Le robot se déplace vers la destination,
mais s'oriente de façon à garder la balle sur le dribleur. C'est la responsabilité de la Tactique de faire les
corrections de trajectoire nécessaire.
:return:
"""
# TODO: Améliorer le comportement en ajoutant l'intervalle d'anle correspondant à la largeur du dribbleur
destination_orientation = get_angle(
self.player.pose.position, self.game_state.get_ball_position())
destination_pose = Pose(self.destination, destination_orientation)
return AICommand(self.player, AICommandType.MOVE,
**{"pose_goal": destination_pose})
def exec(self):
"""
Exécute le déplacement en tenant compte de la possession de la balle. Le robot se déplace vers la destination,
mais s'oriente de façon à garder la balle sur le dribleur. C'est la responsabilité de la Tactique de faire les
corrections de trajectoire nécessaire.
:return: Un tuple (Pose, kick) où Pose est la destination du joueur kick est faux (on ne botte pas)
"""
# TODO: Améliorer le comportement en ajoutant l'intervalle d'anle correspondant à la largeur du dribbleur
destination_orientation = get_angle(self.game_state.get_player_pose(self.player_id).position,
self.game_state.get_ball_position())
destination_pose = Pose(self.destination, destination_orientation)
kick_strength = 0
return AICommand(self.player_id, AICommandType.MOVE, **{"pose_goal": destination_pose})
def support_other_zone(self):
enemy_positions = self.get_enemy_in_zone()
if len(enemy_positions) == 0:
self.next_state = self.support_other_zone
else:
self.next_state = self.cover_zone
destination = stayInsideSquare(self.game_state.get_ball_position(), self.y_top, self.y_bottom, self.x_left,
self.x_right)
destination = self.game_state.game.field.stay_outside_goal_area(destination, self.is_yellow)
orientation = get_angle(destination, self.game_state.get_ball_position())
return MoveToPosition(self.game_state, self.player_id, Pose(destination, orientation))
def exec(self):
"""
Exécute le déplacement en tenant compte de la possession de la balle. Le robot se déplace vers la destination,
mais s'oriente de façon à garder la balle sur le dribleur. C'est la responsabilité de la Tactique de faire les
corrections de trajectoire nécessaire.
:return: Un tuple (Pose, kick) où Pose est la destination du joueur kick est faux (on ne botte pas)
"""
# TODO: Améliorer le comportement en ajoutant l'intervalle d'anle correspondant à la largeur du dribleur
destination_orientation = get_angle(
self.game_state.get_player_pose(self.player_id).position,
self.game_state.get_ball_position())
destination_pose = Pose(self.destination, destination_orientation)
kick_strength = 0
return AICommand(destination_pose, kick_strength)
def _bougerPlusAim(self, joueur):
#TODO : ajuster la deadzone en fonction du type du goal
destination = self._convertirPosition(self.robot_goals[joueur])
cible = self._convertirPosition(self.robot_aim[joueur])
deadzone = 225 #225 est la deadzone nécessaire pour que le robot ne fonce pas dans la balle
player = self.team.players[joueur]
dist = geometry.get_distance(player.pose.position, destination)
angle = m.fabs(geometry.get_angle(player.pose.position, cible) - player.pose.orientation) #angle between the robot and the ball
if(0 < dist <= deadzone and 0 < angle <= 0.09): #0.087 rad = 5 deg : marge d'erreur de l'orientation
self._succeed(joueur)
elif(dist > deadzone and 0 < angle <= 0.09):
command = Command.MoveTo(player, self.team, destination)
self._send_command(command)
elif(0 < dist <= deadzone and angle > 0.09):
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.Rotate(player, self.team, orientation)
self._send_command(command)
else:
orientation = geometry.get_angle(player.pose.position, cible)
command = Command.MoveToAndRotate(player, self.team, Pose(destination, orientation))
self._send_command(command)
"""
def test_GrabBall(self):
self.grab_ball = GetBall(self.game_state, self.player_id)
self.game_state.game.ball.set_position(Position(5, 0), 0)
ai_cmd = self.grab_ball.exec()
ball_position = self.game_state.get_ball_position()
destination_orientation = get_angle(self.game_state.get_player_pose(self.player_id).position, ball_position)
destination_pose = {"pose_goal": Pose(ball_position, destination_orientation)}
ai_cmd_expected = AICommand(self.player_id, AICommandType.MOVE,
**{"pose_goal": destination_pose})
self.assertEqual(ai_cmd, ai_cmd_expected)
self.game_state.game.ball.set_position(Position(-5, 5), 0)
ai_cmd = self.grab_ball.exec()
ai_cmd_expected = AICommand(Pose(Position(-5, 5), 3*pi/4), 0)
self.assertEqual(ai_cmd, ai_cmd_expected)
def support_other_zone(self):
enemy_positions = self.get_enemy_in_zone()
if len(enemy_positions) == 0:
self.next_state = self.support_other_zone
else:
self.next_state = self.cover_zone
destination = stayInsideSquare(self.game_state.get_ball_position(),
self.y_top, self.y_bottom, self.x_left,
self.x_right)
destination = stayOutsideGoalArea(destination, self.is_yellow)
orientation = get_angle(destination,
self.game_state.get_ball_position())
return MoveTo(self.game_state, self.player_id,
Pose(destination, orientation))
def move_to_catch_ball(self):
ball_position = self.game_state.get_ball_position()
if has_ball(self.game_state, self.player_id):
self.next_state = self.halt
self.status_flag = Flags.SUCCESS
return Idle(self.game_state, self.player)
else: # position the robot to be able to catch the ball
current_position = self.player.pose.position
next_ball_position = ball_position + self.game_state.get_ball_velocity()
destination_position = get_closest_point_on_line(current_position, ball_position, next_ball_position)
rotation_towards_ball = get_angle(destination_position, ball_position)
pose_towards_ball = Pose(destination_position, rotation_towards_ball)
self.next_state = self.move_to_catch_ball
self.status_flag = Flags.WIP
return MoveToPosition(self.game_state, self.player, pose_towards_ball)
def move_to_catch_ball(self):
ball_position = self.game_state.get_ball_position()
if has_ball(self.game_state, self.player_id):
self.next_state = self.halt
self.status_flag = Flags.SUCCESS
return Idle(self.game_state, self.player_id)
else: # position the robot to be able to catch the ball
current_position = self.game_state.get_player_position(self.player_id)
next_ball_position = ball_position + self.game_state.get_ball_velocity()
destination_position = get_closest_point_on_line(current_position, ball_position, next_ball_position)
rotation_towards_ball = get_angle(destination_position, ball_position)
pose_towards_ball = Pose(destination_position, rotation_towards_ball)
self.next_state = self.move_to_catch_ball
self.status_flag = Flags.WIP
return MoveToPosition(self.game_state, self.player_id, pose_towards_ball)
def rotate_towards_ball(self):
if player_grabbed_ball(self.game_state, self.player_id):
self.next_state = self.halt
self.status_flag = Flags.SUCCESS
return Idle(self.game_state, self.player_id)
else: # keep rotating
current_position = self.game_state.get_player_position()
ball_position = self.game_state.get_ball_position()
rotation_towards_ball = get_angle(current_position, ball_position)
pose_towards_ball = Pose(current_position, rotation_towards_ball)
move_to = MoveTo(self.game_state, self.player_id,
pose_towards_ball)
self.next_state = self.rotate_towards_ball
self.status_flag = Flags.WIP
return move_to
def forwarder(self, i):
bot_pst = self.team.players[i].pose.position
bot_dir = self.team.players[i].pose.orientation
ball_pst = self.field.ball.position
goal_pst = self.opponent_team.players[0].pose.position
agl = get_angle(bot_pst, ball_pst)
dst = get_distance(bot_pst, ball_pst)
enemy_dst_top = get_distance(goal_pst, Position(-4500, 800))
enemy_dst_bot = get_distance(goal_pst, Position(-4500, -800))
if time.time() > self.beginTime + 12:
self.kickBallAction(i)
elif enemy_dst_bot > enemy_dst_top:
self.kickBallAction(i) if i else self.passBallAction(i)
else:
self.passBallAction(i) if i else self.kickBallAction(i)
def test_GrabBall(self):
self.grab_ball = GetBall(self.game_state, self.player_id)
self.game_state.game.ball.set_position(Position(5, 0), 0)
ai_cmd = self.grab_ball.exec()
ball_position = self.game_state.get_ball_position()
destination_orientation = get_angle(
self.game_state.get_player_pose(self.player_id).position,
ball_position)
destination_pose = {
"pose_goal": Pose(ball_position, destination_orientation)
}
ai_cmd_expected = AICommand(self.player_id, AICommandType.MOVE,
**{"pose_goal": destination_pose})
self.assertEqual(ai_cmd, ai_cmd_expected)
self.game_state.game.ball.set_position(Position(-5, 5), 0)
ai_cmd = self.grab_ball.exec()
ai_cmd_expected = AICommand(Pose(Position(-5, 5), 3 * pi / 4), 0)
self.assertEqual(ai_cmd, ai_cmd_expected)
def _lance_position(self, joueur):
player = self.team.players[joueur]
robot = self._convertirPosition(player)
balle = self._convertirPosition(self.field.ball)
cible = self._convertirPosition(self.robot_aim[joueur])
dist = geometry.get_distance(robot, balle)
lim_dist = dist*0.5
deadzone = lim_dist if lim_dist > 125 else 60
angle = m.fabs(geometry.get_angle(robot, cible) - player.pose.orientation)
if angle > 0.3:
deadzone = max(deadzone, 200)
#print(deadzone)
angle = m.atan2(robot.y-cible.y,
robot.x-cible.x)
x = balle.x + deadzone*m.cos(angle)
y = balle.y + deadzone*m.sin(angle)
return Position(x, y)
def _lance_position(self, joueur):
player = self.team.players[joueur]
robot = self._convertirPosition(player)
balle = self._convertirPosition(self.field.ball)
cible = self._convertirPosition(self.robot_aim[joueur])
dist = geometry.get_distance(robot, balle)
lim_dist = dist*0.5
deadzone = lim_dist if lim_dist > 125 else 60
angle = m.fabs(geometry.get_angle(robot, cible) - player.pose.orientation)
if angle > 0.3:
deadzone = max(deadzone, 200)
#print(deadzone)
angle = m.atan2(robot.y-cible.y,
robot.x-cible.x)
x = balle.x + deadzone*m.cos(angle)
y = balle.y + deadzone*m.sin(angle)
return Position(x, y)
def __init__(self, game_state):
super().__init__(game_state)
self.passer_id = 1
self.passer_position = self.game_state.get_player_pose(
self.passer_id).position
self.receiver_id = 2
self.receiver_pose = self.game_state.get_player_pose(self.receiver_id)
self.ball_position = self.game_state.get_ball_position()
self.add_tactic(0, GoalKeeper(self.game_state, 0))
self.add_tactic(
self.passer_id,
GoGetBall(self.game_state, self.passer_id, self.receiver_pose,
None))
self.add_tactic(
self.passer_id,
RotateAround(self.game_state, self.passer_id, self.ball_position,
self.receiver_pose.position))
self.add_tactic(
self.passer_id,
PassBall(self.game_state, self.passer_id, self.receiver_pose,
None))
self.add_condition(self.passer_id, 0, 1, self.passer_has_ball)
self.add_condition(self.passer_id, 1, 2, self.ready_to_pass)
self.add_condition(self.passer_id, 2, 0, self.pass_failed)
p = Pose(self.receiver_pose.position,
get_angle(self.receiver_pose.position, self.ball_position))
self.add_tactic(
self.receiver_id,
GoToPositionNoPathfinder(self.game_state, self.receiver_id, p))
self.add_tactic(self.receiver_id,
ReceivePass(self.game_state, self.receiver_id, p))
self.add_condition(self.receiver_id, 0, 1, self.pass_was_made)
for i in range(3, PLAYER_PER_TEAM):
self.add_tactic(i, Stop(self.game_state, i))
def get_destination(self):
"""
Calcule le point situé à x pixels derrière la position 1 par rapport à la position 2
:return: Un tuple (Pose, kick) où Pose est la destination du joueur et kick est nul (on ne botte pas)
"""
delta_x = self.position2.x - self.position1.x
delta_y = self.position2.y - self.position1.y
theta = math.atan2(delta_y, delta_x)
"""
# Calcul des coordonnées derrière la position 1 (destination) selon la position 2
# TODO: calculer le "point derriere" optimal au lieu d une distance egale en x et y
if delta_x > 0:
x = self.position1.x - self.distance_behind * math.cos(theta)
elif delta_x < 0:
x = self.position1.x + self.distance_behind * math.cos(theta)
elif delta_x == 0:
x = self.position1.x
if delta_y > 0:
y = self.position1.y - self.distance_behind * math.sin(theta)
elif delta_y < 0:
y = self.position1.y + self.distance_behind * math.sin(theta)
elif delta_y == 0:
y = self.position1.y
"""
x = self.position1.x - self.distance_behind * math.cos(theta)
y = self.position1.y - self.distance_behind * math.sin(theta)
destination_position = Position(x, y)
# Calcul de l'orientation de la pose de destination
destination_orientation = get_angle(destination_position,
self.position1)
destination_pose = Pose(destination_position, destination_orientation)
return destination_pose
def kickAction(self, i, force=5):
agl = get_angle(self.team.players[i].pose.position, self.field.ball.position)
if time.time() - (self.lastKick + 0.25) > 0 and -0.09 < agl - self.team.players[i].pose.orientation < 0.09:
self.lastKick = time.time()
self._send_command(Command.Kick(self.team.players[i], self.team, force if force <= 8 else 8))
def standState(self):
self._send_command(Command.MoveTo(self.team.players[1], self.team, Position(-2000, -2500)))
next_pst = self.comeBehind(self.team.players[0].pose.position, self.field.ball.position, Position(-2000, 2500))
agl = get_angle(self.team.players[0].pose.position, self.field.ball.position)
self._send_command(Command.MoveToAndRotate(self.team.players[0], self.team, Pose(next_pst, agl)))
def get_score(self):
return get_angle(self.centre, self.marque), get_distance(self.centre, self.marque) / self.rayon
def get_destination(self):
"""
Calcule le point situé à x pixels derrière la position 1 par rapport à la position 2
:return: Un tuple (Pose, kick) où Pose est la destination du joueur et kick est nul (on ne botte pas)
"""
delta_x = self.position2.x - self.position1.x
delta_y = self.position2.y - self.position1.y
theta = math.atan2(delta_y, delta_x)
x = self.position1.x - self.distance_behind * math.cos(theta)
y = self.position1.y - self.distance_behind * math.sin(theta)
player_x = self.player.pose.position.x
player_y = self.player.pose.position.y
norm_player_2_position2 = math.sqrt((player_x - self.position2.x)**2 +
(player_y - self.position2.y)**2)
norm_position1_2_position2 = math.sqrt(
(self.position1.x - self.position2.x)**2 +
(self.position1.y - self.position2.y)**2)
if norm_player_2_position2 < norm_position1_2_position2:
# on doit contourner l'objectif
vecteur_position1_2_position2 = np.array([
self.position1.x - self.position2.x,
self.position1.y - self.position2.y, 0
])
vecteur_vertical = np.array([0, 0, 1])
vecteur_player_2_position1 = np.array(
[self.position1.x - player_x, self.position1.y - player_y, 0])
vecteur_perp = np.cross(vecteur_position1_2_position2,
vecteur_vertical)
vecteur_perp /= np.linalg.norm(vecteur_perp)
if np.dot(vecteur_perp, vecteur_player_2_position1) > 0:
vecteur_perp = -vecteur_perp
position_intermediaire_x = x + vecteur_perp[0] * self.rayon_avoid
position_intermediaire_y = y + vecteur_perp[1] * self.rayon_avoid
if math.sqrt((player_x - position_intermediaire_x)**2 +
(player_y - position_intermediaire_y)**2) < 50:
position_intermediaire_x += vecteur_perp[
0] * self.rayon_avoid * 2
position_intermediaire_y += vecteur_perp[
1] * self.rayon_avoid * 2
destination_position = Position(position_intermediaire_x,
position_intermediaire_y)
else:
if math.sqrt((player_x - x)**2 + (player_y - y)**2) < 50:
x -= math.cos(theta) * 2
y -= math.sin(theta) * 2
destination_position = Position(x, y)
# Calcul de l'orientation de la pose de destination
# TODO why?!? MGL 2017/05/22
destination_orientation = 0
if self.orientation == 'front':
destination_orientation = get_angle(destination_position,
self.position1)
elif self.orientation == 'back':
destination_orientation = get_angle(destination_position,
self.position1) + np.pi
destination_pose = Pose(destination_position, destination_orientation)
return destination_pose
def get_score(self):
return get_angle(
self.centre,
self.marque), get_distance(self.centre, self.marque) / self.rayon
def fetchAction(self, i):
agl = get_angle(self.team.players[i].pose.position, self.field.ball.position)
self._send_command(Command.MoveToAndRotate(self.team.players[i], self.team, Pose(self.field.ball.position, agl)))
def exec(self):
"""
Calcul le point le plus proche du robot sur la droite entre les deux positions
:return: Un tuple (Pose, kick) où Pose est la destination du joueur et kick est nul (on ne botte pas)
"""
robot_position = self.game_state.get_player_pose(
self.player_id).position
delta_x = self.position2.x - self.position1.x
delta_y = self.position2.y - self.position1.y
if delta_x != 0 and delta_y != 0: # droite quelconque
# Équation de la droite reliant les deux positions
a1 = delta_y / delta_x # pente
b1 = self.position1.y - a1 * self.position1.x # ordonnée à l'origine
# Équation de la droite perpendiculaire
a2 = -1 / a1 # pente perpendiculaire à a1
b2 = robot_position.y - a2 * robot_position.x # ordonnée à l'origine
# Calcul des coordonnées de la destination
x = (b2 - b1) / (a1 - a2) # a1*x + b1 = a2*x + b2
y = a1 * x + b1
elif delta_x == 0: # droite verticale
x = self.position1.x
y = robot_position.y
elif delta_y == 0: # droite horizontale
x = robot_position.x
y = self.position1.y
destination_position = Position(x, y)
# Vérification que destination_position se trouve entre position1 et position2
distance_positions = math.sqrt(delta_x**2 + delta_y**2)
distance_dest_pos1 = get_distance(self.position1, destination_position)
distance_dest_pos2 = get_distance(self.position2, destination_position)
if distance_dest_pos1 >= distance_positions and distance_dest_pos1 > distance_dest_pos2:
# Si position2 est entre position1 et destination_position
new_x = self.position2.x - self.minimum_distance * delta_x / distance_positions
new_y = self.position2.y - self.minimum_distance * delta_y / distance_positions
destination_position = Position(new_x, new_y)
elif distance_dest_pos2 >= distance_positions and distance_dest_pos2 > distance_dest_pos1:
# Si position1 est entre position2 et destination_position
new_x = self.position1.x + self.minimum_distance * delta_x / distance_positions
new_y = self.position1.y + self.minimum_distance * delta_y / distance_positions
destination_position = Position(new_x, new_y)
# Vérification que destination_position respecte la distance minimale
if distance_dest_pos1 <= distance_dest_pos2:
destination_position = stayOutsideCircle(destination_position,
self.position1,
self.minimum_distance)
else:
destination_position = stayOutsideCircle(destination_position,
self.position2,
self.minimum_distance)
# Calcul de l'orientation de la pose de destination
destination_orientation = get_angle(destination_position, self.target)
destination_pose = Pose(destination_position, destination_orientation)
kick_strength = 0
return AICommand(destination_pose, kick_strength)