diff --git a/Engine/filters/path_smoother.py b/Engine/filters/path_smoother.py index 843d39af..4e53c945 100644 --- a/Engine/filters/path_smoother.py +++ b/Engine/filters/path_smoother.py @@ -9,7 +9,6 @@ def path_smoother(path, speed, end_speed) -> Tuple[Path, float]: - if len(path) < 3: return path, end_speed @@ -25,12 +24,11 @@ def path_smoother(path, speed, end_speed) -> Tuple[Path, float]: point_list = [path.start, p4, p5] turn_radius, _ = compute_turn_radius(*point_list, speed, acc=MAX_LINEAR_ACCELERATION) next_speed = speed_in_corner(turn_radius, acc=MAX_LINEAR_ACCELERATION) - #print(next_speed) return Path.from_sequence(point_list), next_speed def compute_circle_points(p1, p2, p3, speed: float, acc: float) -> Tuple[Position, Position]: - turn_radius, deviation_from_path = compute_turn_radius(p1, p2, p3, speed, acc) + turn_radius, deviation_from_path = compute_turn_radius(p1, p2, p3, speed, acc=acc) distance_on_segment = sqrt((deviation_from_path + turn_radius) ** 2 - turn_radius ** 2) p4 = point_on_segment(p2, p1, distance_on_segment) p5 = point_on_segment(p2, p3, distance_on_segment) @@ -45,7 +43,7 @@ def speed_in_corner(radius: float, acc: float) -> float: return speed -def compute_turn_radius(p1, p2, p3, speed:float , max_deviation:float=100, acc:float=MAX_LINEAR_ACCELERATION) -> Tuple[float, float]: +def compute_turn_radius(p1, p2, p3, speed: float, max_deviation: float=50, acc: float=MAX_LINEAR_ACCELERATION) -> Tuple[float, float]: """Assume the raw path is p1->p2->p3. Deviation is compute from p2 to the circle with a line passing by the center of the circle.""" @@ -53,17 +51,27 @@ def compute_turn_radius(p1, p2, p3, speed:float , max_deviation:float=100, acc:f path_angle = wrap_to_pi((p3 - p2).angle - (p1 - p2).angle) const_speed_deviation = deviation(radius_at_const_speed, path_angle) if const_speed_deviation < max_deviation: - deviation_from_path = const_speed_deviation + speed_deviation = const_speed_deviation turn_radius = radius_at_const_speed else: - deviation_from_path = max_deviation - turn_radius = deviation_from_path / (1 / sin(path_angle / 2) - 1) - - return abs(turn_radius), deviation_from_path + speed *= 0.95 + radius = speed ** 2 / acc + speed_deviation = deviation(radius, path_angle) + turn_radius = speed_deviation / (1 / sin(path_angle / 2) - 1) + while speed_deviation > max_deviation: + speed *= 0.9 + radius = speed ** 2 / acc + speed_deviation = deviation(radius, path_angle) + turn_radius = speed_deviation / (1 / sin(path_angle / 2) - 1) + if abs(speed) < 0.01: + speed_deviation = 0 + turn_radius = 0 + break + return abs(turn_radius), speed_deviation def deviation(radius: float, theta: float) -> float: - return abs(radius / sin(theta / 2) - radius) if sin(theta/2) != 0 else 0 + return abs(radius / sin(theta / 2)) - radius if sin(theta/2) != 0 else 0 def point_on_segment(start: Position, end: Position, distance: float): diff --git a/ai/STA/Tactic/go_kick.py b/ai/STA/Tactic/go_kick.py index 2e46a1ec..9ce37e81 100644 --- a/ai/STA/Tactic/go_kick.py +++ b/ai/STA/Tactic/go_kick.py @@ -100,24 +100,26 @@ def grab_ball(self): if self._get_distance_from_ball() < KICK_DISTANCE: self.next_state = self.kick self.kick_last_time = time.time() - + ball_speed = self.game_state.ball.velocity.norm orientation = (self.target.position - self.game_state.ball_position).angle - distance_behind = self.get_destination_behind_ball(GRAB_BALL_SPACING) + distance_behind = self.get_destination_behind_ball(GRAB_BALL_SPACING * (1 + ball_speed / 1000)) return CmdBuilder().addMoveTo(Pose(distance_behind, orientation), cruise_speed=3, ball_collision=False)\ .addForceDribbler()\ .addChargeKicker()\ - .addKick(self.kick_force)\ .build() def kick(self): if self.auto_update_target: self._find_best_passing_option() + if not self.is_able_to_grab_ball_directly(0.8): + self.next_state = self.go_behind_ball + return self.go_behind_ball() self.next_state = self.validate_kick player_to_target = (self.target.position - self.player.pose.position) - behind_ball = self.game_state.ball_position - normalize(player_to_target) * (BALL_RADIUS + ROBOT_CENTER_TO_KICKER) + behind_ball = self.game_state.ball_position + normalize(player_to_target) * (ROBOT_CENTER_TO_KICKER) orientation = (self.target.position - self.game_state.ball_position).angle return CmdBuilder().addMoveTo(Pose(behind_ball, orientation), @@ -187,5 +189,4 @@ def is_able_to_grab_ball_directly(self, threshold): vec_target_to_ball = normalize(self.game_state.ball.position - self.target.position) alignement_behind = np.dot(vec_target_to_ball.array, (normalize(self.player.position - self.game_state.ball_position)).array) - return threshold < alignement_behind diff --git a/tests/STA/perfect_sim.py b/tests/STA/perfect_sim.py index 600ba522..5678779d 100644 --- a/tests/STA/perfect_sim.py +++ b/tests/STA/perfect_sim.py @@ -71,7 +71,7 @@ def _apply_cmd(self, ia_cmd): self.has_charge_kick = True def _robot_can_hit_ball(self, robot): - KICK_DISTANCE_MIN = ROBOT_CENTER_TO_KICKER + BALL_RADIUS * 0.5 + KICK_DISTANCE_MIN = ROBOT_CENTER_TO_KICKER - BALL_RADIUS * 0.5 KICK_DISTANCE_MAX = ROBOT_CENTER_TO_KICKER + BALL_RADIUS * 1.5 MAX_ANGLE_FOR_KICK = 15