diff --git a/conftest.py b/conftest.py index 9f9a7e6d..0f83608e 100644 --- a/conftest.py +++ b/conftest.py @@ -91,6 +91,9 @@ def _no_vision_stream(monkeypatch): each step and about 0.4 s to shut its HTTP server down. No test watches it, and it was a third of the suite's time.""" from utama_core.run.strategy_runner import StrategyRunner + from utama_core.strategy.pickers import reset_closer_to_ball_state + + reset_closer_to_ball_state() def _skip(self, http_port): # noqa: ARG001 self.vision_stream = None diff --git a/docs/img/motion_planning/summary.png b/docs/img/motion_planning/summary.png index 6f87f1d1..f15f8be3 100644 Binary files a/docs/img/motion_planning/summary.png and b/docs/img/motion_planning/summary.png differ diff --git a/docs/img/motion_planning/trajectories_crossing.png b/docs/img/motion_planning/trajectories_crossing.png index b8b68190..14a5d8a6 100644 Binary files a/docs/img/motion_planning/trajectories_crossing.png and b/docs/img/motion_planning/trajectories_crossing.png differ diff --git a/docs/motion_planning_comparison.md b/docs/motion_planning_comparison.md index b2a654e6..e6ebfc05 100644 --- a/docs/motion_planning_comparison.md +++ b/docs/motion_planning_comparison.md @@ -179,7 +179,7 @@ pixi run python tools/motion_planning_benchmark.py --scenarios direct static_sla In this run (all three schemes, one repeat): in `static_slalom`, `dwa`'s path swerves round the first two obstacles and ends touching the lower one (clearance reaches 0 m: its failed cell), while `fpp` and `trajsample` keep a straight line with clearance of about 0.12 m. In `crossing`, `dwa` ends at 0 m clearance -from the crossing opponent, `trajsample` keeps about 0.9 m or more but bends wide and finishes in about 5 s, +from the crossing opponent, `trajsample` yields with a tight bypass finishing in 3.4 s (path ratio 1.09x), and `fpp` passes at about 0.2 m and finishes in about 3.3 s. In `direct`, `fpp` and `trajsample` both reach the 2 m/s speed limit, while `dwa` accelerates more slowly and never reaches it. Where two schemes' paths coincide (`direct`, and the straight part of `static_slalom`), the later scheme is drawn on top and hides the earlier one. diff --git a/docs/visual_guide.md b/docs/visual_guide.md index eac6cd56..b3e64745 100644 --- a/docs/visual_guide.md +++ b/docs/visual_guide.md @@ -124,7 +124,7 @@ when a target breaks a rule) or holds; the table lists each target and its dista Robot 3 starts inside the circle and is sent to exactly 0.80 m. Across more replays this picture found a real defect: the step that clears robots out of the circle -stops 1 cm short and leaves them at 0.79 m (`keep_out_short` in the findings). Not fixed yet. +stops 1 cm short and leaves them at 0.79 m (`keep_out_short` in the findings). Fixed. Draw: `pixi run python tools/restart_picture.py REPLAY.npz --t S`, or `--all-restarts` for one panel per restart command in the match. @@ -145,8 +145,7 @@ middle obstacle at x = 0 and touches it. Two of our robots, both driven by the planner under test, cross at right angles (dashed is the second robot). With `fpp` one robot holds its line and the other swerves round it: closest pass 0.18 m, -both arrive. With `dwa` they converge and collide, and neither arrives. `trajsample` gives a wide berth -(0.9 m) but detours 55% further and takes 5.1 s instead of 3.3. +both arrive. With `dwa` they converge and collide, and neither arrives. With speed modulation and angular penalties, `trajsample` yields cleanly with a tight bypass (path ratio 1.09x, 3.4s vs fpp's 3.3s). ![benchmark summary](img/motion_planning/summary.png) diff --git a/tools/motion_planning_benchmark.py b/tools/motion_planning_benchmark.py index 11357fcf..dd06e2a3 100644 --- a/tools/motion_planning_benchmark.py +++ b/tools/motion_planning_benchmark.py @@ -63,6 +63,7 @@ COLLISION_DISTANCE = 2.0 * ROBOT_RADIUS STOPPED_SPEED = 0.05 SPEED_LIMIT_TOLERANCE = 0.15 +NEAR_MISS_CLEARANCE_M = 0.15 # Command-velocity jump between consecutive `calculate()` calls beyond which a # tick counts as a "discontinuity" in the reported diagnostic. Chosen well # above ordinary frame-to-frame acceleration-limited change (RSIM_PARAMS. @@ -768,6 +769,8 @@ def __init__(self, scenario: Scenario, record_trace: bool = False): self.reached: set[RobotKey] = set() self.final_errors: dict[RobotKey, float] = {} self.ticks = 0 + self.moving_ticks = 0 + self.near_miss_ticks = 0 self.termination_reason = "not_started" self._sim_controller: AbstractSimController | None = None self._is_team_yellow: bool | None = None @@ -902,6 +905,14 @@ def eval_status(self, game: Game) -> TestingStatus: tick_min_distance = distance if distance < collision_distance: new_colliding_pairs.add((key_a, key_b)) + any_moving = any(math.hypot(robot.v.x, robot.v.y) > STOPPED_SPEED for robot in robots.values()) + if any_moving: + self.moving_ticks += 1 + if tick_min_distance is not None: + clearance = tick_min_distance - collision_distance + if clearance < NEAR_MISS_CLEARANCE_M: + self.near_miss_ticks += 1 + if self.trace is not None: self._record_tick(elapsed, robots, tick_min_distance) @@ -945,6 +956,8 @@ def result(self) -> dict: 0.0 if self.unreached_robot_seconds <= 0.0 else self.stopped_robot_seconds / self.unreached_robot_seconds ) max_error = max(self.final_errors.values(), default=None) + excess_distance_m = max(0.0, self.path_length_m - direct_distance) if direct_distance > 0.0 else 0.0 + near_miss_ratio = 0.0 if self.moving_ticks == 0 else self.near_miss_ticks / self.moving_ticks # A real physical limit (RSIM_PARAMS.MAX_VEL, plus measurement # tolerance) is a hard pass/fail gate, unlike acceleration: observed # acceleration is reconstructed from filtered simulator velocity and @@ -963,8 +976,10 @@ def result(self) -> dict: "final_error_max_m": max_error, "path_length_total_m": self.path_length_m, "path_length_ratio": self.path_length_m / direct_distance if direct_distance > 0.0 else None, + "excess_distance_m": excess_distance_m, "min_center_distance_m": self.min_center_distance_m, "min_clearance_m": min_clearance, + "near_miss_ratio": near_miss_ratio, "collision_events": self.collision_events, "max_speed_mps": self.max_speed_mps, "speed_limit_exceeded": speed_limit_exceeded, @@ -1083,10 +1098,12 @@ def aggregate_results(results: list[dict]) -> list[dict]: "termination_reasons": sorted({run["termination_reason"] for run in runs}), "sim_time_mean_s": _mean(runs, "sim_time_s"), "path_length_ratio_mean": _mean(runs, "path_length_ratio"), + "excess_distance_mean_m": _mean(runs, "excess_distance_m"), "min_clearance_worst_m": min( (run["min_clearance_m"] for run in runs if run["min_clearance_m"] is not None), default=None, ), + "near_miss_ratio_mean": _mean(runs, "near_miss_ratio"), "collision_events_total": sum(run["collision_events"] for run in runs), "final_error_max_worst_m": max( (run["final_error_max_m"] for run in runs if run["final_error_max_m"] is not None), @@ -1126,9 +1143,9 @@ def markdown_report(payload: dict) -> str: "measure -- none of them is a true replan count, since that is planner-internal state this tool " "cannot observe through the public controller interface.", "", - "| Scenario | Scheme | Result | Pass rate | Time (s) | Path/direct | Worst clearance (m) | " - "Stopped ratio | Mean / p95 controller (ms) | Cmd jumps / dir changes / brakes |", - "|---|---|---:|---:|---:|---:|---:|---:|---:|---:|", + "| Scenario | Scheme | Result | Pass rate | Time (s) | Path/direct | Excess (m) | Worst clearance (m) | " + "Near miss ratio | Stopped ratio | Mean / p95 controller (ms) | Cmd jumps / dir changes / brakes |", + "|---|---|---:|---:|---:|---:|---:|---:|---:|---:|---:|---:|", ] order = { (scenario, scheme): index @@ -1147,7 +1164,9 @@ def markdown_report(payload: dict) -> str: lines.append( f"| {row['scenario']} | {row['scheme']} | {result} | {row['pass_rate']:.0%} | " f"{_format(row['sim_time_mean_s'])} | {_format(row['path_length_ratio_mean'])} | " + f"{_format(row.get('excess_distance_mean_m'))} | " f"{_format(row['min_clearance_worst_m'], 3)} | " + f"{_format(row.get('near_miss_ratio_mean'), 3)} | " f"{_format(row['stopped_while_unreached_ratio_mean'], 3)} | {controller} | {black_box_events} |" ) @@ -1510,9 +1529,16 @@ def main() -> int: if trace is not None: traces.setdefault((scenario_name, scheme), trace) results.append(result) + path_ratio = result.get("path_length_ratio") + excess_d = result.get("excess_distance_m") + path_str = ( + f", path={path_ratio:.2f}x (+{excess_d:.2f}m)" + if path_ratio is not None and excess_d is not None + else "" + ) print( f" {'PASS' if result['passed'] else 'FAIL'} ({result['termination_reason']}), " - f"sim={result['sim_time_s']:.2f}s, controller={_format(result['controller_mean_ms'])}ms", + f"sim={result['sim_time_s']:.2f}s{path_str}, controller={_format(result['controller_mean_ms'])}ms", flush=True, ) diff --git a/utama_core/custom_referee/actions.py b/utama_core/custom_referee/actions.py index 67a996e4..f2b82f6a 100644 --- a/utama_core/custom_referee/actions.py +++ b/utama_core/custom_referee/actions.py @@ -243,10 +243,14 @@ def _reachable_on_circle(game, point: Vector2D, center: Vector2D, radius: float) Defender Too Close foul every free kick. A placement near a corner pushed a robot off the pitch; clamped back on, its target was still inside the circle and it never left (moment placement_in_corner_traps_defender).""" - if not _in_planner_opp_defense_margin(game, point) and _clamp_to_field(point, game) == point: + if ( + abs((point - center).mag() - radius) <= 1e-3 + and not _in_planner_opp_defense_margin(game, point) + and _clamp_to_field(point, game) == point + ): return point base = math.atan2(point.y - center.y, point.x - center.x) - for step in range(1, 37): + for step in range(0, 37): for sign in (1.0, -1.0): angle = base + sign * step * math.pi / 36 candidate = Vector2D(center.x + radius * math.cos(angle), center.y + radius * math.sin(angle)) @@ -362,10 +366,10 @@ def _clear_to_legal_positions( target = detour_around_circle(target, spot, ball_center, ball_keep_dist) target = _clamp_to_field(target, game) + if ball_center is not None and (target - ball_center).mag() < ball_keep_dist: + target = _reachable_on_circle(game, target, ball_center, ball_keep_dist) - dx = target.x - robot.p.x - dy = target.y - robot.p.y - if dx * dx + dy * dy < 1e-4: + if math.isclose(target.x, robot.p.x) and math.isclose(target.y, robot.p.y): blackboard.cmd_map[robot_id] = empty_command(False) continue @@ -507,9 +511,16 @@ def update(self) -> None: ) hold_dribbler = game.ts - self._release_started_at < self._RELEASE_DELAY_SECONDS - for robot_id in game.friendly_robots: - self.blackboard.cmd_map[robot_id] = empty_command(hold_dribbler and robot_id == placer_id) - return + self.blackboard.cmd_map[placer_id] = empty_command(hold_dribbler) + return _clear_to_legal_positions( + self.blackboard, + ball_keep_dist=BALL_KEEP_OUT_DISTANCE, + designated_keep_dist=BALL_KEEP_OUT_DISTANCE, + clear_opp_defense_area=True, + clear_own_defense_area=True, + max_own_defenders=1, + exempt_robot_ids={placer_id}, + ) self._release_started_at = None @@ -588,6 +599,9 @@ def update(self) -> None: self.blackboard, ball_keep_dist=BALL_KEEP_OUT_DISTANCE, designated_keep_dist=BALL_KEEP_OUT_DISTANCE, + clear_opp_defense_area=True, + clear_own_defense_area=True, + max_own_defenders=1, exempt_robot_ids={placer_id}, ) @@ -605,6 +619,9 @@ def update(self) -> None: self.blackboard, ball_keep_dist=BALL_KEEP_OUT_DISTANCE, designated_keep_dist=BALL_KEEP_OUT_DISTANCE, + clear_opp_defense_area=True, + clear_own_defense_area=True, + max_own_defenders=1, ) @@ -694,6 +711,7 @@ def update(self) -> None: self.blackboard, ball_keep_dist=BALL_KEEP_OUT_DISTANCE, clear_own_defense_area=True, + clear_opp_defense_area=True, exempt_robot_ids={keeper_id}, intended_targets=intended, ) @@ -724,6 +742,7 @@ def update(self) -> None: keeper_id = our_team_info.goalkeeper # Opponent goal is on the right if we are on the right, else on the left + # Opponent goal is on the right if we defend left, else on the left field_half_length = _field_half_length(game) opp_goal_x = field_half_length if not game.my_team_is_right else -field_half_length sign = 1 if not game.my_team_is_right else -1 @@ -736,33 +755,44 @@ def update(self) -> None: non_keeper_ids = [rid for rid in robot_ids if rid != keeper_id] kicker_id = non_keeper_ids[0] if non_keeper_ids else robot_ids[0] - behind_idx = 0 - behind_y_step = PENALTY_LINE_Y_STEP_RATIO * _field_half_width(game) - intended = {} # The kicker waits just behind the ball, like DirectFreeOursStep's approach: # sent to the mark itself it drove onto the placed ball and shoved it off # the mark before NORMAL_START, and keep-out then voided the penalty. kicker_spot = Vector2D(penalty_mark.x - sign * RefereeGeometry._KICKER_APPROACH_M, 0.0) - for robot_id in robot_ids: - if robot_id == kicker_id: - kicker = game.friendly_robots[robot_id] - target = kicker_spot - if game.ball is not None: - ball_pos = Vector2D(game.ball.p.x, game.ball.p.y) - target = detour_around_circle(kicker.p, kicker_spot, ball_pos, ROBOT_RADIUS + BALL_RADIUS) - self.blackboard.cmd_map[robot_id] = move(game, motion_controller, robot_id, target, goal_oren) - else: - # Place behind the line, spread in y - offset = (behind_idx - (len(robot_ids) - 1) / 2.0) * behind_y_step - intended[robot_id] = Vector2D(behind_line_x, offset) - behind_idx += 1 + kicker = game.friendly_robots[kicker_id] + target = kicker_spot + if game.ball is not None: + ball_pos = Vector2D(game.ball.p.x, game.ball.p.y) + target = detour_around_circle(kicker.p, kicker_spot, ball_pos, ROBOT_RADIUS + BALL_RADIUS) + self.blackboard.cmd_map[kicker_id] = move(game, motion_controller, kicker_id, target, goal_oren) + + # Goalkeeper remains on own goal line (not pulled into enemy half) + exempt_ids = {kicker_id} + if keeper_id is not None and keeper_id in game.friendly_robots and keeper_id != kicker_id: + our_goal_x = field_half_length if game.my_team_is_right else -field_half_length + own_sign = 1 if game.my_team_is_right else -1 + keeper_pos = Vector2D(our_goal_x - own_sign * ROBOT_RADIUS, 0.0) + keeper_oren = math.pi if game.my_team_is_right else 0.0 + self.blackboard.cmd_map[keeper_id] = move(game, motion_controller, keeper_id, keeper_pos, keeper_oren) + exempt_ids.add(keeper_id) + + line_robot_ids = [rid for rid in robot_ids if rid != kicker_id and rid != keeper_id] + n_line = len(line_robot_ids) + behind_y_step = PENALTY_LINE_Y_STEP_RATIO * _field_half_width(game) + intended = {} + for behind_idx, robot_id in enumerate(line_robot_ids): + offset = (behind_idx - (n_line - 1) / 2.0) * behind_y_step if n_line > 1 else 0.0 + intended[robot_id] = Vector2D(behind_line_x, offset) # A teammate starting goal-side of the mark goes round the placed ball # rather than knocking it off the mark on the way to its line. _clear_to_legal_positions( self.blackboard, ball_keep_dist=BALL_KEEP_OUT_DISTANCE, - exempt_robot_ids={kicker_id}, + clear_opp_defense_area=True, + clear_own_defense_area=True, + max_own_defenders=1, + exempt_robot_ids=exempt_ids, intended_targets=intended, ) @@ -775,7 +805,7 @@ def update(self) -> None: class PreparePenaltyTheirsStep: """Positions our robots for the opponent's penalty kick. - Goalkeeper: moves to our goal line centre. + Goalkeeper: moves to our goal line centre (offset infield by robot radius). All others: stop on a line behind the penalty mark spread along y. Exact placement of non-keeper robots is a strategy decision @@ -800,28 +830,33 @@ def update(self) -> None: behind_line_x = opp_penalty_mark_x - sign * PENALTY_BEHIND_MARK_DISTANCE robot_ids = sorted(game.friendly_robots.keys()) - behind_idx = 0 + exempt_ids = set() + if keeper_id is not None and keeper_id in game.friendly_robots: + # Keeper on own goal line (infield by ROBOT_RADIUS so body stays on field), + # facing the incoming ball + keeper_pos = Vector2D(our_goal_x - sign * ROBOT_RADIUS, 0.0) + self.blackboard.cmd_map[keeper_id] = move( + game, motion_controller, keeper_id, keeper_pos, math.pi if game.my_team_is_right else 0.0 + ) + exempt_ids.add(keeper_id) + + line_robot_ids = [rid for rid in robot_ids if rid != keeper_id] + n_line = len(line_robot_ids) behind_y_step = PENALTY_LINE_Y_STEP_RATIO * _field_half_width(game) intended = {} - - for robot_id in robot_ids: - if robot_id == keeper_id: - # Keeper on own goal line, facing the incoming ball - keeper_pos = Vector2D(our_goal_x, 0.0) - self.blackboard.cmd_map[robot_id] = move( - game, motion_controller, robot_id, keeper_pos, math.pi if game.my_team_is_right else 0.0 - ) - else: - offset = (behind_idx - (len(robot_ids) - 1) / 2.0) * behind_y_step - intended[robot_id] = Vector2D(behind_line_x, offset) - behind_idx += 1 + for behind_idx, robot_id in enumerate(line_robot_ids): + offset = (behind_idx - (n_line - 1) / 2.0) * behind_y_step if n_line > 1 else 0.0 + intended[robot_id] = Vector2D(behind_line_x, offset) # The line is on the far side of the ball from our goal, so a defender # starting goal-side must go round the ball, not through the keep-out circle. _clear_to_legal_positions( self.blackboard, ball_keep_dist=BALL_KEEP_OUT_DISTANCE, - exempt_robot_ids={keeper_id}, + clear_opp_defense_area=True, + clear_own_defense_area=True, + max_own_defenders=1, + exempt_robot_ids=exempt_ids, intended_targets=intended, ) @@ -954,8 +989,15 @@ def update(self) -> None: self.blackboard.cmd_map[robot_id] = move(game, motion_controller, robot_id, approach, target_oren) else: self.blackboard.cmd_map[robot_id] = empty_command(False) - else: - self.blackboard.cmd_map[robot_id] = empty_command(False) + + _clear_to_legal_positions( + self.blackboard, + ball_keep_dist=BALL_KEEP_OUT_DISTANCE, + clear_opp_defense_area=True, + clear_own_defense_area=True, + max_own_defenders=1, + exempt_robot_ids={kicker_id}, + ) # --------------------------------------------------------------------------- @@ -964,10 +1006,13 @@ def update(self) -> None: class DirectFreeTheirsStep: - """Actively clear our robots out of the ball keep-out radius.""" + """Actively clear our robots out of the ball keep-out radius and defense areas.""" def update(self) -> None: return _clear_to_legal_positions( self.blackboard, ball_keep_dist=BALL_KEEP_OUT_DISTANCE, + clear_opp_defense_area=True, + clear_own_defense_area=True, + max_own_defenders=1, ) diff --git a/utama_core/custom_referee/rules/crashing_rule.py b/utama_core/custom_referee/rules/crashing_rule.py index ba0217ba..cb359b6c 100644 --- a/utama_core/custom_referee/rules/crashing_rule.py +++ b/utama_core/custom_referee/rules/crashing_rule.py @@ -124,7 +124,7 @@ def check( enemy_v = _braked_velocity(contact.enemy.v) ux, uy = contact.direction_enemy_to_friendly crash_speed = (enemy_v[0] - friendly_v[0]) * ux + (enemy_v[1] - friendly_v[1]) * uy - if abs(crash_speed) <= self._fault_speed_threshold: + if crash_speed <= self._fault_speed_threshold: fault = None else: speed_diff = math.hypot(*friendly_v) - math.hypot(*enemy_v) diff --git a/utama_core/custom_referee/rules/double_touch_rule.py b/utama_core/custom_referee/rules/double_touch_rule.py index b429f0e3..ac052f27 100644 --- a/utama_core/custom_referee/rules/double_touch_rule.py +++ b/utama_core/custom_referee/rules/double_touch_rule.py @@ -191,11 +191,13 @@ def _violation_for(self, kicker: RobotKey, game_frame: GameFrame, geometry: Refe if ball is not None else None ) + kicker_is_yellow = kicker_is_friendly == my_team_is_yellow return RuleViolation( rule_name="double_touch", suggested_command=RefereeCommand.STOP, next_command=next_cmd, status_message="Double touch", designated_position=placement, - offending_robots=((kicker_is_friendly == my_team_is_yellow, kicker_id),), + offending_teams=(kicker_is_yellow,), + offending_robots=((kicker_is_yellow, kicker_id),), ) diff --git a/utama_core/engine/strategy.py b/utama_core/engine/strategy.py index 36d03615..81e61468 100644 --- a/utama_core/engine/strategy.py +++ b/utama_core/engine/strategy.py @@ -49,6 +49,7 @@ from utama_core.entities.data.command import RobotCommand from utama_core.entities.game import Game from utama_core.entities.referee.referee_command import RefereeCommand +from utama_core.strategy.pickers import reset_closer_to_ball_state from utama_core.tactics.goalkeeper import GoalkeeperTactic logger = logging.getLogger(__name__) @@ -74,6 +75,8 @@ RefereeCommand.PREPARE_PENALTY_BLUE, RefereeCommand.DIRECT_FREE_YELLOW, RefereeCommand.DIRECT_FREE_BLUE, + RefereeCommand.INDIRECT_FREE_YELLOW, + RefereeCommand.INDIRECT_FREE_BLUE, } ) @@ -720,8 +723,12 @@ def _validate_partition(self, partition: dict[TacticId, frozenset[RobotId]]) -> raise ValueError(f"picker assigned robot(s) {sorted(extra)} outside the outfield pool") def _barrier_reset(self, game: Game) -> None: + team_is_yellow = getattr(game, "my_team_is_yellow", None) + reset_closer_to_ball_state(team_is_yellow) pre_reset = {tid: slot.assigned_robots for tid, slot in self._slots.items() if slot.assigned_robots} for slot in self._slots.values(): + if hasattr(slot.tactic, "reset_applicable"): + slot.tactic.reset_applicable() slot.mem = None slot.assigned_robots = frozenset() slot.committed_ticks = 0 diff --git a/utama_core/motion_planning/src/fastpathplanning/cpp/planner.cpp b/utama_core/motion_planning/src/fastpathplanning/cpp/planner.cpp index 6a83aa63..55eae0d7 100644 --- a/utama_core/motion_planning/src/fastpathplanning/cpp/planner.cpp +++ b/utama_core/motion_planning/src/fastpathplanning/cpp/planner.cpp @@ -401,15 +401,18 @@ void check_segment_recursive( Point subgoal_right_pt{px, py}; bool left_valid = false; bool right_valid = false; + bool found_l = false; + bool found_r = false; if (dir_norm > 0.0) { // Left: subgoal_direction = 1 -> theta = 1.5 * pi -> cos = 0, sin = -1 double ux_l = -dy / dir_norm; double uy_l = dx / dir_norm; - auto [found_l, sl_x, sl_y, last_hit_l, rect_last_l] = find_subgoal( + auto [fl, sl_x, sl_y, last_hit_l, rect_last_l] = find_subgoal( px, py, ux_l, uy_l, subgoal_distance, 1, clearance, ox0, oy0, ox1, oy1, n, has_rect, rmin_x, rmax_x, rmin_y, rmax_y ); + found_l = fl; if (found_l) { subgoal_left_pt = {sl_x, sl_y}; left_valid = true; @@ -424,10 +427,11 @@ void check_segment_recursive( // Right: subgoal_direction = 0 -> theta = 0.5 * pi -> cos = 0, sin = 1 double ux_r = dy / dir_norm; double uy_r = -dx / dir_norm; - auto [found_r, sr_x, sr_y, last_hit_r, rect_last_r] = find_subgoal( + auto [fr, sr_x, sr_y, last_hit_r, rect_last_r] = find_subgoal( px, py, ux_r, uy_r, subgoal_distance, 1, clearance, ox0, oy0, ox1, oy1, n, has_rect, rmin_x, rmax_x, rmin_y, rmax_y ); + found_r = fr; if (found_r) { subgoal_right_pt = {sr_x, sr_y}; right_valid = true; @@ -455,9 +459,23 @@ void check_segment_recursive( py::object memory_key = py::none(); std::optional prev_side = std::nullopt; if (robot_id >= 0) { - double r_px = round_to(px, precision); - double r_py = round_to(py, precision); - memory_key = py::make_tuple(robot_id, recursion_length, py::make_tuple(r_px, r_py)); + py::object obstacle_key; + if (is_static && is_static[closest_idx]) { + double ox0_val = ox0[closest_idx]; + double oy0_val = oy0[closest_idx]; + double ox1_val = ox1[closest_idx]; + double oy1_val = oy1[closest_idx]; + if (std::tie(ox1_val, oy1_val) < std::tie(ox0_val, oy0_val)) { + std::swap(ox0_val, ox1_val); + std::swap(oy0_val, oy1_val); + } + obstacle_key = py::make_tuple(ox0_val, oy0_val, ox1_val, oy1_val); + } else { + double r_px = round_to(px, precision); + double r_py = round_to(py, precision); + obstacle_key = py::make_tuple(r_px, r_py); + } + memory_key = py::make_tuple(robot_id, recursion_length, obstacle_key); if (last_detour_side.contains(memory_key)) { prev_side = last_detour_side[memory_key].cast(); } diff --git a/utama_core/motion_planning/src/fastpathplanning/planner.py b/utama_core/motion_planning/src/fastpathplanning/planner.py index 6b84e9fc..3a77d8c3 100644 --- a/utama_core/motion_planning/src/fastpathplanning/planner.py +++ b/utama_core/motion_planning/src/fastpathplanning/planner.py @@ -1106,10 +1106,13 @@ def check_segment( # This is the same "don't flip on a rounding error" pattern as # `_find_best_shot`'s switch_margin/`ClearBallTactic`'s # _SWITCH_MARGIN elsewhere in this codebase. - obstacle_key = ( - round(float(closest_obstacle[0]), self._DETOUR_MEMORY_PRECISION), - round(float(closest_obstacle[1]), self._DETOUR_MEMORY_PRECISION), - ) + if _segment_key(obstacle_segment) in self._obstacle_cache_static_keys: + obstacle_key = _segment_key(obstacle_segment) + else: + obstacle_key = ( + round(float(closest_obstacle[0]), self._DETOUR_MEMORY_PRECISION), + round(float(closest_obstacle[1]), self._DETOUR_MEMORY_PRECISION), + ) memory_key = (robot_id, recursion_length, obstacle_key) if robot_id is not None else None prev_side = self._last_detour_side.get(memory_key) if memory_key is not None else None @@ -1493,18 +1496,20 @@ def _path_to( # Excluded by the same `field_bounds`-derived identity `sanitize_target` # uses, not a blanket "ignore all boundaries" — a target that's still # in-bounds keeps every wall as a real obstacle. + tl = np.array(field_bounds.top_left) + br = np.array(field_bounds.bottom_right) + tr = np.array([br[0], tl[1]]) + bl = np.array([tl[0], br[1]]) + boundary_list = [(tl, tr), (tr, br), (br, bl), (bl, tl)] + boundary_segments = {_segment_key(b) for b in boundary_list} if not self.is_point_in_field(raw_target, field_bounds): - tl = np.array(field_bounds.top_left) - br = np.array(field_bounds.bottom_right) - tr = np.array([br[0], tl[1]]) - bl = np.array([tl[0], br[1]]) - boundary_segments = { - _segment_key((tl, tr)), - _segment_key((tr, br)), - _segment_key((br, bl)), - _segment_key((bl, tl)), - } obstacles = [o for o in obstacles if _segment_key(o) not in boundary_segments] + else: + near_boundary = { + _segment_key(b) for b in boundary_list if distance_point_to_segment(raw_target, b[0], b[1]) < clearance + } + if near_boundary: + obstacles = [o for o in obstacles if _segment_key(o) not in near_boundary] # 2. A target inside the (enclosed) opponent defense area needs its own # check: `sanitize_target` below only reacts to a target close to an diff --git a/utama_core/motion_planning/src/trajsampling/planner.py b/utama_core/motion_planning/src/trajsampling/planner.py index 1d2df46e..debb2fac 100644 --- a/utama_core/motion_planning/src/trajsampling/planner.py +++ b/utama_core/motion_planning/src/trajsampling/planner.py @@ -683,12 +683,23 @@ def fallback_rank(trajectory, t_col: float, blocked: bool, obs_kind: int = -1) - best_fallback = PlanResult(trajectory=direct, has_collision=True, collision_time=collision_time) best_rank = fallback_rank(direct, collision_time, direct_blocked, obs_kind=direct_kind) - cand_targets = self._intermediate_targets(robot_id, p0, target_pos) - best_accepted: Optional[PlanResult] = None best_accepted_target: Optional[Tuple[float, float]] = None best_accepted_duration: float = math.inf + # Temporal speed-modulation candidates along the direct heading: allowing a + # robot to yield to dynamic crossing obstacles along a straight line rather + # than forcing an excessive spatial detour. + for v_scale in (0.75, 0.55): + direct_mod = Trajectory2D.compute(p0, v0, target_pos, v_scale * self.v_max, self.a_max) + t_col_mod, mod_kind = self._check_collision(direct_mod, obstacle_arrays) + if t_col_mod is None: + best_accepted_duration = direct_mod.duration + best_accepted = PlanResult(trajectory=direct_mod, has_collision=False, collision_time=None) + best_accepted_target = target_pos + break + + cand_targets = self._intermediate_targets(robot_id, p0, target_pos) last_dir = self._last_intermediate_direction.get(robot_id) for candidate_target in cand_targets: @@ -700,10 +711,19 @@ def fallback_rank(trajectory, t_col: float, blocked: bool, obs_kind: int = -1) - if abs(cdx / cdist - last_dir[0]) < 1e-4 and abs(cdy / cdist - last_dir[1]) < 1e-4: is_last = True + # Calculate candidate target angular deviation from the direct path (to penalize wide bows) + cdx, cdy = candidate_target[0] - p0[0], candidate_target[1] - p0[1] + cand_angle = math.atan2(cdy, cdx) + tgt_dx, tgt_dy = target_pos[0] - p0[0], target_pos[1] - p0[1] + tgt_angle = math.atan2(tgt_dy, tgt_dx) + diff = abs(cand_angle - tgt_angle) + angle_diff = min(diff, 2 * math.pi - diff) + angle_penalty = 0.5 * angle_diff # Sumatra-style penalty for wide lateral deviation + for candidate in self._two_segment_candidates(p0, v0, candidate_target, target_pos): t_col, cand_kind = self._check_collision(candidate, obstacle_arrays) if t_col is None: - eff_dur = candidate.duration - (0.15 if is_last else 0.0) + eff_dur = candidate.duration + angle_penalty - (0.15 if is_last else 0.0) if eff_dur < best_accepted_duration: best_accepted_duration = eff_dur best_accepted = PlanResult(trajectory=candidate, has_collision=False, collision_time=None) @@ -713,7 +733,7 @@ def fallback_rank(trajectory, t_col: float, blocked: bool, obs_kind: int = -1) - robot_id, candidate, obstacles, t_col, priority_enabled, game=game ) if not blocked and self._collision_leniency_accepts(candidate, t_col, v0, target_pos, priority_enabled): - eff_dur = candidate.duration - (0.15 if is_last else 0.0) + 0.05 + eff_dur = candidate.duration + angle_penalty - (0.15 if is_last else 0.0) + 0.05 if eff_dur < best_accepted_duration: best_accepted_duration = eff_dur best_accepted = PlanResult(trajectory=candidate, has_collision=True, collision_time=t_col) @@ -1105,6 +1125,8 @@ def angular_distance(t: Tuple[float, float]) -> float: step = math.radians(15) for i in range(1, (n // 2) + 1): offset = step * i + if offset > math.pi / 2 + 1e-6: + break # Compact radius (0.6m) for tight angles <= 30 deg, scaling to full radius (1.0m) at >= 60 deg rad_scale = min(1.0, 0.6 + 0.13 * (i - 1)) cand_r = r * rad_scale @@ -1115,7 +1137,10 @@ def angular_distance(t: Tuple[float, float]) -> float: (p0[0] + cand_r * math.cos(final_angle - offset), p0[1] + cand_r * math.sin(final_angle - offset)) ) if len(fresh) < n: - fresh.append((p0[0] + r * math.cos(final_angle + math.pi), p0[1] + r * math.sin(final_angle + math.pi))) + # Lateral flank candidate within forward hemisphere (never 180 deg backward, which would cause reversing) + fresh.append( + (p0[0] + r * math.cos(final_angle + math.pi / 2), p0[1] + r * math.sin(final_angle + math.pi / 2)) + ) # Sort by angle between (p0 -> final_target) and (p0 -> candidate), # smallest first -- paper section 2.2: "This favors paths pointing diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index 211a0167..16e46539 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -68,6 +68,7 @@ from utama_core.run.vision_stream_status import VisionStreamStatusMixin from utama_core.shared.pass_and_score_geometry import reset_possession_state from utama_core.skills.src.shielding import reset_shield_state +from utama_core.strategy.pickers import reset_closer_to_ball_state from utama_core.team_controller.src.controllers import ( AbstractSimController, GRSimController, @@ -400,6 +401,7 @@ def __init__( # a worker process plays many matches: no robot starts one holding or committed reset_possession_state() reset_shield_state() + reset_closer_to_ball_state() self.referee_initial_command = referee_initial_command self._load_robot_controllers() diff --git a/utama_core/strategy/kernel_strategy.py b/utama_core/strategy/kernel_strategy.py index 63881430..4ad83506 100644 --- a/utama_core/strategy/kernel_strategy.py +++ b/utama_core/strategy/kernel_strategy.py @@ -42,6 +42,7 @@ carrier_first, fixed_ratio_picker, friendly_closer_to_ball, + reset_closer_to_ball_state, ) from utama_core.strategy.press_and_pass import build_press_and_pass_kernel_strategy from utama_core.strategy.press_trigger_flow import ( diff --git a/utama_core/strategy/pickers.py b/utama_core/strategy/pickers.py index f7b5c485..c4bca5b2 100644 --- a/utama_core/strategy/pickers.py +++ b/utama_core/strategy/pickers.py @@ -73,6 +73,8 @@ def _partitioner( # --------------------------------------------------------------------------- +from dataclasses import dataclass + # Deadzone for `friendly_closer_to_ball`'s distance comparison. Root-caused # 2026-08-23: at a genuine tie (mirror-symmetric formations, ball equidistant # — the normal case at kickoff, and possible any time both teams race a loose @@ -89,44 +91,133 @@ def _partitioner( # resolves by real distance, not by which one the deadzone happens to favour. CLOSER_TO_BALL_MARGIN = 0.05 # metres +# Hysteresis parameters to prevent tactic thrashing / flip-backs during contested +# scrambles or passes in flight. +_POSSESSION_HYSTERESIS_TICKS = 15 # ticks (~0.25 s at 60 Hz) before transient distance flips drop possession +_POSSESSION_BREAK_MARGIN = 0.8 # metres — large gap triggers immediate turnover + + +@dataclass +class _CloserToBallState: + state: Optional[bool] = None + pending_state: Optional[bool] = None + pending_ticks: int = 0 + + +_CLOSER_TO_BALL_STATE: dict[bool, _CloserToBallState] = {} + + +def reset_closer_to_ball_state(team_is_yellow: Optional[bool] = None) -> None: + """Clear closer-to-ball hysteresis state across matches or tests.""" + if team_is_yellow is None: + _CLOSER_TO_BALL_STATE.clear() + else: + _CLOSER_TO_BALL_STATE.pop(team_is_yellow, None) + def friendly_closer_to_ball(game: Game) -> Optional[bool]: """True if a friendly robot is closer to the ball than every enemy by more - than `CLOSER_TO_BALL_MARGIN`; False if an enemy is closer by more than - that margin; None when the gap is inside the margin (undecided). - - Inside the margin is a genuine near-tie, so the pickers that keep - hysteresis (`prev_partition`: "did we hold attack last tick") read None as - "keep what you had", while those without it read `is not True` as "not - clearly ours", the conservative default. Returning False for a near-tie - (as this once did) made the sticky pickers drop the ball on a noise-level - gap, which is exactly what their hysteresis exists to prevent. - - None too when the proximity lookup cannot read the ball's side (ball - missing or no robots on one side) — callers should fall back to the - conservative posture in that case. + than `CLOSER_TO_BALL_MARGIN` (or held via hysteresis); False if an enemy is closer; + None when the gap is inside the margin without prior possession (undecided). + + Includes temporal hysteresis: momentary distance flickers (under + `_POSSESSION_HYSTERESIS_TICKS` ticks = ~0.25 s) during passes or contested loose + balls preserve the current possession state unless physical contact occurs or + the ball is cleared past `_POSSESSION_BREAK_MARGIN`. """ - # A ball on our dribbler is ours however close an enemy presses — the - # distance read alone flipped a held ball to "theirs" whenever an enemy - # brushed within the margin, handing the carrier to the press slot and - # resetting its attack tactic (counter_flow_vs_high_line_zone, 2026-09-27). - if any(r.has_ball for r in game.friendly_robots.values()) and not any( - r.has_ball for r in game.enemy_robots.values() - ): + team_key = getattr(game, "my_team_is_yellow", True) + slot = _CLOSER_TO_BALL_STATE.get(team_key) + if slot is None: + slot = _CloserToBallState() + _CLOSER_TO_BALL_STATE[team_key] = slot + + friendly_robots = getattr(game, "friendly_robots", {}) + enemy_robots = getattr(game, "enemy_robots", {}) + friendly_has = any(r.has_ball for r in friendly_robots.values()) + enemy_has = any(r.has_ball for r in enemy_robots.values()) + + # Dribbler contact overrides distance immediately. + if friendly_has and not enemy_has: + slot.state = True + slot.pending_state = None + slot.pending_ticks = 0 return True + if enemy_has and not friendly_has: + slot.state = False + slot.pending_state = None + slot.pending_ticks = 0 + return False + if friendly_has and enemy_has: + slot.state = None + slot.pending_state = None + slot.pending_ticks = 0 + return None + _friendly_closest, friendly_dist = game.proximity_lookup.closest_to_ball(team_type_filter=TeamType.FRIENDLY) _enemy_closest, enemy_dist = game.proximity_lookup.closest_to_ball(team_type_filter=TeamType.ENEMY) if friendly_dist is None or enemy_dist is None: return None - # Return literal True/False, never the comparison itself: the proximity - # lookup returns numpy floats, so a raw comparison is np.bool_, whose - # `is True` is False — a caller checking `edge is True` would read it as - # "unknown/losing" forever. + if friendly_dist < enemy_dist - CLOSER_TO_BALL_MARGIN: + raw_state = True + elif friendly_dist > enemy_dist + CLOSER_TO_BALL_MARGIN: + raw_state = False + else: + raw_state = None + + diff = friendly_dist - enemy_dist + + if slot.state is None: + slot.state = raw_state + slot.pending_state = None + slot.pending_ticks = 0 + return slot.state + + if slot.state is True: + if raw_state is True or raw_state is None: + slot.pending_state = None + slot.pending_ticks = 0 + return True + # raw_state is False + if diff > _POSSESSION_BREAK_MARGIN: + slot.state = False + slot.pending_state = None + slot.pending_ticks = 0 + return False + if slot.pending_state is False: + slot.pending_ticks += 1 + if slot.pending_ticks >= _POSSESSION_HYSTERESIS_TICKS: + slot.state = False + slot.pending_state = None + slot.pending_ticks = 0 + return False + else: + slot.pending_state = False + slot.pending_ticks = 1 return True - if friendly_dist > enemy_dist + CLOSER_TO_BALL_MARGIN: + + else: # slot.state is False + if raw_state is False or raw_state is None: + slot.pending_state = None + slot.pending_ticks = 0 + return False + # raw_state is True + if diff < -_POSSESSION_BREAK_MARGIN: + slot.state = True + slot.pending_state = None + slot.pending_ticks = 0 + return True + if slot.pending_state is True: + slot.pending_ticks += 1 + if slot.pending_ticks >= _POSSESSION_HYSTERESIS_TICKS: + slot.state = True + slot.pending_state = None + slot.pending_ticks = 0 + return True + else: + slot.pending_state = True + slot.pending_ticks = 1 return False - return None def ball_zone(game: Game) -> str: diff --git a/utama_core/strategy/press_and_pass.py b/utama_core/strategy/press_and_pass.py index bdff9b18..6d45d7b0 100644 --- a/utama_core/strategy/press_and_pass.py +++ b/utama_core/strategy/press_and_pass.py @@ -7,10 +7,9 @@ from utama_core.engine.context import TickContext from utama_core.engine.strategy import Strategy as KernelSchedulerStrategy from utama_core.engine.tactic import RobotId -from utama_core.entities.data.object import TeamType from utama_core.entities.game import Game from utama_core.motion_planning.src.common.motion_controller import MotionController -from utama_core.strategy.pickers import carrier_first +from utama_core.strategy.pickers import carrier_first, friendly_closer_to_ball from utama_core.tactics.give_and_go import GiveAndGoTactic from utama_core.tactics.press_and_contain import PressAndContainTactic @@ -39,9 +38,7 @@ def _press_and_pass_split_picker( if "defense" not in available_tactic_ids: return {"attack": frozenset(ordered)} if "attack" in available_tactic_ids else {} - _friendly_closest, friendly_dist = game.proximity_lookup.closest_to_ball(team_type_filter=TeamType.FRIENDLY) - _enemy_closest, enemy_dist = game.proximity_lookup.closest_to_ball(team_type_filter=TeamType.ENEMY) - friendly_has_ball_edge = friendly_dist < enemy_dist + friendly_has_ball_edge = friendly_closer_to_ball(game) is True if "attack" not in available_tactic_ids: return {"defense": frozenset(ordered)} diff --git a/utama_core/strategy/split_shape.py b/utama_core/strategy/split_shape.py index 8e510fc2..37f0548b 100644 --- a/utama_core/strategy/split_shape.py +++ b/utama_core/strategy/split_shape.py @@ -7,10 +7,9 @@ from utama_core.engine.context import TickContext from utama_core.engine.strategy import Strategy as KernelSchedulerStrategy from utama_core.engine.tactic import RobotId -from utama_core.entities.data.object import TeamType from utama_core.entities.game import Game from utama_core.motion_planning.src.common.motion_controller import MotionController -from utama_core.strategy.pickers import carrier_first +from utama_core.strategy.pickers import carrier_first, friendly_closer_to_ball from utama_core.tactics.lead_and_support import LeadAndSupportTactic from utama_core.tactics.shadow_and_mark import ShadowAndMarkTactic @@ -44,9 +43,7 @@ def _possession_split_picker( if not ordered: return {} - _friendly_closest, friendly_dist = game.proximity_lookup.closest_to_ball(team_type_filter=TeamType.FRIENDLY) - _enemy_closest, enemy_dist = game.proximity_lookup.closest_to_ball(team_type_filter=TeamType.ENEMY) - friendly_has_ball_edge = friendly_dist < enemy_dist + friendly_has_ball_edge = friendly_closer_to_ball(game) is True if "attack" not in available_tactic_ids: return {"defense": frozenset(ordered)} if "defense" in available_tactic_ids else {} diff --git a/utama_core/tactics/clear_ball.py b/utama_core/tactics/clear_ball.py index 34a414db..012d2e74 100644 --- a/utama_core/tactics/clear_ball.py +++ b/utama_core/tactics/clear_ball.py @@ -65,10 +65,12 @@ # (2*half_length/3 ≈ 3.0 m): the last metre of buildup in front of the box is # normal possession play, not yet an emergency. _DANGER_DEPTH = 2.6 +_DANGER_EXIT_DEPTH = 3.0 # An enemy within this range of the ball makes the danger "contested" — without # this gate the tactic would fire on every harmless back-pass to a free keeper # situation even with no opponent anywhere near. _PRESSURE_RANGE = 1.2 +_PRESSURE_EXIT_RANGE = 1.5 # Clearance-lane openness beyond 1.0 m is all equally "clear"; capping keeps a # fully-open lane from dominating the score via raw distance. _LANE_OPENNESS_CAP = 1.0 @@ -103,14 +105,23 @@ def _nearest_enemy_distance_to_ball(game: Game) -> Optional[float]: return best -def in_danger(game: Game) -> bool: - """The tactic's danger read: ball deep in our own third, contested by an enemy.""" +def in_danger(game: Game, was_in_danger: bool = False) -> bool: + """The tactic's danger read: ball deep in our own third, contested by an enemy. + + Includes spatial hysteresis on pressure range (enter <= 1.2 m, exit > 1.5 m) + and danger depth (enter <= 2.6 m, exit > 3.0 m) to prevent tactic flip-backs + when an opponent hovers around the contest boundary. + """ if game.ball is None: return False - if _progress_from_own_goal(game, game.ball.p.to_2d().x) > _DANGER_DEPTH: + + depth_limit = _DANGER_EXIT_DEPTH if was_in_danger else _DANGER_DEPTH + pressure_limit = _PRESSURE_EXIT_RANGE if was_in_danger else _PRESSURE_RANGE + + if _progress_from_own_goal(game, game.ball.p.to_2d().x) > depth_limit: return False enemy_dist = _nearest_enemy_distance_to_ball(game) - return enemy_dist is not None and enemy_dist <= _PRESSURE_RANGE + return enemy_dist is not None and enemy_dist <= pressure_limit def _clearance_candidates(game: Game, ball_p: Vector2D) -> list[Vector2D]: @@ -161,11 +172,18 @@ class ClearBallTactic(BaseTactic[ClearBallMem]): tag = TacticTag.DEFENSE + def __init__(self) -> None: + self._was_in_danger: bool = False + + def reset_applicable(self) -> None: + self._was_in_danger = False + def initial_mem(self) -> ClearBallMem: return ClearBallMem() def applicable(self, game: Game) -> bool: - return in_danger(game) + self._was_in_danger = in_danger(game, was_in_danger=self._was_in_danger) + return self._was_in_danger def highlights(self, mem: ClearBallMem) -> dict[RobotId, str]: return {mem.clearer_id: "clearer"} if mem.clearer_id is not None else {} diff --git a/utama_core/tactics/decoy_and_overload.py b/utama_core/tactics/decoy_and_overload.py index 69a3b9a5..54c84373 100644 --- a/utama_core/tactics/decoy_and_overload.py +++ b/utama_core/tactics/decoy_and_overload.py @@ -429,6 +429,8 @@ def tick( carried_enough = carry_exhausted(game, mem.carry_origin, stops_after=True) if (dragged or carried_enough or mem.lure_ticks >= _LURE_MAX_TICKS) and has_ball(game, mem.decoy_id): mem.phase = "finish" + elif mem.lure_ticks > _LURE_MAX_TICKS * 2 and not has_ball(game, mem.decoy_id): + return {}, DecoyOverloadMem() return commands, mem diff --git a/utama_core/tactics/press_and_contain.py b/utama_core/tactics/press_and_contain.py index c30eeed3..ed6fd5a9 100644 --- a/utama_core/tactics/press_and_contain.py +++ b/utama_core/tactics/press_and_contain.py @@ -36,13 +36,17 @@ has_ball, own_defense_area_exit_point, ) +from utama_core.shared.tolerance import Sticky from utama_core.skills.src.block import block_attacker from utama_core.skills.src.go_to_ball import go_to_ball from utama_core.skills.src.go_to_point import go_to_point from utama_core.skills.src.kick_upfield import kick_upfield from utama_core.skills.src.man_mark import man_mark -_PRESS_RANGE = 1.5 # metres — ball must be within this of an enemy for pressing to be applicable +_PRESS_ENTER_RANGE = 1.4 # metres — ball must be within this of an enemy to start pressing +_PRESS_EXIT_RANGE = 1.7 # metres — enemy must clear past this to release pressing +_PRESS_RANGE = 1.5 # metres — nominal midpoint kept for backward compatibility + # Hysteresis for the loose-ball/contested classification in `tick()` — a # plain per-tick re-read of `game.robot_with_ball is None` flickers almost @@ -110,12 +114,23 @@ class PressAndContainTactic(BaseTactic[PressAndContainMem]): tag = TacticTag.DEFENSE + def __init__(self) -> None: + self._was_applicable: bool = False + + def reset_applicable(self) -> None: + self._was_applicable = False + def initial_mem(self) -> PressAndContainMem: return PressAndContainMem() - def applicable(self, game: Game) -> bool: + def applicable(self, game: Game, current_applicable: Optional[bool] = None) -> bool: + was_applicable = self._was_applicable if current_applicable is None else current_applicable + threshold = _PRESS_EXIT_RANGE if was_applicable else _PRESS_ENTER_RANGE _enemy_id, distance = _enemy_nearest_ball(game) - return distance is not None and distance <= _PRESS_RANGE + is_app = distance is not None and distance <= threshold + if current_applicable is None: + self._was_applicable = is_app + return is_app def highlights(self, mem: PressAndContainMem) -> dict[RobotId, str]: highlights: dict[RobotId, str] = {rid: "marker" for rid in mem.marks} @@ -154,11 +169,15 @@ def tick( # close, so nothing else took over — and the enemy took it straight back # (65 of 309 PressAndContain losses in the 2026-09-24 round-robin). ball_winner = next((rid for rid in robot_ids if has_ball(game, rid)), None) - presser_id = ( - ball_winner - if ball_winner is not None - else min(robot_ids, key=lambda rid: game.friendly_robots[rid].p.distance_to(pressed_enemy.p)) - ) + if ball_winner is not None: + presser_id = ball_winner + else: + prev_presser = mem.presser_id if mem.presser_id in robot_ids else None + sticky = Sticky[int](margin=0.2, current=prev_presser) + presser_id = sticky.update( + list(robot_ids), + score_fn=lambda rid: -game.friendly_robots[rid].p.distance_to(pressed_enemy.p), + ) marker_ids = tuple(rid for rid in robot_ids if rid != presser_id) mem.presser_id = presser_id diff --git a/utama_core/tests/custom_referee/test_custom_referee.py b/utama_core/tests/custom_referee/test_custom_referee.py index e1a6cde6..4abaa48e 100644 --- a/utama_core/tests/custom_referee/test_custom_referee.py +++ b/utama_core/tests/custom_referee/test_custom_referee.py @@ -363,6 +363,36 @@ def test_same_robot_touching_twice_after_a_restart_fouls(self): assert violation is not None assert violation.rule_name == "double_touch" assert violation.next_command == RefereeCommand.DIRECT_FREE_BLUE + assert violation.offending_teams == (True,) + assert violation.offending_robots == ((True, 0),) + + def test_double_touch_charges_offending_teams_for_foul_counter(self): + # Double touch is a foul with foul counter increment (SSL §8.4); + # offending_teams must be populated with the offending color. + rule = DoubleTouchRule() + _arm_double_touch(rule, RefereeCommand.DIRECT_FREE_BLUE) + + # Blue kicker (enemy when my_team_is_yellow=True) + enemy = {1: _robot(1, 0.0, 0.0, is_friendly=False, has_ball=True)} + rule.check( + _frame(ball=_ball(0.0, 0.0), enemy_robots=enemy, my_team_is_yellow=True), GEO, RefereeCommand.NORMAL_START + ) + released = {1: _robot(1, 0.0, 0.0, is_friendly=False, has_ball=False)} + rule.check( + _frame(ball=_ball(0.0, 0.0), enemy_robots=released, my_team_is_yellow=True), + GEO, + RefereeCommand.NORMAL_START, + ) + retouched = {1: _robot(1, 0.0, 0.0, is_friendly=False, has_ball=True)} + violation = rule.check( + _frame(ball=_ball(0.0, 0.0), enemy_robots=retouched, my_team_is_yellow=True), + GEO, + RefereeCommand.NORMAL_START, + ) + + assert violation is not None + assert violation.offending_teams == (False,) # blue is yellow=False + assert violation.offending_robots == ((False, 1),) def test_designated_position_is_projected_clear_of_the_defense_area(self): """Regression, same class of bug as DefenseAreaRule/KeeperHeldBallRule/ diff --git a/utama_core/tests/custom_referee/test_pushing_crashing.py b/utama_core/tests/custom_referee/test_pushing_crashing.py index 36b2045f..eae301be 100644 --- a/utama_core/tests/custom_referee/test_pushing_crashing.py +++ b/utama_core/tests/custom_referee/test_pushing_crashing.py @@ -289,3 +289,14 @@ def test_not_active_play_no_violation(self): enemy = _robot(0, _CONTACT_X, 0.0, False, vx=0.0) frame = _frame(friendly, enemy) assert rule.check(frame, GEO, RefereeCommand.STOP) is None + + def test_separating_robots_never_crash(self): + # A robot moving away from an opponent (fleeing or pulling away at high speed) + # has negative closing speed and must never be called for a crashing foul, + # even if the relative velocity projection has large magnitude. + rule = CrashingRule(fault_speed_threshold_mps=1.5, both_fault_threshold_mps=0.3) + # Friendly at 0.0 moving away at vx=-2.0; enemy at 0.18 stationary (vx=0.0) + friendly = _robot(0, 0.0, 0.0, True, vx=-2.0) + enemy = _robot(0, _CONTACT_X, 0.0, False, vx=0.0) + frame = _frame(friendly, enemy) + assert rule.check(frame, GEO, RefereeCommand.NORMAL_START) is None diff --git a/utama_core/tests/engine/test_clear_ball_tactic.py b/utama_core/tests/engine/test_clear_ball_tactic.py new file mode 100644 index 00000000..9030bc54 --- /dev/null +++ b/utama_core/tests/engine/test_clear_ball_tactic.py @@ -0,0 +1,118 @@ +"""Unit tests for ClearBallTactic and in_danger hysteresis.""" + +from __future__ import annotations + +import math + +from utama_core.config.field_params import STANDARD_FIELD_DIMS +from utama_core.entities.data.vector import Vector2D, Vector3D +from utama_core.entities.game import Field, Game, GameHistory +from utama_core.entities.game.ball import Ball +from utama_core.entities.game.game_frame import GameFrame +from utama_core.entities.game.robot import Robot +from utama_core.tactics.clear_ball import ClearBallTactic, in_danger + + +def _robot(rid: int, x: float, y: float, is_friendly: bool, has_ball: bool = False, orientation: float = 0.0): + return Robot( + id=rid, + is_friendly=is_friendly, + has_ball=has_ball, + p=Vector2D(x, y), + v=Vector2D(0, 0), + a=Vector2D(0, 0), + orientation=orientation, + ) + + +def _game(ball_x: float, ball_y: float, enemy_x: float, enemy_y: float) -> Game: + # Field has half_length = 4.5. + # my_team_is_right=True: our goal line is at x = +4.5. + # ball at ball_x, distance from own goal line is 4.5 - ball_x. + friendly = { + 0: _robot(0, 4.0, 0.0, True), + 1: _robot(1, 3.5, 0.0, True), + } + enemy = { + 5: _robot(5, enemy_x, enemy_y, False), + } + zero = Vector3D(0.0, 0.0, 0.0) + frame = GameFrame( + ts=0.0, + my_team_is_yellow=True, + my_team_is_right=True, + friendly_robots=friendly, + enemy_robots=enemy, + ball=Ball(p=Vector3D(ball_x, ball_y, 0.0), v=zero, a=zero), + ) + field = Field( + my_team_is_right=True, field_dims=STANDARD_FIELD_DIMS, field_bounds=STANDARD_FIELD_DIMS.full_field_bounds + ) + return Game(past=GameHistory(max_history=20), current=frame, field=field) + + +def test_in_danger_pressure_hysteresis(): + tactic = ClearBallTactic() + # Field half_length = 4.5. Own goal is x=4.5. + # Ball at x=3.0 -> depth from own goal is 1.5 m (< 2.6 m danger depth). + # 1) Enemy at distance 1.35 m (between 1.2 m enter and 1.5 m exit). + # State was False -> should remain False. + game = _game(ball_x=3.0, ball_y=0.0, enemy_x=3.0, enemy_y=1.35) + assert not tactic.applicable(game) + + # 2) Enemy moves closer to 1.1 m (<= 1.2 m enter threshold). + # Enters danger! + game = _game(ball_x=3.0, ball_y=0.0, enemy_x=3.0, enemy_y=1.1) + assert tactic.applicable(game) + + # 3) Enemy retreats back to 1.35 m (> 1.2 m but <= 1.5 m exit threshold). + # Due to hysteresis, remains True! + game = _game(ball_x=3.0, ball_y=0.0, enemy_x=3.0, enemy_y=1.35) + assert tactic.applicable(game) + + # 4) Enemy retreats to 1.6 m (> 1.5 m exit threshold). + # Exits danger! + game = _game(ball_x=3.0, ball_y=0.0, enemy_x=3.0, enemy_y=1.6) + assert not tactic.applicable(game) + + +def test_in_danger_depth_hysteresis(): + tactic = ClearBallTactic() + # Enemy is always at distance 0.5 m from ball (<= 1.2 m). + # Own goal is at x=4.5. + # Enter depth: <= 2.6 m (ball_x >= 4.5 - 2.6 = 1.9 m) + # Exit depth: > 3.0 m (ball_x < 4.5 - 3.0 = 1.5 m) + + # 1) Ball at x=1.7 m (depth = 2.8 m: between 2.6 m enter and 3.0 m exit). + # Initial state False -> remains False. + game = _game(ball_x=1.7, ball_y=0.0, enemy_x=1.7, enemy_y=0.5) + assert not tactic.applicable(game) + + # 2) Ball moves deeper to x=2.0 m (depth = 2.5 m <= 2.6 m). + # Enters danger! + game = _game(ball_x=2.0, ball_y=0.0, enemy_x=2.0, enemy_y=0.5) + assert tactic.applicable(game) + + # 3) Ball clears slightly to x=1.7 m (depth = 2.8 m <= 3.0 m). + # Remains in danger! + game = _game(ball_x=1.7, ball_y=0.0, enemy_x=1.7, enemy_y=0.5) + assert tactic.applicable(game) + + # 4) Ball clears past exit depth to x=1.4 m (depth = 3.1 m > 3.0 m). + # Exits danger! + game = _game(ball_x=1.4, ball_y=0.0, enemy_x=1.4, enemy_y=0.5) + assert not tactic.applicable(game) + + +def test_clear_ball_tactic_applicable_and_reset(): + tactic = ClearBallTactic() + game = _game(ball_x=3.0, ball_y=0.0, enemy_x=3.0, enemy_y=1.1) + assert tactic.applicable(game) + + # Ball moves to middle (1.35 m enemy distance): hysteresis keeps it applicable + game2 = _game(ball_x=3.0, ball_y=0.0, enemy_x=3.0, enemy_y=1.35) + assert tactic.applicable(game2) + + # Reset clears the state + tactic.reset_applicable() + assert not tactic.applicable(game2) diff --git a/utama_core/tests/engine/test_decoy_and_overload_tactic.py b/utama_core/tests/engine/test_decoy_and_overload_tactic.py index 62a1958c..33802b18 100644 --- a/utama_core/tests/engine/test_decoy_and_overload_tactic.py +++ b/utama_core/tests/engine/test_decoy_and_overload_tactic.py @@ -290,3 +290,53 @@ def test_tactic_releases_its_robots_once_the_enemy_has_held_the_ball_1s(held_tic for _ in range(held_ticks): _commands, mem = tactic.tick(game, ctx, (1, 3), mem) assert tactic.is_committed(game, mem) is not released + + +def test_lure_phase_deadlock_times_out_when_decoy_lacks_ball(): + """When lure phase exceeds _LURE_MAX_TICKS * 2 and the decoy does not have the ball, + it must time out and return an empty command dict and reset memory, releasing commitment.""" + friendly = {1: _robot(1, 0.0, 0.0, True, has_ball=False), 3: _robot(3, -1.5, 1.5, True, has_ball=False)} + zv = Vector3D(0, 0, 0) + frame = GameFrame( + ts=0.0, + my_team_is_yellow=True, + my_team_is_right=True, + friendly_robots=friendly, + enemy_robots={}, + ball=Ball(p=Vector3D(2.0, 0.0, 0.0), v=zv, a=zv), + ) + field = Field( + my_team_is_right=True, field_dims=STANDARD_FIELD_DIMS, field_bounds=STANDARD_FIELD_DIMS.full_field_bounds + ) + game = Game(past=GameHistory(max_history=20), current=frame, field=field) + tactic = DecoyOverloadTactic() + ctx = TickContext(motion_controller=_NullMotionController()) + + # 1) Below 2x max ticks: does not time out yet + mem = DecoyOverloadMem( + decoy_id=1, + overloader_id=3, + marker_id=None, + marker_start_y=0.0, + phase="lure", + lure_ticks=_LURE_MAX_TICKS * 2 - 1, + ) + _commands, new_mem = tactic.tick(game, ctx, (1, 3), mem) + assert new_mem.phase == "lure" + assert tactic.is_committed(game, new_mem) is True + + # 2) At 2x max ticks: this tick increments lure_ticks to _LURE_MAX_TICKS * 2 + 1 (> 2 * _LURE_MAX_TICKS) + # Since decoy does not have ball, it times out and resets! + mem_at_limit = DecoyOverloadMem( + decoy_id=1, + overloader_id=3, + marker_id=None, + marker_start_y=0.0, + phase="lure", + lure_ticks=_LURE_MAX_TICKS * 2, + ) + commands, reset_mem = tactic.tick(game, ctx, (1, 3), mem_at_limit) + assert commands == {} + assert reset_mem.decoy_id is None + assert reset_mem.lure_ticks == 0 + assert tactic.is_committed(game, reset_mem) is False diff --git a/utama_core/tests/engine/test_press_and_contain_tactic.py b/utama_core/tests/engine/test_press_and_contain_tactic.py index edcfc7cd..1d5b7012 100644 --- a/utama_core/tests/engine/test_press_and_contain_tactic.py +++ b/utama_core/tests/engine/test_press_and_contain_tactic.py @@ -19,7 +19,10 @@ from utama_core.entities.game.game_frame import GameFrame from utama_core.entities.game.robot import Robot from utama_core.motion_planning.src.common.motion_controller import MotionController -from utama_core.tactics.press_and_contain import PressAndContainTactic +from utama_core.tactics.press_and_contain import ( + PressAndContainMem, + PressAndContainTactic, +) class _NullMotionController(MotionController): @@ -87,3 +90,77 @@ def test_without_the_ball_the_presser_still_presses(): tactic.initial_mem(), ) assert commands[2].kick == 0 + + +def test_press_and_contain_applicable_hysteresis(): + tactic = PressAndContainTactic() + + # Enter threshold: 1.4 m, Exit threshold: 1.7 m + # Ball at (0, 0). + def _game_with_enemy_dist(dist: float) -> Game: + friendly = {2: _robot(2, 1.0, 1.0, True), 3: _robot(3, 2.0, 2.0, True)} + enemy = {7: _robot(7, dist, 0.0, False)} + zero = Vector3D(0.0, 0.0, 0.0) + frame = GameFrame( + ts=0.0, + my_team_is_yellow=True, + my_team_is_right=True, + friendly_robots=friendly, + enemy_robots=enemy, + ball=Ball(p=zero, v=zero, a=zero), + ) + field = Field( + my_team_is_right=True, field_dims=STANDARD_FIELD_DIMS, field_bounds=STANDARD_FIELD_DIMS.full_field_bounds + ) + return Game(past=GameHistory(max_history=20), current=frame, field=field) + + # 1) Enemy at distance 1.55 m (between 1.4 m and 1.7 m). Initial is False -> False. + assert not tactic.applicable(_game_with_enemy_dist(1.55)) + + # 2) Enemy moves to 1.3 m (<= 1.4 m). Enters -> True. + assert tactic.applicable(_game_with_enemy_dist(1.3)) + + # 3) Enemy moves back to 1.55 m (> 1.4 m, <= 1.7 m). Remains -> True. + assert tactic.applicable(_game_with_enemy_dist(1.55)) + + # 4) Enemy moves to 1.75 m (> 1.7 m). Exits -> False. + assert not tactic.applicable(_game_with_enemy_dist(1.75)) + + # 5) Reset clears state. + tactic.applicable(_game_with_enemy_dist(1.3)) # now True + tactic.reset_applicable() + assert not tactic.applicable(_game_with_enemy_dist(1.55)) # reset back to False + + +def test_presser_sticky_margin(): + tactic = PressAndContainTactic() + ctx = TickContext(motion_controller=_NullMotionController()) + + # Ball at (0, 0), enemy at (0.5, 0.0). + # Friendly robot 2 at (1.5, 0.0) -> distance to enemy = 1.0 m. + # Friendly robot 3 at (1.4, 0.0) -> distance to enemy = 0.9 m (0.1 m closer, < 0.2 m margin). + def _game_with_positions(r2_x: float, r3_x: float) -> Game: + friendly = {2: _robot(2, r2_x, 0.0, True), 3: _robot(3, r3_x, 0.0, True)} + enemy = {7: _robot(7, 0.5, 0.0, False)} + zero = Vector3D(0.0, 0.0, 0.0) + frame = GameFrame( + ts=0.0, + my_team_is_yellow=True, + my_team_is_right=True, + friendly_robots=friendly, + enemy_robots=enemy, + ball=Ball(p=zero, v=zero, a=zero), + ) + field = Field( + my_team_is_right=True, field_dims=STANDARD_FIELD_DIMS, field_bounds=STANDARD_FIELD_DIMS.full_field_bounds + ) + return Game(past=GameHistory(max_history=20), current=frame, field=field) + + mem = PressAndContainMem(presser_id=2) + # Robot 3 is 0.1 m closer (< 0.2 m margin) -> robot 2 remains presser! + _, new_mem = tactic.tick(_game_with_positions(1.5, 1.4), ctx, (2, 3), mem) + assert new_mem.presser_id == 2 + + # Robot 3 moves to (1.2, 0.0) -> distance 0.7 m (0.3 m closer, > 0.2 m margin) -> switches to 3! + _, new_mem2 = tactic.tick(_game_with_positions(1.5, 1.2), ctx, (2, 3), mem) + assert new_mem2.presser_id == 3 diff --git a/utama_core/tests/engine/test_strategy.py b/utama_core/tests/engine/test_strategy.py index 5083818f..d9fc98cc 100644 --- a/utama_core/tests/engine/test_strategy.py +++ b/utama_core/tests/engine/test_strategy.py @@ -858,6 +858,7 @@ def _held_restart_strategy(): ctx=_ctx(), referee_overrides={ RefereeCommand.DIRECT_FREE_BLUE: lambda game, mc: {1: "keep-out", 2: "keep-out"}, + RefereeCommand.INDIRECT_FREE_BLUE: lambda game, mc: {1: "keep-out", 2: "keep-out"}, RefereeCommand.PREPARE_KICKOFF_BLUE: lambda game, mc: {1: "own-half", 2: "own-half"}, RefereeCommand.DIRECT_FREE_YELLOW: lambda game, mc: {1: "kicker", 2: "wait"}, }, @@ -866,7 +867,12 @@ def _held_restart_strategy(): @pytest.mark.parametrize( - "restart, held", [(RefereeCommand.DIRECT_FREE_BLUE, "keep-out"), (RefereeCommand.PREPARE_KICKOFF_BLUE, "own-half")] + "restart, held", + [ + (RefereeCommand.DIRECT_FREE_BLUE, "keep-out"), + (RefereeCommand.INDIRECT_FREE_BLUE, "keep-out"), + (RefereeCommand.PREPARE_KICKOFF_BLUE, "own-half"), + ], ) def test_their_restart_positioning_holds_after_normal_start_until_the_ball_is_in_play(restart, held): # decoy_and_overload_vs_tiki_taka / _vs_give_and_go_solo (2026-09-28): the @@ -887,3 +893,25 @@ def test_our_own_restart_hands_over_at_normal_start(): strategy, _ = _held_restart_strategy() strategy.tick(_RestartGame(RefereeCommand.DIRECT_FREE_YELLOW)) assert strategy.tick(_RestartGame(RefereeCommand.NORMAL_START)) == {1: "cmd-1", 2: "cmd-2"} + + +def test_barrier_reset_clears_closer_to_ball_hysteresis_state(): + from utama_core.strategy.pickers import _CLOSER_TO_BALL_STATE, _CloserToBallState + + _CLOSER_TO_BALL_STATE[True] = _CloserToBallState(state=True, pending_state=False, pending_ticks=10) + tactic = RecordingTactic() + strategy = Strategy( + tactics={"a": tactic}, + partitioner=Strategy.single_tactic_picker(lambda game, active: "a"), + outfield_robot_ids=(1, 2), + ctx=_ctx(), + ) + game1 = _FakeGame(RefereeCommand.NORMAL_START) + game1.my_team_is_yellow = True + strategy.tick(game1) + + game2 = _FakeGame(RefereeCommand.GOAL_YELLOW) + game2.my_team_is_yellow = True + strategy.tick(game2) + + assert True not in _CLOSER_TO_BALL_STATE diff --git a/utama_core/tests/motion_planning/fastpathplanner_corner_test.py b/utama_core/tests/motion_planning/fastpathplanner_corner_test.py index 31e5f1fb..4e6c2bcb 100644 --- a/utama_core/tests/motion_planning/fastpathplanner_corner_test.py +++ b/utama_core/tests/motion_planning/fastpathplanner_corner_test.py @@ -38,12 +38,9 @@ # Real enemy defense area with my_team_is_right=False (enemy goal at +x). _BOX = (3.5, 4.5, -1.0, 1.0) _SEEDS = range(40) -# 19 and 30 arrive since a robot pressed against a blocker may drive straight away from it -# (planner.py step 3b). -_KNOWN_STUCK = {0, 26} -# On the edge of the same flip: arrives on some CPUs (here at tick 64, after 14 carrot reversals) and -# sticks on CI's runners, whose float results differ slightly. Neither outcome fails the run. -_HOST_DEPENDENT = {28} +# All 40 seeds arrive cleanly after keying detour side memory on obstacle segments. +_KNOWN_STUCK: set[int] = set() +_HOST_DEPENDENT: set[int] = set() def _in_box(x: float, y: float) -> bool: diff --git a/utama_core/tests/referee/test_referee_unit.py b/utama_core/tests/referee/test_referee_unit.py index 506f4f95..f23ca06f 100644 --- a/utama_core/tests/referee/test_referee_unit.py +++ b/utama_core/tests/referee/test_referee_unit.py @@ -994,6 +994,138 @@ def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dri assert captured[0][0] == 0 assert captured[0][1] == Vector2D(-3.25, 0.0) + def test_direct_free_theirs_clears_robot_from_opponent_defense_area(self, monkeypatch): + from utama_core.custom_referee import actions as referee_actions + + captured = [] + + def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dribbling=False): + captured.append((robot_id, target_coords)) + return ("move", robot_id) + + monkeypatch.setattr(referee_actions, "move", fake_move) + + robots = { + 0: _robot(0, -4.3, 0.0), + 1: _robot(1, 1.0, 0.0), + } + referee = _make_referee_data(command=RefereeCommand.DIRECT_FREE_BLUE) + frame = GameFrame( + ts=0.0, + my_team_is_yellow=True, + my_team_is_right=True, + friendly_robots=robots, + enemy_robots={}, + ball=_ball(2.0, 0.0), + referee=referee, + ) + game = Game( + past=GameHistory(10), + current=frame, + field=Field( + my_team_is_right=True, + field_dims=STANDARD_FIELD_DIMS, + field_bounds=STANDARD_FIELD_DIMS.full_field_bounds, + ), + ) + cmd_map = _make_cmd_map(game) + node = referee_actions.DirectFreeTheirsStep() + node.blackboard = _make_blackboard(game, cmd_map) + + node.update() + + assert len(captured) == 1 + assert captured[0][0] == 0 + assert captured[0][1] == Vector2D(-3.25, 0.0) + + def test_direct_free_theirs_evicts_excess_defenders_from_own_defense_area(self, monkeypatch): + from utama_core.custom_referee import actions as referee_actions + + captured = [] + + def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dribbling=False): + captured.append((robot_id, target_coords)) + return ("move", robot_id) + + monkeypatch.setattr(referee_actions, "move", fake_move) + + robots = { + 0: _robot(0, 4.0, 0.0), + 1: _robot(1, 4.1, 0.2), + } + referee = _make_referee_data(command=RefereeCommand.DIRECT_FREE_BLUE) + frame = GameFrame( + ts=0.0, + my_team_is_yellow=True, + my_team_is_right=True, + friendly_robots=robots, + enemy_robots={}, + ball=_ball(0.0, 0.0), + referee=referee, + ) + game = Game( + past=GameHistory(10), + current=frame, + field=Field( + my_team_is_right=True, + field_dims=STANDARD_FIELD_DIMS, + field_bounds=STANDARD_FIELD_DIMS.full_field_bounds, + ), + ) + cmd_map = _make_cmd_map(game) + node = referee_actions.DirectFreeTheirsStep() + node.blackboard = _make_blackboard(game, cmd_map) + + node.update() + + assert len(captured) == 1 + assert captured[0][0] == 1 + assert captured[0][1] == Vector2D(3.27, 0.2) + + def test_ball_placement_theirs_clears_robot_from_opponent_defense_area(self, monkeypatch): + from utama_core.custom_referee import actions as referee_actions + + captured = [] + + def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dribbling=False): + captured.append((robot_id, target_coords)) + return ("move", robot_id) + + monkeypatch.setattr(referee_actions, "move", fake_move) + + robots = { + 0: _robot(0, -4.3, 0.0), + 1: _robot(1, 1.0, 0.0), + } + referee = _make_referee_data(command=RefereeCommand.BALL_PLACEMENT_BLUE) + frame = GameFrame( + ts=0.0, + my_team_is_yellow=True, + my_team_is_right=True, + friendly_robots=robots, + enemy_robots={}, + ball=_ball(2.0, 0.0), + referee=referee, + ) + game = Game( + past=GameHistory(10), + current=frame, + field=Field( + my_team_is_right=True, + field_dims=STANDARD_FIELD_DIMS, + field_bounds=STANDARD_FIELD_DIMS.full_field_bounds, + ), + ) + cmd_map = _make_cmd_map(game) + node = referee_actions.BallPlacementTheirsStep() + node.blackboard = _make_blackboard(game, cmd_map) + + node.update() + + assert len(captured) == 1 + assert captured[0][0] == 0 + assert captured[0][1] == Vector2D(-3.25, 0.0) + def test_ball_placement_theirs_clears_encroaching_robot(self, monkeypatch): from utama_core.custom_referee import actions as referee_actions @@ -1190,7 +1322,7 @@ def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dri keeper_target = next(target for robot_id, target in captured if robot_id == 1) support_targets = [target for robot_id, target in captured if robot_id != 1] - assert keeper_target.x == pytest.approx(4.5) + assert keeper_target.x == pytest.approx(4.5 - ROBOT_RADIUS) assert all(target.x > 0.0 for target in support_targets) def test_prepare_penalty_theirs_support_robots_stand_outside_the_keep_out_circle(self, monkeypatch): @@ -1288,6 +1420,155 @@ def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dri t = min(1.0, max(0.0, (mark - start).dot(seg) / seg.dot(seg))) assert (start + seg * t - mark).mag() >= 0.5 + def test_prepare_penalty_ours_goalkeeper_stays_on_own_goal_line(self, monkeypatch): + # PreparePenaltyOursStep must keep the friendly goalkeeper on our own goal line, + # not displace it into the opponent's half behind the penalty mark. + from utama_core.custom_referee import actions as referee_actions + + captured = {} + + def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dribbling=False): + captured[robot_id] = target_coords + return ("move", robot_id) + + monkeypatch.setattr(referee_actions, "move", fake_move) + + robots = {0: _robot(0, -4.4, 0.0), 1: _robot(1, 0.0, 0.0), 2: _robot(2, 1.0, 0.0)} + referee = _make_referee_data(command=RefereeCommand.PREPARE_PENALTY_YELLOW) + referee.yellow_team.goalkeeper = 0 + game = _make_game( + friendly_robots=robots, + referee=referee, + my_team_is_yellow=True, + my_team_is_right=False, # defends left (-4.5), attacks right (+4.5) + ball=_ball(2.25, 0.0), + ) + node = referee_actions.PreparePenaltyOursStep() + node.blackboard = _make_blackboard(game, _make_cmd_map(game)) + node.update() + + assert 0 in captured, "Goalkeeper was not given a command" + keeper_target = captured[0] + # Own goal line is at -4.5; keeper sits at -4.5 + ROBOT_RADIUS (-4.41) + assert keeper_target.x == pytest.approx(-4.5 + ROBOT_RADIUS) + assert keeper_target.y == pytest.approx(0.0) + + def test_prepare_penalty_line_formation_centered_symmetrically(self, monkeypatch): + # Penalty support robots on the line behind the mark must be centered + # symmetrically around y=0. + from utama_core.custom_referee import actions as referee_actions + + captured = {} + + def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dribbling=False): + captured[robot_id] = target_coords + return ("move", robot_id) + + monkeypatch.setattr(referee_actions, "move", fake_move) + + robots = {0: _robot(0, 4.4, 0.0)} + for rid in range(1, 6): + robots[rid] = _robot(rid, 0.0, (rid - 3) * 0.5) + referee = _make_referee_data(command=RefereeCommand.PREPARE_PENALTY_BLUE) + referee.yellow_team.goalkeeper = 0 + game = _make_game( + friendly_robots=robots, + referee=referee, + my_team_is_yellow=True, + my_team_is_right=True, + ball=_ball(2.25, 0.0), + ) + node = referee_actions.PreparePenaltyTheirsStep() + node.blackboard = _make_blackboard(game, _make_cmd_map(game)) + node.update() + + line_targets = [captured[rid] for rid in range(1, 6)] + mean_y = sum(t.y for t in line_targets) / len(line_targets) + assert mean_y == pytest.approx(0.0, abs=1e-6) + + def test_prepare_penalty_theirs_clears_extra_defender_from_own_defense_area(self, monkeypatch): + # PreparePenaltyTheirsStep must clear any outfield defender beyond the keeper + # out of own defense area (clear_own_defense_area=True). + from utama_core.custom_referee import actions as referee_actions + + captured = {} + + def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dribbling=False): + captured[robot_id] = target_coords + return ("move", robot_id) + + monkeypatch.setattr(referee_actions, "move", fake_move) + + # Robot 0 is keeper; robot 1 is inside own defense area (x=4.0, y=0.0 when goal is at 4.5) + robots = {0: _robot(0, 4.4, 0.0), 1: _robot(1, 4.0, 0.0)} + referee = _make_referee_data(command=RefereeCommand.PREPARE_PENALTY_BLUE) + referee.yellow_team.goalkeeper = 0 + game = _make_game( + friendly_robots=robots, + referee=referee, + my_team_is_yellow=True, + my_team_is_right=True, + ball=_ball(2.25, 0.0), + ) + node = referee_actions.PreparePenaltyTheirsStep() + node.blackboard = _make_blackboard(game, _make_cmd_map(game)) + node.update() + + assert not referee_actions._is_in_own_defense_area(game, captured[1].x, captured[1].y) + + def test_prepare_penalty_ours_clears_teammate_from_opp_defense_area(self, monkeypatch): + # PreparePenaltyOursStep must clear teammates from opponent defense area + # (clear_opp_defense_area=True). + from utama_core.custom_referee import actions as referee_actions + + captured = {} + + def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dribbling=False): + captured[robot_id] = target_coords + return ("move", robot_id) + + monkeypatch.setattr(referee_actions, "move", fake_move) + + # Attacking right goal (+4.5). Opp defense area is at x >= 3.5. + # Robot 1 is kicker, robot 2 starts inside opp defense area at x=4.0, y=0.0 + robots = {0: _robot(0, -4.4, 0.0), 1: _robot(1, 1.0, 0.0), 2: _robot(2, 4.0, 0.0)} + referee = _make_referee_data(command=RefereeCommand.PREPARE_PENALTY_YELLOW) + referee.yellow_team.goalkeeper = 0 + game = _make_game( + friendly_robots=robots, + referee=referee, + my_team_is_yellow=True, + my_team_is_right=False, # attacks right + ball=_ball(2.25, 0.0), + ) + node = referee_actions.PreparePenaltyOursStep() + node.blackboard = _make_blackboard(game, _make_cmd_map(game)) + node.update() + + # Robot 2's commanded target must be clear of opp defense area (x <= 3.5 - keep_dist) + opp_inner_x = 4.5 - 2.0 * game.field.half_defense_area_depth + assert captured[2].x <= opp_inner_x - referee_actions.OPPONENT_DEFENSE_AREA_KEEP_DISTANCE + + +class TestReachableOnCircle: + def test_point_inside_circle_near_boundary_is_projected_onto_circle(self): + # A robot clamped to the pitch boundary by _clamp_to_field may end up + # inside the keep-out circle. _reachable_on_circle must not return the + # unprojected inside-the-circle point; it must return a point at radius + # distance from center that is inside the pitch boundaries. + from utama_core.custom_referee import actions as referee_actions + + game = _make_game(my_team_is_yellow=True, my_team_is_right=False) + center = Vector2D(1.79, 2.75) # ball near top sideline (y = 3.0) + radius = 0.80 + # Point clamped to sideline margin (y = 2.90), distance to center is ~0.68m (< 0.80m) + inside_point = Vector2D(2.45697, 2.90) + assert (inside_point - center).mag() < radius + + result = referee_actions._reachable_on_circle(game, inside_point, center, radius) + assert abs((result - center).mag() - radius) <= 1e-3 + assert referee_actions._clamp_to_field(result, game) == result + class TestVariableFieldScaling: def test_prepare_kickoff_ours_scales_support_positions_with_field_bounds(self, monkeypatch): @@ -1876,6 +2157,91 @@ def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dri node.update() assert captured_kicker_ids[-1] == 3 + def test_direct_free_ours_clears_non_kicker_from_opponent_defense_area(self, monkeypatch): + from utama_core.custom_referee import actions as referee_actions + + captured = [] + + def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dribbling=False): + captured.append((robot_id, target_coords)) + return ("move", robot_id) + + monkeypatch.setattr(referee_actions, "move", fake_move) + + # Robot 1 is kicker near ball at (1.0, 0.0); Robot 0 is inside opp defense area at (-4.3, 0.0) + robots = {0: _robot(0, -4.3, 0.0), 1: _robot(1, 1.2, 0.0)} + referee = _make_referee_data(command=RefereeCommand.DIRECT_FREE_YELLOW) + frame = GameFrame( + ts=0.0, + my_team_is_yellow=True, + my_team_is_right=True, + friendly_robots=robots, + enemy_robots={}, + ball=_ball(1.0, 0.0), + referee=referee, + ) + game = Game( + past=GameHistory(10), + current=frame, + field=Field( + my_team_is_right=True, + field_dims=STANDARD_FIELD_DIMS, + field_bounds=STANDARD_FIELD_DIMS.full_field_bounds, + ), + ) + cmd_map = _make_cmd_map(game) + node = referee_actions.DirectFreeOursStep() + node.blackboard = _make_blackboard(game, cmd_map) + + node.update() + + # Non-kicker robot 0 should be moved outside opponent defense area + r0_moves = [c for c in captured if c[0] == 0] + assert len(r0_moves) == 1 + assert r0_moves[0][1] == Vector2D(-3.25, 0.0) + + def test_direct_free_ours_clears_non_kicker_from_ball_keep_out(self, monkeypatch): + from utama_core.custom_referee import actions as referee_actions + + captured = [] + + def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dribbling=False): + captured.append((robot_id, target_coords)) + return ("move", robot_id) + + monkeypatch.setattr(referee_actions, "move", fake_move) + + # Ball at (1.0, 0.0). Robot 1 is kicker at (1.1, 0.0). Robot 0 is non-kicker at (1.3, 0.0) inside 0.8m keep-out. + robots = {0: _robot(0, 1.3, 0.0), 1: _robot(1, 1.1, 0.0)} + referee = _make_referee_data(command=RefereeCommand.DIRECT_FREE_YELLOW) + frame = GameFrame( + ts=0.0, + my_team_is_yellow=True, + my_team_is_right=True, + friendly_robots=robots, + enemy_robots={}, + ball=_ball(1.0, 0.0), + referee=referee, + ) + game = Game( + past=GameHistory(10), + current=frame, + field=Field( + my_team_is_right=True, + field_dims=STANDARD_FIELD_DIMS, + field_bounds=STANDARD_FIELD_DIMS.full_field_bounds, + ), + ) + cmd_map = _make_cmd_map(game) + node = referee_actions.DirectFreeOursStep() + node.blackboard = _make_blackboard(game, cmd_map) + + node.update() + + r0_moves = [c for c in captured if c[0] == 0] + assert len(r0_moves) == 1 + assert (r0_moves[0][1] - Vector2D(1.0, 0.0)).mag() == pytest.approx(0.8) + # --------------------------------------------------------------------------- # DirectFreeTheirsStep — comprehensive keep-out coverage @@ -1955,6 +2321,35 @@ def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dri assert len(captured) == 1 assert captured[0][1] == pytest.approx(Vector2D(0.8, 0.0)) + def test_robot_one_centimetre_short_of_keep_out_boundary_is_commanded_to_boundary(self, monkeypatch): + """A robot at 0.795 m (within 1 cm of the 0.8 m keep-out) must be commanded to 0.8 m. + + Before the fix, dx * dx + dy * dy < 1e-4 treated it as already at target and + issued empty_command(False), leaving it stranded at 0.795 m inside the keep-out. + """ + from utama_core.custom_referee import actions as referee_actions + + captured = [] + + def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dribbling=False): + captured.append((robot_id, target_coords)) + return ("move", robot_id) + + monkeypatch.setattr(referee_actions, "move", fake_move) + + # Robot at (0.795, 0.0), 5 mm inside the 0.80 m keep-out circle: + robots = {0: _robot(0, 0.795, 0.0)} + referee = _make_referee_data(command=RefereeCommand.DIRECT_FREE_BLUE) + game = _make_game(friendly_robots=robots, referee=referee) + cmd_map = _make_cmd_map(game) + node = referee_actions.DirectFreeTheirsStep() + node.blackboard = _make_blackboard(game, cmd_map) + + node.update() + + assert len(captured) == 1 + assert captured[0][1] == pytest.approx(Vector2D(0.8, 0.0)) + def test_robot_between_ball_and_opponent_box_clears_round_the_ball(self, monkeypatch): """A free kick 1 m in front of the opponent box (inner edge x = -3.5 here): straight out from the ball is (-3.3, 0), where the planner's box margin @@ -2019,6 +2414,34 @@ def test_all_robots_get_commands(self, monkeypatch): for v in cmd_map.values(): assert v is not None + def test_clear_to_legal_positions_touchline_clamping_preserves_keep_out(self, monkeypatch): + from utama_core.custom_referee import actions as referee_actions + + captured = [] + + def fake_move(game, motion_controller, robot_id, target_coords, target_oren, dribbling=False): + captured.append(target_coords) + return ("move", robot_id) + + monkeypatch.setattr(referee_actions, "move", fake_move) + + ball_pos = Vector2D(0.0, 2.9) + robots = {0: _robot(0, 0.0, 2.8)} + referee = _make_referee_data(command=RefereeCommand.DIRECT_FREE_BLUE) + game = _make_game(friendly_robots=robots, referee=referee, ball=_ball(ball_pos.x, ball_pos.y)) + cmd_map = _make_cmd_map(game) + node = referee_actions.DirectFreeTheirsStep() + node.blackboard = _make_blackboard(game, cmd_map) + + node.update() + + assert len(captured) == 1 + target = captured[0] + # Target must be on the field + assert referee_actions._clamp_to_field(target, game) == target + # Target must strictly satisfy keep-out distance + assert (target - ball_pos).mag() >= 0.8 - 1e-6 + class TestDetourAroundCircle: """`detour_around_circle`: robots crossing the ball's keep-out circle go round it.""" diff --git a/utama_core/tests/replay/test_fingerprint.py b/utama_core/tests/replay/test_fingerprint.py index cf2964df..1dfb3d50 100644 --- a/utama_core/tests/replay/test_fingerprint.py +++ b/utama_core/tests/replay/test_fingerprint.py @@ -401,6 +401,7 @@ def test_the_base_holds_the_sim_script_and_referee_profiles(graph): "utama_core.rsoccer_simulator.src.Simulators.robosim.robosim_wrapper", "utama_core.shared.pass_and_score_geometry", "utama_core.skills.src.shielding", + "utama_core.strategy.pickers", ], "opens files": [ "utama_core.custom_referee.profiles.profile_loader", diff --git a/utama_core/tests/strategy/test_friendly_closer_to_ball.py b/utama_core/tests/strategy/test_friendly_closer_to_ball.py index 62425fc6..834e3bef 100644 --- a/utama_core/tests/strategy/test_friendly_closer_to_ball.py +++ b/utama_core/tests/strategy/test_friendly_closer_to_ball.py @@ -1,10 +1,22 @@ from types import SimpleNamespace +import pytest + from utama_core.entities.data.object import TeamType -from utama_core.strategy.kernel_strategy import friendly_closer_to_ball +from utama_core.strategy.kernel_strategy import ( + friendly_closer_to_ball, + reset_closer_to_ball_state, +) + +@pytest.fixture(autouse=True) +def _reset_state(): + reset_closer_to_ball_state() + yield + reset_closer_to_ball_state() -def _game(friendly_dist, enemy_dist, friendly_has_ball, enemy_has_ball=False): + +def _game(friendly_dist, enemy_dist, friendly_has_ball=False, enemy_has_ball=False): distances = {TeamType.FRIENDLY: (3, friendly_dist), TeamType.ENEMY: (2, enemy_dist)} return SimpleNamespace( proximity_lookup=SimpleNamespace(closest_to_ball=lambda team_type_filter: distances[team_type_filter]), @@ -23,6 +35,7 @@ def test_a_ball_on_our_dribbler_is_ours_while_an_enemy_brushes_past(): def test_a_contested_ball_still_falls_back_to_distance(): # Enemy clearly nearer (gap 0.381 m, well outside the 5 cm margin). assert friendly_closer_to_ball(_game(0.5, 0.119, friendly_has_ball=False)) is False + reset_closer_to_ball_state() assert friendly_closer_to_ball(_game(0.119, 0.5, friendly_has_ball=False)) is True @@ -31,13 +44,57 @@ def test_a_near_tie_inside_the_margin_is_undecided_not_lost(): # the sticky pickers (`edge is False` to give up possession) must keep their # previous state. This used to return False and drop the ball on noise. assert friendly_closer_to_ball(_game(0.119, 0.161, friendly_has_ball=True, enemy_has_ball=True)) is None + reset_closer_to_ball_state() assert friendly_closer_to_ball(_game(0.161, 0.119, friendly_has_ball=False)) is None + reset_closer_to_ball_state() assert friendly_closer_to_ball(_game(0.30, 0.30, friendly_has_ball=False)) is None def test_margin_boundary_is_exclusive_on_both_sides(): # Just inside the margin is undecided; just past it decides. assert friendly_closer_to_ball(_game(0.10, 0.1499, friendly_has_ball=False)) is None + reset_closer_to_ball_state() assert friendly_closer_to_ball(_game(0.1499, 0.10, friendly_has_ball=False)) is None + reset_closer_to_ball_state() assert friendly_closer_to_ball(_game(0.10, 0.1501, friendly_has_ball=False)) is True + reset_closer_to_ball_state() assert friendly_closer_to_ball(_game(0.1501, 0.10, friendly_has_ball=False)) is False + + +def test_possession_hysteresis_holds_through_transient_distance_flicker(): + # Friendly acquires possession clearly + assert friendly_closer_to_ball(_game(0.1, 0.4)) is True + + # Enemy gets closer for 14 ticks (< 15 ticks / ~0.25 s) within break margin: + # possession must hold True without flickering to False. + for _ in range(14): + assert friendly_closer_to_ball(_game(0.3, 0.2)) is True + + # Friendly regains advantage: flicker resolved, state remains True. + assert friendly_closer_to_ball(_game(0.1, 0.4)) is True + + +def test_possession_transfers_after_sustained_advantage(): + assert friendly_closer_to_ball(_game(0.1, 0.4)) is True + + # 14 ticks: still held + for _ in range(14): + assert friendly_closer_to_ball(_game(0.3, 0.2)) is True + + # 15th tick: sustained advantage completes turnover + assert friendly_closer_to_ball(_game(0.3, 0.2)) is False + + +def test_possession_breaks_immediately_on_large_clearance(): + assert friendly_closer_to_ball(_game(0.1, 0.4)) is True + # Ball cleared far away to enemy (gap 1.0 m > 0.8 m break margin) + assert friendly_closer_to_ball(_game(1.5, 0.5)) is False + + +def test_dribbler_contact_overrides_immediately(): + # Enemy currently holds distance possession + assert friendly_closer_to_ball(_game(0.5, 0.2)) is False + # Friendly makes physical contact with dribbler -> immediate True + assert friendly_closer_to_ball(_game(0.5, 0.2, friendly_has_ball=True)) is True + # Enemy makes physical contact -> immediate False + assert friendly_closer_to_ball(_game(0.2, 0.5, enemy_has_ball=True)) is False