def reshape_path(self, path, player: OurPlayer):
self.dist_from_path = 50 # mm
self.path = path
self.player = player
self.player_id = player.id
cmd = self.player.ai_command
vel_cruise = cmd.cruise_speed * 1000
# print(vel_cruise)
point_list = [self.path.start]
speed_list = [0]
radius_at_const_speed = (vel_cruise**2) / self.player.max_acc
p1 = self.path.points[0].conv_2_np()
for idx, point in enumerate(self.path.points[1:-1]):
idx += 1
p2 = point.conv_2_np()
p3 = self.path.points[idx + 1].conv_2_np()
theta = np.math.atan2(p3[1] - p2[1],
p3[0] - p2[0]) - np.math.atan2(
p1[1] - p2[1], p1[0] - p2[0])
radius = abs(self.dist_from_path * np.sin(theta / 2) /
(1 - np.sin(theta / 2)))
if radius > radius_at_const_speed:
radius = radius_at_const_speed
self.dist_from_path = -radius + radius / abs(
np.math.sin(theta / 2))
if np.linalg.norm(p1 - p2) < 0.001 or np.linalg.norm(
p2 - p3) < 0.001 or np.linalg.norm(p1 - p3) < 0.001:
# on traite tout le cas ou le problème dégènere
point_list += [p2]
speed_list += [vel_cruise / 1000]
else:
p4 = p2 + np.sqrt(np.square(self.dist_from_path + radius) - radius ** 2) * \
(p1 - p2)/np.linalg.norm(p1-p2)
p5 = p2 + np.sqrt(np.square(self.dist_from_path + radius) - radius ** 2) * \
(p3 - p2) / np.linalg.norm(p3 - p2)
if np.linalg.norm(p4 - p5) > np.linalg.norm(p3 - p1):
point_list += [point]
speed_list += [vel_cruise / 1000]
else:
point_list += [Position.from_np(p4), Position.from_np(p5)]
speed_list += [
np.sqrt(radius / (self.player.max_acc * 1000)),
np.sqrt(radius / (self.player.max_acc * 1000))
]
p1 = point_list[-1][:]
speed_list += [0]
point_list += [self.path.goal]
# print(point_list)
# print(speed_list)
return Path().generate_path_from_points(point_list, speed_list)
def verify_sub_target(self, sub_target):
for pose_obs in self.pose_obstacle:
dist_sub_2_obs = get_distance(Position.from_np(pose_obs),
sub_target)
if dist_sub_2_obs < self.gap_proxy:
return True
return False
def find_closest_obstacle(self, point, path):
dist_point_obs = np.inf
closest_obs = None
closest_player = self.players_obstacles[0].pose.position.conv_2_np()
if get_distance(path.start, path.goal) < 0.001:
return [closest_obs, dist_point_obs, closest_player]
if point == path.start:
return [closest_obs, dist_point_obs, closest_player]
pose_start = path.start.conv_2_np()
direction = (point.conv_2_np() - pose_start
) / np.linalg.norm(point.conv_2_np() - pose_start)
for idx, pose_obs in enumerate(self.pose_obstacle):
vec_robot_2_obs_temp = pose_obs - pose_start
dist_from_path_temp = np.linalg.norm(
np.cross(direction, vec_robot_2_obs_temp))
if self.gap_proxy > dist_from_path_temp and self.is_path_collide(
path, [pose_obs]):
obstacle_pos = Position.from_np(pose_obs)
dist = get_distance(path.start, obstacle_pos)
if dist < dist_point_obs:
dist_point_obs = dist
closest_obs = obstacle_pos
closest_player = self.players_obstacles[idx]
return [closest_obs, dist_point_obs, closest_player]
def go_between_ball_and_target(self):
self.status_flag = Flags.WIP
ball = self.game_state.get_ball_position()
ball_velocity = self.game_state.get_ball_velocity().conv_2_np()
if np.linalg.norm(ball_velocity) > 50:
self.target = Pose(
Position.from_np(ball.conv_2_np() - ball_velocity), 0)
dist_behind = np.linalg.norm(ball_velocity) + 1 / np.sqrt(
np.linalg.norm(ball_velocity))
else:
self.target = None
dist_behind = 250
if self.target is None:
if self.game_state.get_our_team_color() == 0: # yellow
self.target = Pose(
self.game_state.const["FIELD_GOAL_BLUE_MID_GOAL"], 0)
else:
self.target = Pose(
self.game_state.const["FIELD_GOAL_YELLOW_MID_GOAL"], 0)
if self._is_player_towards_ball_and_target():
self.next_state = self.grab_ball
else:
self.next_state = self.go_between_ball_and_target
return GoBehind(self.game_state, self.player, ball,
self.target.position, dist_behind)
def exec(self):
ball = self.game_state.get_ball_position().conv_2_np()
player = self.player.pose.position.conv_2_np()
player_to_ball = ball - player
player_to_ball = 0.3 * player_to_ball / np.linalg.norm(player_to_ball)
speed_pose = Pose(Position.from_np(player_to_ball))
return AICommand(self.player, AICommandType.MOVE, **{"pose_goal": speed_pose, "speed_flag": True})
def find_closest_obstacle(self, point, path):
dist_point_obs = np.inf
closest_obs = None
closest_player = self.players_obstacles[0].pose.position.conv_2_np()
if get_distance(path.start, path.goal) < 0.001:
return closest_obs, dist_point_obs, closest_player
if point == path.start:
return closest_obs, dist_point_obs, closest_player
pose_start = path.start.conv_2_np()
direction = (point.conv_2_np() - pose_start
) / np.linalg.norm(point.conv_2_np() - pose_start)
for idx, pose_obs in enumerate(self.pose_obstacle):
vec_robot_2_obs_temp = pose_obs - pose_start
len_along_path_temp = np.dot(vec_robot_2_obs_temp, direction)
dist_from_path_temp = np.sqrt(
np.linalg.norm(vec_robot_2_obs_temp)**2 -
len_along_path_temp**2)
if self.gap_proxy > dist_from_path_temp and len_along_path_temp > 0:
obstacle_pos = Position.from_np(pose_obs)
dist = get_distance(path.start, obstacle_pos)
if dist < dist_point_obs:
dist_point_obs = dist
closest_obs = obstacle_pos
# print(len(self.players_obstacles), " -> ", idx)
closest_player = self.players_obstacles[idx]
return closest_obs, dist_point_obs, closest_player
def generate_destination(self):
player = self.player.pose.position.conv_2_np()
target = self.target.position.conv_2_np()
player_to_target_orientation = np.arctan2(target[1] - player[1], target[0] - player[0])
target_orientation = self.target.orientation
delta_theta = player_to_target_orientation - target_orientation
delta_theta = min(abs(delta_theta), np.pi/6) * np.sign(delta_theta)
rotation_matrix = np.array([[np.cos(delta_theta), np.sin(delta_theta)], [-np.sin(delta_theta), np.cos(delta_theta)]])
player_to_ball_rot = np.dot(rotation_matrix, player - target)
translation = player_to_ball_rot / np.linalg.norm(player_to_ball_rot) * self.rayon
destination = translation + target
orientation = target_orientation
return Pose(Position.from_np(destination), orientation)
def exec(self):
if self.check_success():
self.status_flag = Flags.SUCCESS
else:
self.status_flag = Flags.WIP
ball = self.game_state.get_ball_position().conv_2_np()
target = self.target.position.conv_2_np()
ball_to_target = target - ball
orientation = np.arctan2(ball_to_target[1], ball_to_target[0])
next_action = RotateAround(self.game_state, self.player,
Pose(Position.from_np(ball), orientation),
90)
return next_action.exec()
def get_behind_ball(self):
self.status_flag = Flags.WIP
player = self.player.pose.position.conv_2_np()
ball = self.game_state.get_ball_position().conv_2_np()
target = self.game_state.get_player_position(self.target_id, True).conv_2_np()
vector_player_2_ball = ball - player
vector_player_2_ball /= np.linalg.norm(vector_player_2_ball)
if self._is_player_towards_ball_and_target():
self.next_state = self.grab_ball
else:
self.next_state = self.get_behind_ball
return GoBehind(self.game_state, self.player, self.game_state.get_ball_position(),
Position.from_np(target), 120, pathfinder_on=True)
def exec(self):
"""
Execute le kick
"""
target = self.target.position.conv_2_np()
player = self.player.pose.position.conv_2_np()
player_to_target = target - player
player_to_target = 0.3 * player_to_target / np.linalg.norm(
player_to_target)
self.speed_pose = Pose(Position.from_np(player_to_target))
return AICommand(
self.player, AICommandType.MOVE, **{
"pose_goal": self.speed_pose,
"speed_flag": True,
"kick": True,
"kick_strength": self.force
})
def push_ball(self):
# self.debug.add_log(1, "Grab ball called")
# self.debug.add_log(1, "vector player 2 ball : {} mm".format(self.vector_norm))
if get_distance(self.last_ball_position, self.player.pose.position) < 40:
self.next_state = self.halt
self.last_time = time.time()
elif self._is_player_opposing_ball_and_target(-0.9):
self.next_state = self.push_ball
else:
self.next_state = self.get_behind_ball
# self.debug.add_log(1, "orientation go get ball {}".format(self.last_angle))
target = self.target.position.conv_2_np()
player = self.player.pose.position.conv_2_np()
player_to_target = target - player
player_to_target = 0.5 * player_to_target / np.linalg.norm(player_to_target)
speed_pose = Pose(Position.from_np(player_to_target))
return Move(self.game_state, self.player, speed_pose)
def move_to_enemy(self):
if self._is_player_between_ball_and_enemy():
self.next_state = self.move_to_enemy
self.status_flag = Flags.SUCCESS
else:
self.next_state = self.go_between_ball_and_enemy
self.status_flag = Flags.WIP
player = self.player.pose.position.conv_2_np()
enemy = self.player.pose.position.conv_2_np()
ball = self.game_state.get_ball_position().conv_2_np()
ball_to_enemy = enemy - ball
player_to_ball = ball - player
destination = enemy - 300 * ball_to_enemy / np.linalg.norm(
ball_to_enemy)
destination_orientation = np.arctan2(player_to_ball[1],
player_to_ball[0])
return MoveToPosition(
self.game_state, self.player,
Pose(Position.from_np(destination), destination_orientation))
def get_destination(self) -> Pose:
"""
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)
"""
player = self.player.pose.position.conv_2_np()
pt1 = self.position1.conv_2_np()
pt2 = self.position2.conv_2_np()
delta = self.minimum_distance * (pt2 - pt1) / np.linalg.norm(pt2 - pt1)
pt1 = pt1 + delta
pt2 = pt2 - delta
pt1_to_player = player - pt1
pt2_to_player = player - pt2
pt1_to_pt2 = pt2 - pt1
destination = np.cross(
pt1_to_player, pt1_to_pt2) / np.linalg.norm(pt1_to_pt2) + player
outside_x = (destination[0] > pt1[0] and destination[0] > pt2[0]) or \
(destination[0] < pt1[0] and destination[0] < pt2[0])
outside_y = (destination[1] > pt1[1] and destination[1] > pt2[1]) or \
(destination[1] < pt1[1] and destination[1] < pt2[1])
if outside_x or outside_y:
if np.linalg.norm(pt1_to_player) > np.linalg.norm(pt2_to_player):
destination = pt1
else:
destination = pt2
target = self.target.conv_2_np()
player_to_target = target - player
destination_orientation = np.arctan2(player_to_target[1],
player_to_target[0])
# TODO remove MGL 2017/05/23
'''
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_goal": Pose(destination_position, destination_orientation)}
kick_strength = 0
'''
return Pose(Position.from_np(destination), destination_orientation)