fix: motion planning detours & hysteresis, custom referee rules, and benchmark metrics

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

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.

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)
 

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

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