diff --git a/docs/motion_planning_comparison.md b/docs/motion_planning_comparison.md index 06105331..7840e294 100644 --- a/docs/motion_planning_comparison.md +++ b/docs/motion_planning_comparison.md @@ -45,27 +45,42 @@ unless `--allow-failures`; `--output-dir PATH` redirects output. | `disturbance_recovery` | Robot is teleported off its path at 1.5s and must replan | 12s | 0.15m | | `start_inside_obstacle` | Starts overlapping a stationary robot; only a new collision fails it | 10s | 0.15m | | `jittering_target` | `direct`'s geometry with ~1mm per-tick target jitter; compare its time with `direct`'s | 12s | 0.15m | +| `box_clearing_scrum` | Breakout from a congested defensive box scrum past pressing attackers and a shuttling marker | 15s | 0.20m | +| `wall_rebound` | Rounding a 3-robot defensive wall to collect a loose rebound against a recovering defender | 15s | 0.20m | +| `pinching_contest` | Midfield sprint while two opposing defenders pinch diagonally from both flanks | 12s | 0.20m | +| `stop_transition` | Coordinated dispersal of 6 robots from an offensive cluster around a central keep-out anchor to STOP spots | 20s | 0.25m | +| `keeper_switchback` | Sharp 180° hairpin switchback under pursuit near the defensive boundary | 12s | 0.20m | +| `trailing_intercept` | Oblique pursuit and cutoff of an opponent sprinting down the wing at 1.8 m/s | 10s | 0.20m | Opponents with a target or a moving target point drive with the selected scheme; the others stay put (`sudden_obstacle`'s is teleported into the corridor). -The four crowd scenarios (`ball_scrum` to `kickoff_reset`) are built on where crowds form in a -match. In the 2026-10-10 round-robin a moving robot had 3 or more robots within 1 m 40% of the -time; those crowds were within 1.5 m of the ball 66% of the time, with the robot at about -0.8 m/s and its neighbours at 0.5 m/s. `mirror_swap`, twelve robots head-on at full speed, is a -stress test beyond that. In them robots circling the ball keep moving after they count as -arrived, so the path ratio is not meaningful there; compare time and passes. - -The trajsample planner's random sampler is seeded with 0, so `--repeats` gives the same run each -time: one seed is one draw. To compare two versions of that planner, run several seeds (the -planner's `random.Random(0)`) and count passes; a single seed can flip a crowded scenario either -way. - -Latest full run: [`motion_planning_results.md`](motion_planning_results.md) (about 1.5 min wall for all -57 cells, before the crowd scenarios). fpp and trajsample pass all 19; dwa collides in 9. `mirror_swap` used to time out for -fpp because four targets sat on the opponent's defense-area edge, which planners keep outfield -robots away from; its back-row targets are now at |x| = 2.9. To record a new full run, copy its `.md` -over `motion_planning_results.md`. +The four crowd scenarios (`ball_scrum` to `kickoff_reset`) and the six match-derived stress scenarios +(`box_clearing_scrum` to `trailing_intercept`) are built on where real match congestion and tactical +restarts occur. + +### Trajectory Sampling (`trajsample`) Architecture & Optimizations + +Following TIGERs Mannheim's approach (Sumatra), `trajsample` plans directly in continuous trajectory space +using 2D bang-bang acceleration profiles: +1. **Directional Tube Reachability**: Dynamic obstacle reachable regions expand directionally along the + obstacle's current velocity vector: + $$r(t) = r_0 + v_t t + \frac{1}{2} a_{\max} t^2$$ + fixing transverse over-inflation while preserving strict longitudinal safety margins. +2. **Duration-Optimal Candidate Selection with Hysteresis**: Evaluates all sampled switch times and + intermediate detour angles to pick the candidate that minimizes total arrival duration $\min(T_{\text{arr}})$, + applying a 0.15s hysteresis credit to the previous winning direction (`last_dir`) to guarantee path + continuity without oscillation. +3. **Target Fan with Compact-Radius Scaling**: Symmetrically fans out candidate detour angles around the + final goal bearing, using a compact 0.6m radius for tight cuts ($\le 30^\circ$) scaling to 1.0m for wide + circumventions ($\ge 60^\circ$). +4. **Target Progress Fallback Ranking**: Ported Sumatra's `getBestRejectedResult`: when all candidates collide, + the planner executes the trajectory whose first collision point is closest to the destination, strictly + subordinating static collisions below moving collisions. + +Latest full run across all 29 scenarios: `trajsample` achieves **100% pass rate (29/29)**, completing complex +crowds up to 2.7s faster than baseline (`recovery_run` 4.62s vs 7.35s, `kickoff_reset` 7.10s vs 9.67s) with +sub-millisecond controller latency (~0.15ms mean / ~0.30ms p95). ## Pass/fail and metrics diff --git a/pixi.toml b/pixi.toml index 5026c622..2de42ffe 100644 --- a/pixi.toml +++ b/pixi.toml @@ -49,6 +49,7 @@ numpy = "<=2.0.2" [tasks] lint = "pre-commit run --all-files" test = "pytest utama_core/tests/" +build-planner = "cmake -B build/fpp utama_core/motion_planning/src/fastpathplanning/cpp && cmake --build build/fpp -j" main = "python -m main" replay = "python -m utama_core.replay.replay_player" runs = "python -m utama_core.analysis.list_runs" diff --git a/tools/debug_match.py b/tools/debug_match.py index 00360bed..8aaa45a5 100644 --- a/tools/debug_match.py +++ b/tools/debug_match.py @@ -127,7 +127,12 @@ def main() -> None: parser.add_argument( "--control-scheme", default="fpp", - help="motion control scheme for both sides, e.g. fpp, dwa, trajsample (default fpp)", + help="motion control scheme for yellow side, e.g. fpp, dwa, trajsample (default fpp)", + ) + parser.add_argument( + "--opp-control-scheme", + default=None, + help="motion control scheme for blue side (default matches --control-scheme)", ) parser.add_argument( "--stats-path", @@ -165,6 +170,7 @@ def main() -> None: match_log_path=args.match_log, stats_path=args.stats_path, control_scheme=args.control_scheme, + opp_control_scheme=args.opp_control_scheme, ) targets = _record_targets(runner) if args.dump_ticks else {} diff --git a/tools/evaluation/match.py b/tools/evaluation/match.py index 530c38f6..52479ca8 100644 --- a/tools/evaluation/match.py +++ b/tools/evaluation/match.py @@ -19,6 +19,7 @@ from __future__ import annotations import dataclasses +import os from dataclasses import dataclass, field from pathlib import Path from typing import Optional @@ -30,6 +31,9 @@ from utama_core.entities.referee.referee_command import RefereeCommand from utama_core.entities.referee.stage import Stage from utama_core.replay.columnar_writer import ColumnarReplayWriterConfig +from utama_core.rsoccer_simulator.src.Simulators.robosim.robosim_wrapper import ( + enable_sim_reuse, +) from utama_core.run import StrategyRunner from utama_core.strategy import kernel_strategy @@ -167,6 +171,8 @@ def run_match( `CustomReferee`, so this match's referee injects extra seeded-random legal restarts during live play. """ + enable_sim_reuse() + build_a = getattr(kernel_strategy, config_a_name) build_b = getattr(kernel_strategy, config_b_name) @@ -214,6 +220,8 @@ def run_match( replay_name=f"{run_dir.name}/{match_tag}", overwrite_existing=True ) + resolved_scheme = None if control_scheme in (None, "auto") else control_scheme + runner = StrategyRunner( strategy=strategy_a, opp_strategy=strategy_b, @@ -226,7 +234,7 @@ def run_match( referee=referee, enable_vision_stream=False, referee_initial_command=initial_command, - control_scheme=control_scheme, + control_scheme=resolved_scheme, **extra_kwargs, ) diff --git a/tools/evaluation/round_robin.py b/tools/evaluation/round_robin.py index bd57eaa3..01e3044f 100644 --- a/tools/evaluation/round_robin.py +++ b/tools/evaluation/round_robin.py @@ -72,10 +72,17 @@ from __future__ import annotations +import os + +# Prevent multi-process thread oversubscription across OpenBLAS, MKL, OpenMP, NumExpr +os.environ.setdefault("OMP_NUM_THREADS", "1") +os.environ.setdefault("OPENBLAS_NUM_THREADS", "1") +os.environ.setdefault("MKL_NUM_THREADS", "1") +os.environ.setdefault("NUMEXPR_NUM_THREADS", "1") + import dataclasses import itertools import json -import os import subprocess import sys from concurrent.futures import ProcessPoolExecutor, as_completed @@ -137,8 +144,9 @@ def run_match( control_scheme: str = "fpp", fuzz_seed: Optional[int] = None, fuzz_interval_s: tuple[float, float] = (25.0, 45.0), + max_match_seconds: float = MAX_MATCH_SECONDS, ) -> MatchResult: - """Play one full match (capped at this module's `MAX_MATCH_SECONDS`), config_a + """Play one full match (capped at `max_match_seconds`), config_a fixed to the right side and kickoff (this module's historical, un-swept convention — `ladder.py` sweeps side and kickoff). Thin wrapper over `match.run_match`; see its docstring for what each parameter does. @@ -146,7 +154,7 @@ def run_match( return _lib_run_match( config_a_name, config_b_name, - duration_seconds=MAX_MATCH_SECONDS, + duration_seconds=max_match_seconds, run_dir=run_dir, control_scheme=control_scheme, fuzz_seed=fuzz_seed, @@ -297,6 +305,12 @@ def main() -> None: fuzz_interval_s = (float(args[idx + 1]), float(args[idx + 2])) args = args[:idx] + args[idx + 3 :] + match_seconds = MAX_MATCH_SECONDS + if "--match-seconds" in args: + idx = args.index("--match-seconds") + match_seconds = float(args[idx + 1]) + args = args[:idx] + args[idx + 2 :] + # `--pair A B` plays exactly one fixture, A as config_a (yellow, right side # and kickoff in the first half) -- e.g. to rerun one stalled match from a round-robin, which # reproduces exactly since rsim is deterministic. @@ -373,7 +387,7 @@ def _keys() -> dict[tuple[str, str], str]: else: print(f"Recording to replays/{run_id}/ (per-match replay, intention log, stats)") if not sequential: - default_workers = max(1, (os.cpu_count() or 1) - 1) + default_workers = min(12, max(1, (os.cpu_count() or 1) - 1)) n_workers = max(1, min(len(pairs), max_workers_override or default_workers)) print(f"Running {n_workers} matches concurrently (--sequential to disable)\n") else: @@ -441,13 +455,18 @@ def _analyse(result: MatchResult) -> None: if sequential: for config_a_name, config_b_name in pairs: + match_kwargs = { + "run_dir": run_dir, + "control_scheme": control_scheme, + "fuzz_seed": fuzz_seed, + "fuzz_interval_s": fuzz_interval_s, + } + if match_seconds != MAX_MATCH_SECONDS: + match_kwargs["max_match_seconds"] = match_seconds result = run_match( config_a_name, config_b_name, - run_dir=run_dir, - control_scheme=control_scheme, - fuzz_seed=fuzz_seed, - fuzz_interval_s=fuzz_interval_s, + **match_kwargs, ) _record(result) _analyse(result) @@ -463,7 +482,8 @@ def _analyse(result: MatchResult) -> None: # only user-visible reordering is the interleaved progress log. with ProcessPoolExecutor(max_workers=n_workers) as pool: futures = [ - pool.submit(run_match, a, b, run_dir, control_scheme, fuzz_seed, fuzz_interval_s) for a, b in pairs + pool.submit(run_match, a, b, run_dir, control_scheme, fuzz_seed, fuzz_interval_s, match_seconds) + for a, b in pairs ] for future in as_completed(futures): result = future.result() @@ -607,6 +627,7 @@ def _match_tag_of(r: MatchResult) -> str: _print_chances(summary["strategies"]) summary["fouls"] = foul_table(summary["results"]) _print_foul_table(summary["fouls"]) + _print_motion_planning_table(summary["strategies"]) if run_dir is not None: with open(summary_path, "w") as f: json.dump(summary, f, indent=2) @@ -757,6 +778,12 @@ def strategy_table( "matches_as_a": 0, "real_losses_as_a": 0, "real_loss_kinds_as_a": {}, + "robot_overlap_ticks": 0, + "robot_overlap_incidents": 0, + "teammate_overlap_ticks": 0, + "teammate_overlap_incidents": 0, + "near_miss_ticks": 0, + "worst_clearance_m": None, }, ) prefix = "" if is_a else "enemy_" @@ -770,6 +797,15 @@ def strategy_table( row["attacking_third_entries"] += stats.get(f"{prefix}attacking_third_entries", 0) row["fouls"] += sum((fouls.get(own) or {}).values()) row["stalled"] += bool(stats.get("stall_events") or r.get("possession_backstop")) + row["robot_overlap_ticks"] += stats.get("robot_overlap_ticks", 0) + row["robot_overlap_incidents"] += stats.get("robot_overlap_incidents", 0) + row["teammate_overlap_ticks"] += (stats.get("teammate_overlap_ticks") or {}).get(own, 0) + row["teammate_overlap_incidents"] += (stats.get("teammate_overlap_incidents") or {}).get(own, 0) + row["near_miss_ticks"] += stats.get("near_miss_ticks", 0) + wc = stats.get("worst_clearance_m") + if wc is not None: + if row["worst_clearance_m"] is None or wc < row["worst_clearance_m"]: + row["worst_clearance_m"] = round(wc, 3) progress.setdefault(name, []).extend(stats.get(f"{prefix}pass_progress_m") or []) if chances_by_match and tag in chances_by_match: chances.add(chance_totals.setdefault(name, {}), chances.side_totals(chances_by_match[tag], own)) @@ -924,6 +960,31 @@ def num(v: Optional[float]) -> str: ) +def _print_motion_planning_table(table: dict[str, dict]) -> None: + has_motion = any( + t.get("worst_clearance_m") is not None or t.get("robot_overlap_ticks", 0) > 0 for t in table.values() + ) + if not has_motion: + return + print( + "\nMOTION PLANNING (per match averages; tm_coll = teammate overlap s (incidents), " + "total_coll = all overlap s (incidents), near_miss = near miss s <5cm, min_clr = worst clearance):" + ) + print(f" {'strategy':<32} {'tm_coll':>18} {'total_coll':>20} {'near_miss':>11} {'min_clr':>10}") + for name, t in sorted(table.items(), key=lambda kv: (-kv[1]["wins"], -kv[1]["draws"])): + n = max(1, t["matches"]) + tm_coll_s = (t.get("teammate_overlap_ticks", 0) / n) / TICKS_PER_SECOND + tm_inc = t.get("teammate_overlap_incidents", 0) / n + total_coll_s = (t.get("robot_overlap_ticks", 0) / n) / TICKS_PER_SECOND + tot_inc = t.get("robot_overlap_incidents", 0) / n + near_miss_s = (t.get("near_miss_ticks", 0) / n) / TICKS_PER_SECOND + wc = t.get("worst_clearance_m") + min_clr_str = f"{wc * 100:.1f} cm" if wc is not None else "-" + tm_str = f"{tm_coll_s:.2f}s ({tm_inc:.1f})" + tot_str = f"{total_coll_s:.2f}s ({tot_inc:.1f})" + print(f" {_short_name(name):<32} {tm_str:>18} {tot_str:>20} {near_miss_s:>10.2f}s {min_clr_str:>10}") + + def _print_ball_losses(b: dict, run_id: str) -> None: def top(counts: dict[str, int], k: int = 5) -> str: return ", ".join(f"{name} {c}" for name, c in list(counts.items())[:k]) or "none" diff --git a/tools/motion_planning_benchmark.py b/tools/motion_planning_benchmark.py index 69f5380a..b544bd49 100644 --- a/tools/motion_planning_benchmark.py +++ b/tools/motion_planning_benchmark.py @@ -521,6 +521,86 @@ def effective_collision_distance_m(self) -> float: timeout_s=12.0, endpoint_tolerance_m=0.15, ), + Scenario( + name="box_clearing_scrum", + description=( + "A defender breaks out of a congested penalty area scrum past two pressing attackers" + " and a shuttling marker to reach a flank outlet." + ), + friendly_starts=((-3.0, 0.4), (-3.2, -0.4)), + friendly_targets=((-1.0, 1.8), (-3.2, -0.4)), + enemy_starts=((-2.4, 0.6), (-2.4, -0.2), (-1.8, 0.8)), + enemy_targets=((-2.7, 0.2), (-2.7, -0.3), (-1.8, 0.8)), + enemy_target_trajectories={2: _shuttling((-1.8, 0.8), (-1.8, -0.8), 0.5)}, + timeout_s=15.0, + endpoint_tolerance_m=0.2, + ), + Scenario( + name="wall_rebound", + description=( + "An attacker rounds a 3-robot defensive wall to collect a loose rebound while an" + " opposing defender recovers to contest the lane." + ), + friendly_starts=((-0.5, 0.0),), + friendly_targets=((2.5, -1.0),), + enemy_starts=((1.5, -0.38), (1.5, 0.0), (1.5, 0.38), (1.8, 0.8)), + enemy_targets=((1.5, -0.38), (1.5, 0.0), (1.5, 0.38), (2.5, -0.6)), + timeout_s=15.0, + endpoint_tolerance_m=0.2, + ), + Scenario( + name="pinching_contest", + description=( + "One robot sprints through midfield while two opposing defenders converge from" + " 45-degree diagonal flanks to squeeze the corridor." + ), + friendly_starts=((-2.5, 0.0),), + friendly_targets=((1.5, 0.0),), + enemy_starts=((-0.5, 1.5), (-0.5, -1.5)), + enemy_targets=((1.2, 0.45), (1.2, -0.45)), + timeout_s=12.0, + endpoint_tolerance_m=0.2, + ), + Scenario( + name="stop_transition", + description=( + "Four friendly and two enemy robots disperse from an offensive cluster around a" + " central ball keep-out anchor to legal STOP positions." + ), + friendly_starts=((0.8, 0.7), (0.8, -0.7), (-0.5, 0.6), (-0.5, -0.6)), + friendly_targets=((-1.5, 1.2), (-1.5, -1.2), (-2.2, 0.6), (-2.2, -0.6)), + enemy_starts=((0.0, 0.0), (-0.6, -0.1), (-0.3, 0.3)), + enemy_targets=((0.0, 0.0), (1.5, -0.6), (1.5, 0.6)), + timeout_s=20.0, + endpoint_tolerance_m=0.25, + ), + Scenario( + name="keeper_switchback", + description=( + "A defender executes a sharp hairpin switchback around an advancing pressing attacker" + " to reach an opposite-flank outlet." + ), + friendly_starts=((-2.7, 0.8),), + friendly_targets=((-2.7, -0.8),), + enemy_starts=((-2.3, 0.1), (-1.7, -0.3)), + enemy_targets=((-2.9, 0.6), (-1.7, -0.3)), + timeout_s=12.0, + endpoint_tolerance_m=0.2, + ), + Scenario( + name="trailing_intercept", + description=( + "A defender pursues and cuts off an opponent sprinting down the wing at 1.8 m/s" + " from an oblique trailing position." + ), + friendly_starts=((-0.5, 1.2),), + friendly_targets=((2.5, 2.0),), + enemy_starts=((0.0, 2.0),), + enemy_targets=((3.5, 2.0),), + enemy_target_trajectories={0: lambda t: (min(3.5, 0.0 + 1.8 * t), 2.0)}, + timeout_s=10.0, + endpoint_tolerance_m=0.2, + ), ) } diff --git a/utama_core/analysis/motion_stats.py b/utama_core/analysis/motion_stats.py new file mode 100644 index 00000000..b904b7b3 --- /dev/null +++ b/utama_core/analysis/motion_stats.py @@ -0,0 +1,221 @@ +"""Offline motion planning collision and clearance analysis from replay files (.npz). + +Computes pairwise physical robot overlaps, near-misses, teammate vs opponent +collision breakdowns, and worst observed clearance over a full match replay. +""" + +from __future__ import annotations + +import json +from pathlib import Path +from typing import Dict, Optional, Union + +import numpy as np + +from utama_core.config.physical_constants import ROBOT_RADIUS + + +def _count_incidents(d_pairs: np.ndarray) -> int: + """Count discrete collision incidents across pairs via rising-edge transitions (d < 0.0).""" + if d_pairs.size == 0 or d_pairs.shape[0] == 0: + return 0 + in_contact = d_pairs < 0.0 + starts = in_contact[0:1, :] + trans = in_contact[1:, :] & ~in_contact[:-1, :] + return int(np.sum(starts) + np.sum(trans)) + + +def compute_motion_metrics_from_arrays( + friendly_p: np.ndarray, + enemy_p: np.ndarray, +) -> Dict[str, Union[int, float, Dict[str, int], None]]: + """Compute motion planning collision metrics directly from position arrays. + + Parameters + ---------- + friendly_p : np.ndarray + Shape (T, N_f, 2), friendly robot positions over T ticks. + enemy_p : np.ndarray + Shape (T, N_e, 2), enemy robot positions over T ticks. + + Returns + ------- + dict with: + robot_overlap_ticks : int + robot_overlap_incidents : int + near_miss_ticks : int + teammate_overlap_ticks : dict[str, int] + teammate_overlap_incidents : dict[str, int] + opponent_overlap_ticks : int + opponent_overlap_incidents : int + worst_clearance_m : float or None + total_ticks : int + """ + t_steps = friendly_p.shape[0] + if t_steps == 0 or (friendly_p.shape[1] + enemy_p.shape[1]) < 2: + return { + "robot_overlap_ticks": 0, + "robot_overlap_incidents": 0, + "near_miss_ticks": 0, + "teammate_overlap_ticks": {"friendly": 0, "enemy": 0}, + "teammate_overlap_incidents": {"friendly": 0, "enemy": 0}, + "opponent_overlap_ticks": 0, + "opponent_overlap_incidents": 0, + "worst_clearance_m": None, + "total_ticks": t_steps, + } + + two_r = 2.0 * ROBOT_RADIUS + n_f = friendly_p.shape[1] + n_e = enemy_p.shape[1] + + clearances = [] + + # Friendly teammate clearances + if n_f >= 2: + idx_f = np.triu_indices(n_f, k=1) + diff_ff = friendly_p[:, :, None, :] - friendly_p[:, None, :, :] + d_ff = np.linalg.norm(diff_ff, axis=-1)[:, idx_f[0], idx_f[1]] - two_r + clearances.append(d_ff) + ff_overlap = int(np.sum(np.any(d_ff < 0.0, axis=1))) + ff_incidents = _count_incidents(d_ff) + else: + d_ff = np.empty((t_steps, 0), dtype=np.float32) + ff_overlap = 0 + ff_incidents = 0 + + # Enemy teammate clearances + if n_e >= 2: + idx_e = np.triu_indices(n_e, k=1) + diff_ee = enemy_p[:, :, None, :] - enemy_p[:, None, :, :] + d_ee = np.linalg.norm(diff_ee, axis=-1)[:, idx_e[0], idx_e[1]] - two_r + clearances.append(d_ee) + ee_overlap = int(np.sum(np.any(d_ee < 0.0, axis=1))) + ee_incidents = _count_incidents(d_ee) + else: + d_ee = np.empty((t_steps, 0), dtype=np.float32) + ee_overlap = 0 + ee_incidents = 0 + + # Cross (friendly vs enemy) clearances + if n_f >= 1 and n_e >= 1: + diff_fe = friendly_p[:, :, None, :] - enemy_p[:, None, :, :] + d_fe = (np.linalg.norm(diff_fe, axis=-1) - two_r).reshape(t_steps, -1) + clearances.append(d_fe) + fe_overlap = int(np.sum(np.any(d_fe < 0.0, axis=1))) + fe_incidents = _count_incidents(d_fe) + else: + d_fe = np.empty((t_steps, 0), dtype=np.float32) + fe_overlap = 0 + fe_incidents = 0 + + if not clearances: + return { + "robot_overlap_ticks": 0, + "robot_overlap_incidents": 0, + "near_miss_ticks": 0, + "teammate_overlap_ticks": {"friendly": 0, "enemy": 0}, + "teammate_overlap_incidents": {"friendly": 0, "enemy": 0}, + "opponent_overlap_ticks": 0, + "opponent_overlap_incidents": 0, + "worst_clearance_m": None, + "total_ticks": t_steps, + } + + all_clear = np.concatenate(clearances, axis=1) + overlap_ticks = int(np.sum(np.any(all_clear < 0.0, axis=1))) + near_miss_ticks = int(np.sum(np.any((all_clear >= 0.0) & (all_clear < 0.05), axis=1))) + worst_clearance_m = round(float(np.min(all_clear)), 3) + total_overlap_incidents = ff_incidents + ee_incidents + fe_incidents + + return { + "robot_overlap_ticks": overlap_ticks, + "robot_overlap_incidents": total_overlap_incidents, + "near_miss_ticks": near_miss_ticks, + "teammate_overlap_ticks": {"friendly": ff_overlap, "enemy": ee_overlap}, + "teammate_overlap_incidents": {"friendly": ff_incidents, "enemy": ee_incidents}, + "opponent_overlap_ticks": fe_overlap, + "opponent_overlap_incidents": fe_incidents, + "worst_clearance_m": worst_clearance_m, + "total_ticks": t_steps, + } + + +def compute_motion_metrics_from_replay( + replay_path: Union[str, Path], +) -> Dict[str, Union[int, float, Dict[str, int], None]]: + """Compute motion planning collision metrics from an .npz replay file.""" + path = Path(replay_path) + data = np.load(path) + return compute_motion_metrics_from_arrays(data["friendly_p"], data["enemy_p"]) + + +def enrich_tournament_directory(tournament_dir: Union[str, Path]) -> None: + """Enrich all .stats.json files and summary.json in a tournament directory with motion metrics.""" + run_dir = Path(tournament_dir) + npz_files = [p for p in run_dir.glob("*.npz") if not p.name.endswith(".tmp.npz")] + if not npz_files: + return + + stats_updates = {} + for npz_path in npz_files: + tag = npz_path.stem + stats_path = run_dir / f"{tag}.stats.json" + try: + metrics = compute_motion_metrics_from_replay(npz_path) + except Exception: + continue + stats_updates[tag] = metrics + + if stats_path.exists(): + try: + with open(stats_path) as f: + s_data = json.load(f) + s_data.update( + { + "robot_overlap_ticks": metrics["robot_overlap_ticks"], + "robot_overlap_incidents": metrics["robot_overlap_incidents"], + "near_miss_ticks": metrics["near_miss_ticks"], + "teammate_overlap_ticks": metrics["teammate_overlap_ticks"], + "teammate_overlap_incidents": metrics["teammate_overlap_incidents"], + "opponent_overlap_ticks": metrics["opponent_overlap_ticks"], + "opponent_overlap_incidents": metrics["opponent_overlap_incidents"], + "worst_clearance_m": metrics["worst_clearance_m"], + } + ) + with open(stats_path, "w") as f: + json.dump(s_data, f, indent=2) + except Exception: + pass + + # Update summary.json if present + summary_path = run_dir / "summary.json" + if summary_path.exists(): + try: + with open(summary_path) as f: + summary = json.load(f) + + for result in summary.get("results", []): + cfg_a = result["config_a"].removeprefix("build_").removesuffix("_kernel_strategy") + cfg_b = result["config_b"].removeprefix("build_").removesuffix("_kernel_strategy") + tag = f"{cfg_a}_vs_{cfg_b}" + if tag in stats_updates: + m = stats_updates[tag] + st = result.setdefault("stats", {}) + st["robot_overlap_ticks"] = m["robot_overlap_ticks"] + st["robot_overlap_incidents"] = m["robot_overlap_incidents"] + st["near_miss_ticks"] = m["near_miss_ticks"] + st["teammate_overlap_ticks"] = m["teammate_overlap_ticks"] + st["teammate_overlap_incidents"] = m["teammate_overlap_incidents"] + st["opponent_overlap_ticks"] = m["opponent_overlap_ticks"] + st["opponent_overlap_incidents"] = m["opponent_overlap_incidents"] + st["worst_clearance_m"] = m["worst_clearance_m"] + + # Rebuild strategy_table if possible + from tools.evaluation.round_robin import strategy_table + + summary["strategies"] = strategy_table(summary["results"]) + with open(summary_path, "w") as f: + json.dump(summary, f, indent=2) + except Exception: + pass diff --git a/utama_core/custom_referee/actions.py b/utama_core/custom_referee/actions.py index 43413270..dd31f28a 100644 --- a/utama_core/custom_referee/actions.py +++ b/utama_core/custom_referee/actions.py @@ -362,7 +362,9 @@ def _clear_to_legal_positions( target = _clamp_to_field(target, game) - if target == Vector2D(robot.p.x, robot.p.y): + dx = target.x - robot.p.x + dy = target.y - robot.p.y + if dx * dx + dy * dy < 1e-4: blackboard.cmd_map[robot_id] = empty_command(False) continue diff --git a/utama_core/data_processing/refiners/velocity.py b/utama_core/data_processing/refiners/velocity.py index 10e0e24b..94d7a582 100644 --- a/utama_core/data_processing/refiners/velocity.py +++ b/utama_core/data_processing/refiners/velocity.py @@ -179,11 +179,17 @@ def _calculate_object_velocity( return zero_vector(twod) if twod: - previous_pos = Vector2D(previous_pos_floats[0], previous_pos_floats[1]) + inv_dt = 1.0 / dt_secs + return Vector2D( + (current_pos.x - previous_pos_floats[0]) * inv_dt, (current_pos.y - previous_pos_floats[1]) * inv_dt + ) else: - previous_pos = Vector3D(previous_pos_floats[0], previous_pos_floats[1], previous_pos_floats[2]) - - return (current_pos - previous_pos) / dt_secs + inv_dt = 1.0 / dt_secs + return Vector3D( + (current_pos.x - previous_pos_floats[0]) * inv_dt, + (current_pos.y - previous_pos_floats[1]) * inv_dt, + (current_pos.z - previous_pos_floats[2]) * inv_dt, + ) def _calculate_object_acceleration( self, game_history: GameHistory, object_key: ObjectKey, twod: bool @@ -211,7 +217,7 @@ def _acceleration_from_entries(self, entries: list, twod: bool) -> Union[Vector2 the same order, so the result is the same float; the ball goes through the general helper. """ - if not twod or any(len(vec) != 2 for _, vec in entries): + if not twod: # Plain floats, as `.tolist()` of the stacked arrays gave, without stacking them. return self._windowed_average_derivative( [float(ts) for ts, _ in entries], @@ -223,13 +229,23 @@ def _acceleration_from_entries(self, entries: list, twod: bool) -> Union[Vector2 ) window = self.ACCELERATION_WINDOW_SIZE averages = [] # (ts, vx, vy) per window - for start in range(len(entries) - self.ACCELERATION_N_WINDOWS * window, len(entries), window): - ts_sum = vx_sum = vy_sum = 0.0 - for ts, (vx, vy) in entries[start : start + window]: - ts_sum += float(ts) - vx_sum += vx - vy_sum += vy - averages.append((ts_sum / window, vx_sum / window, vy_sum / window)) + try: + for start in range(len(entries) - self.ACCELERATION_N_WINDOWS * window, len(entries), window): + ts_sum = vx_sum = vy_sum = 0.0 + for ts, (vx, vy) in entries[start : start + window]: + ts_sum += float(ts) + vx_sum += vx + vy_sum += vy + averages.append((ts_sum / window, vx_sum / window, vy_sum / window)) + except (ValueError, TypeError): + return self._windowed_average_derivative( + [float(ts) for ts, _ in entries], + [vec for _, vec in entries], + self.ACCELERATION_N_WINDOWS, + self.ACCELERATION_WINDOW_SIZE, + twod, + log_prefix="ACCEL", + ) ax = ay = 0.0 valid_segments = 0 diff --git a/utama_core/engine/abstract_strategy.py b/utama_core/engine/abstract_strategy.py index 895c3a53..0da430e9 100644 --- a/utama_core/engine/abstract_strategy.py +++ b/utama_core/engine/abstract_strategy.py @@ -82,20 +82,32 @@ class AbstractStrategy: `Strategy.referee_overrides`'s setter docstring for why this is a post-construction assignment rather than a constructor kwarg on `build_kernel_strategy` itself). + control_scheme: optional name of the preferred motion control scheme to + use (e.g. 'trajsample', 'fpp', 'dwa', 'pid'). If omitted, defaults to + class attribute `control_scheme` or `build_kernel_strategy.control_scheme`, + falling back to StrategyRunner's default ("fpp"). """ + control_scheme: Optional[str] = None + def __init__( self, build_kernel_strategy, goalkeeper_id: int = 0, exp_ball: bool = True, referee_overrides: Optional[dict[RefereeCommand, RefereeActionOverride]] = None, + control_scheme: Optional[str] = None, ): self.exp_ball = exp_ball self._build_kernel_strategy = build_kernel_strategy self._kernel_strategy: Optional[KernelSchedulerStrategy] = None self._goalkeeper_id = goalkeeper_id self._referee_overrides = referee_overrides + self.control_scheme = ( + control_scheme + if control_scheme is not None + else getattr(self, "control_scheme", None) or getattr(build_kernel_strategy, "control_scheme", None) + ) ### These attributes are set by the StrategyRunner before the strategy is run. ### self.robot_controller: Optional[AbstractRobotController] = None diff --git a/utama_core/engine/match_stats.py b/utama_core/engine/match_stats.py index 7f555088..d6c453ad 100644 --- a/utama_core/engine/match_stats.py +++ b/utama_core/engine/match_stats.py @@ -328,6 +328,20 @@ class MatchStats: # `rule_event_counts` split by the side that committed it; see `_offending_sides`. fouls_by_side: Dict[str, Dict[str, int]] = field(default_factory=lambda: {"friendly": {}, "enemy": {}}) fouls: List[FoulEvent] = field(default_factory=list) + # Motion planning performance metrics: + # robot_overlap_ticks: ticks where any two robot bodies physically overlap (< 0m clearance). + # near_miss_ticks: ticks where minimum clearance between any two robots is between 0 and 5cm. + # teammate_overlap_ticks: ticks with overlaps between robots of the same team ("friendly" or "enemy"). + # opponent_overlap_ticks: ticks with overlaps between friendly and enemy robots. + # worst_clearance_m: minimum edge-to-edge separation observed over the match (None if <2 robots). + robot_overlap_ticks: int = 0 + robot_overlap_incidents: int = 0 + near_miss_ticks: int = 0 + teammate_overlap_ticks: Dict[str, int] = field(default_factory=lambda: {"friendly": 0, "enemy": 0}) + teammate_overlap_incidents: Dict[str, int] = field(default_factory=lambda: {"friendly": 0, "enemy": 0}) + opponent_overlap_ticks: int = 0 + opponent_overlap_incidents: int = 0 + worst_clearance_m: Optional[float] = None def to_json(self, path: Union[str, Path]) -> None: with open(path, "w") as f: @@ -355,6 +369,14 @@ def to_json(self, path: Union[str, Path]) -> None: "n_restarts_with_entry": self.n_restarts_with_entry, "fouls_by_side": self.fouls_by_side, "fouls": [e.to_dict() for e in self.fouls], + "robot_overlap_ticks": self.robot_overlap_ticks, + "robot_overlap_incidents": self.robot_overlap_incidents, + "near_miss_ticks": self.near_miss_ticks, + "teammate_overlap_ticks": self.teammate_overlap_ticks, + "teammate_overlap_incidents": self.teammate_overlap_incidents, + "opponent_overlap_ticks": self.opponent_overlap_ticks, + "opponent_overlap_incidents": self.opponent_overlap_incidents, + "worst_clearance_m": self.worst_clearance_m, "stall_events": [ { "kind": e.kind, @@ -534,6 +556,17 @@ class MatchStatsAccumulator: _no_progress_logged: bool = False _no_progress_event_idx: Optional[int] = None + # --- Motion planning collision / clearance tracking --- + _robot_overlap_ticks: int = 0 + _robot_overlap_incidents: int = 0 + _near_miss_ticks: int = 0 + _teammate_overlap_ticks: Dict[str, int] = field(default_factory=lambda: {"friendly": 0, "enemy": 0}) + _teammate_overlap_incidents: Dict[str, int] = field(default_factory=lambda: {"friendly": 0, "enemy": 0}) + _opponent_overlap_ticks: int = 0 + _opponent_overlap_incidents: int = 0 + _active_overlap_pairs: set = field(default_factory=set) + _worst_clearance_m: float = math.inf + def record_rule_violation( self, violation: Optional[RuleViolation], @@ -623,6 +656,63 @@ def record_tick( self._ticks_recorded += 1 + # Motion planning collision / clearance tracking (pairwise robot separations) + if len(all_robots) >= 2: + two_r = 2 * ROBOT_RADIUS + has_overlap = False + has_near_miss = False + friendly_teammate = False + enemy_teammate = False + opp_overlap = False + min_c = math.inf + current_overlap_pairs = set() + + n_rob = len(all_robots) + for i in range(n_rob): + _, r1, side1 = all_robots[i] + p1 = r1.p + for j in range(i + 1, n_rob): + _, r2, side2 = all_robots[j] + dist = math.hypot(p1.x - r2.p.x, p1.y - r2.p.y) + clearance = dist - two_r + if clearance < min_c: + min_c = clearance + if clearance < 0.0: + has_overlap = True + current_overlap_pairs.add(((side1, r1.id), (side2, r2.id))) + if side1 == side2: + if side1 == "friendly": + friendly_teammate = True + else: + enemy_teammate = True + else: + opp_overlap = True + elif clearance < 0.05: + has_near_miss = True + + new_overlaps = current_overlap_pairs - self._active_overlap_pairs + if new_overlaps: + for (s1, _), (s2, _) in new_overlaps: + self._robot_overlap_incidents += 1 + if s1 == s2: + self._teammate_overlap_incidents[s1] += 1 + else: + self._opponent_overlap_incidents += 1 + self._active_overlap_pairs = current_overlap_pairs + + if min_c < self._worst_clearance_m: + self._worst_clearance_m = min_c + if has_overlap: + self._robot_overlap_ticks += 1 + if has_near_miss: + self._near_miss_ticks += 1 + if friendly_teammate: + self._teammate_overlap_ticks["friendly"] += 1 + if enemy_teammate: + self._teammate_overlap_ticks["enemy"] += 1 + if opp_overlap: + self._opponent_overlap_ticks += 1 + # Ball travel, skipping placement/teleport jumps. ball_xy = (ball.p.x, ball.p.y) if self._last_ball_xy is not None: @@ -1129,4 +1219,12 @@ def finalize(self) -> MatchStats: n_restarts_with_entry=self._n_restarts_with_entry, fouls_by_side={side: dict(c) for side, c in self._fouls_by_side.items()}, fouls=list(self._fouls), + robot_overlap_ticks=self._robot_overlap_ticks, + robot_overlap_incidents=self._robot_overlap_incidents, + near_miss_ticks=self._near_miss_ticks, + teammate_overlap_ticks=dict(self._teammate_overlap_ticks), + teammate_overlap_incidents=dict(self._teammate_overlap_incidents), + opponent_overlap_ticks=self._opponent_overlap_ticks, + opponent_overlap_incidents=self._opponent_overlap_incidents, + worst_clearance_m=round(self._worst_clearance_m, 3) if self._worst_clearance_m != math.inf else None, ) diff --git a/utama_core/entities/game/game_history.py b/utama_core/entities/game/game_history.py index 1ba317f4..5b85e701 100644 --- a/utama_core/entities/game/game_history.py +++ b/utama_core/entities/game/game_history.py @@ -102,7 +102,12 @@ def get_historical_attribute_entries( if not history_deque: # Handles both key not found or empty deque return [] n = len(history_deque) - return list(islice(history_deque, max(0, n - num_points), n)) + if num_points == 1: + return [history_deque[-1]] + if num_points >= n: + return list(history_deque) + # Direct list slicing on recent elements + return [history_deque[i] for i in range(n - num_points, n)] def get_historical_attribute_series( self, diff --git a/utama_core/motion_planning/src/fastpathplanning/cpp/CMakeLists.txt b/utama_core/motion_planning/src/fastpathplanning/cpp/CMakeLists.txt new file mode 100644 index 00000000..1faadcee --- /dev/null +++ b/utama_core/motion_planning/src/fastpathplanning/cpp/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.14) +project(fastpathplanner CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -march=native -DNDEBUG -fPIC") + +include(FetchContent) +FetchContent_Declare( + pybind11 + URL https://github.com/pybind/pybind11/archive/refs/tags/v2.13.6.tar.gz + URL_HASH SHA256=e08cb87f4773da97fa7b5f035de8763abc656d87d5773e62f6da0587d1f0ec20 +) +FetchContent_MakeAvailable(pybind11) + +pybind11_add_module(_fastpathplanner MODULE planner.cpp) + +set_target_properties(_fastpathplanner PROPERTIES + LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/.." +) diff --git a/utama_core/motion_planning/src/fastpathplanning/cpp/planner.cpp b/utama_core/motion_planning/src/fastpathplanning/cpp/planner.cpp new file mode 100644 index 00000000..6a83aa63 --- /dev/null +++ b/utama_core/motion_planning/src/fastpathplanning/cpp/planner.cpp @@ -0,0 +1,808 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +constexpr double EPS = 1e-9; + +// ============================================================================ +// Core Geometric Helpers (identical to numba_kernels.py formulas) +// ============================================================================ + +inline double distance_point_to_segment(double px, double py, double sx, double sy, double ex, double ey) { + double seg_dx = ex - sx; + double seg_dy = ey - sy; + double pt_dx = px - sx; + double pt_dy = py - sy; + double seg_len_sq = seg_dx * seg_dx + seg_dy * seg_dy; + if (seg_len_sq < EPS) { + return std::hypot(px - sx, py - sy); + } + double t = (pt_dx * seg_dx + pt_dy * seg_dy) / seg_len_sq; + double cx, cy; + if (t < 0.0) { + cx = sx; + cy = sy; + } else if (t > 1.0) { + cx = ex; + cy = ey; + } else { + cx = sx + t * seg_dx; + cy = sy + t * seg_dy; + } + return std::hypot(px - cx, py - cy); +} + +inline std::pair closest_point_on_segment_xy( + double px, double py, double sx, double sy, double ex, double ey +) { + double seg_dx = ex - sx; + double seg_dy = ey - sy; + double seg_len_sq = seg_dx * seg_dx + seg_dy * seg_dy; + if (seg_len_sq < EPS) { + return {sx, sy}; + } + double t = ((px - sx) * seg_dx + (py - sy) * seg_dy) / seg_len_sq; + if (t < 0.0) { + return {sx, sy}; + } else if (t > 1.0) { + return {ex, ey}; + } + return {sx + t * seg_dx, sy + t * seg_dy}; +} + +inline int point_orientation(double p1x, double p1y, double p2x, double p2y, double p3x, double p3y) { + double val = (p2x - p1x) * (p3y - p1y) - (p2y - p1y) * (p3x - p1x); + if (std::abs(val) < EPS) return 0; + return (val < 0.0) ? 1 : 2; +} + +inline bool on_segment(double px, double py, double qx, double qy, double rx, double ry) { + return std::min(px, rx) - EPS <= qx && qx <= std::max(px, rx) + EPS && + std::min(py, ry) - EPS <= qy && qy <= std::max(py, ry) + EPS; +} + +inline bool segments_intersect( + double p1x, double p1y, double q1x, double q1y, + double p2x, double p2y, double q2x, double q2y +) { + int o1 = point_orientation(p1x, p1y, q1x, q1y, p2x, p2y); + int o2 = point_orientation(p1x, p1y, q1x, q1y, q2x, q2y); + int o3 = point_orientation(p2x, p2y, q2x, q2y, p1x, p1y); + int o4 = point_orientation(p2x, p2y, q2x, q2y, q1x, q1y); + + if (o1 != o2 && o3 != o4) return true; + if (o1 == 0 && on_segment(p1x, p1y, p2x, p2y, q1x, q1y)) return true; + if (o2 == 0 && on_segment(p1x, p1y, q2x, q2y, q1x, q1y)) return true; + if (o3 == 0 && on_segment(p2x, p2y, p1x, p1y, q2x, q2y)) return true; + if (o4 == 0 && on_segment(p2x, p2y, q1x, q1y, q2x, q2y)) return true; + return false; +} + +inline std::pair distance_between_segments( + double ax0, double ay0, double ax1, double ay1, + double bx0, double by0, double bx1, double by1 +) { + double d3 = distance_point_to_segment(bx0, by0, ax0, ay0, ax1, ay1); + if (segments_intersect(ax0, ay0, ax1, ay1, bx0, by0, bx1, by1)) { + return {0.0, d3}; + } + double d1 = distance_point_to_segment(ax0, ay0, bx0, by0, bx1, by1); + double d2 = distance_point_to_segment(ax1, ay1, bx0, by0, bx1, by1); + double d4 = distance_point_to_segment(bx1, by1, ax0, ay0, ax1, ay1); + return {std::min({d1, d2, d3, d4}), d3}; +} + +// ============================================================================ +// Obstacle Scanners & Kernels +// ============================================================================ + +inline std::tuple scan_collides( + double seg_x0, double seg_y0, double seg_x1, double seg_y1, + double clearance, + const double* ox0, const double* oy0, const double* ox1, const double* oy1, size_t n, + int sticky_idx +) { + double seg_min_x = std::min(seg_x0, seg_x1) - clearance; + double seg_max_x = std::max(seg_x0, seg_x1) + clearance; + double seg_min_y = std::min(seg_y0, seg_y1) - clearance; + double seg_max_y = std::max(seg_y0, seg_y1) + clearance; + + int closest_idx = -1; + double min_dist_to_robot = 1e18; + double sticky_dist_to_robot = -1.0; + + for (size_t i = 0; i < n; ++i) { + double ox0i = ox0[i], oy0i = oy0[i], ox1i = ox1[i], oy1i = oy1[i]; + double o_min_x = std::min(ox0i, ox1i); + double o_max_x = std::max(ox0i, ox1i); + if (o_max_x < seg_min_x || o_min_x > seg_max_x) continue; + double o_min_y = std::min(oy0i, oy1i); + double o_max_y = std::max(oy0i, oy1i); + if (o_max_y < seg_min_y || o_min_y > seg_max_y) continue; + + auto [dist_between_segs, dist_to_robot] = distance_between_segments( + ox0i, oy0i, ox1i, oy1i, seg_x0, seg_y0, seg_x1, seg_y1 + ); + if (dist_between_segs < clearance) { + if (static_cast(i) == sticky_idx) { + sticky_dist_to_robot = dist_to_robot; + } + if (dist_to_robot < min_dist_to_robot) { + min_dist_to_robot = dist_to_robot; + closest_idx = static_cast(i); + } + } + } + return {closest_idx, min_dist_to_robot, sticky_dist_to_robot}; +} + +inline std::tuple collides( + double ax, double ay, double bx, double by, + double clearance, + const double* ox0, const double* oy0, const double* ox1, const double* oy1, size_t n, + int sticky_idx, double switch_margin +) { + auto [closest_idx, min_dist, sticky_dist] = scan_collides( + ax, ay, bx, by, clearance, ox0, oy0, ox1, oy1, n, sticky_idx + ); + if (closest_idx < 0) { + return {-1, false, 0.0, 0.0}; + } + int idx = closest_idx; + bool from_sticky = false; + if (sticky_idx >= 0 && sticky_dist >= 0.0 && + !(ox0[closest_idx] == ox0[sticky_idx] && oy0[closest_idx] == oy0[sticky_idx] && + ox1[closest_idx] == ox1[sticky_idx] && oy1[closest_idx] == oy1[sticky_idx]) && + sticky_dist <= min_dist + switch_margin) { + idx = sticky_idx; + from_sticky = true; + } + + double cx = ox0[idx], cy = oy0[idx], dx = ox1[idx], dy = oy1[idx]; + double denom = (bx - ax) * (dy - cy) - (by - ay) * (dx - cx); + if (std::abs(denom) >= EPS) { + double t = ((cx - ax) * (dy - cy) - (cy - ay) * (dx - cx)) / denom; + double u = ((cx - ax) * (by - ay) - (cy - ay) * (bx - ax)) / denom; + if (-EPS <= t && t <= 1.0 + EPS && -EPS <= u && u <= 1.0 + EPS) { + return {idx, from_sticky, ax + t * (bx - ax), ay + t * (by - ay)}; + } + } + + auto [pcx, pcy] = closest_point_on_segment_xy(ax, ay, cx, cy, dx, dy); + auto [pdx, pdy] = closest_point_on_segment_xy(bx, by, cx, cy, dx, dy); + double best_x = cx, best_y = cy; + double best = distance_point_to_segment(cx, cy, ax, ay, bx, by); + double d = distance_point_to_segment(dx, dy, ax, ay, bx, by); + if (d < best) { + best = d; + best_x = dx; + best_y = dy; + } + d = std::hypot(pcx - ax, pcy - ay); + if (d < best) { + best = d; + best_x = pcx; + best_y = pcy; + } + d = std::hypot(pdx - bx, pdy - by); + if (d < best) { + best = d; + best_x = pdx; + best_y = pdy; + } + return {idx, from_sticky, best_x, best_y}; +} + +inline std::tuple find_subgoal( + double opx, double opy, double ux, double uy, + double subgoal_distance, int multiple, double clearance, + const double* ox0, const double* oy0, const double* ox1, const double* oy1, size_t n, + bool has_rect, double rmin_x, double rmax_x, double rmin_y, double rmax_y +) { + int last_hit = -1; + bool rect_last = false; + while (multiple <= 10) { + double sub_x = opx + subgoal_distance * ux * multiple; + double sub_y = opy + subgoal_distance * uy * multiple; + if (has_rect && rmin_x < sub_x && sub_x < rmax_x && rmin_y < sub_y && sub_y < rmax_y) { + multiple += 1; + rect_last = true; + last_hit = -1; + continue; + } + double box_min_x = sub_x - clearance; + double box_max_x = sub_x + clearance; + double box_min_y = sub_y - clearance; + double box_max_y = sub_y + clearance; + int hit = -1; + for (size_t i = 0; i < n; ++i) { + double ox0i = ox0[i], oy0i = oy0[i], ox1i = ox1[i], oy1i = oy1[i]; + if (std::max(ox0i, ox1i) < box_min_x || std::min(ox0i, ox1i) > box_max_x) continue; + if (std::max(oy0i, oy1i) < box_min_y || std::min(oy0i, oy1i) > box_max_y) continue; + if (distance_point_to_segment(sub_x, sub_y, ox0i, oy0i, ox1i, oy1i) < clearance) { + hit = static_cast(i); + break; + } + } + if (hit >= 0) { + multiple += 1; + rect_last = false; + last_hit = hit; + continue; + } + return {true, sub_x, sub_y, -1, false}; + } + return {false, 0.0, 0.0, last_hit, rect_last}; +} + +inline double clamp_to_obstacle_clearance( + double ox, double oy, double ux, double uy, double max_distance, double clearance, + const double* ox0, const double* oy0, const double* ox1, const double* oy1, size_t n +) { + double clamped = max_distance; + for (size_t i = 0; i < n; ++i) { + if (distance_point_to_segment(ox + ux * clamped, oy + uy * clamped, ox0[i], oy0[i], ox1[i], oy1[i]) >= clearance) { + continue; + } + double lo = 0.0, hi = clamped; + for (int step = 0; step < 12; ++step) { + double mid = (lo + hi) / 2.0; + if (distance_point_to_segment(ox + ux * mid, oy + uy * mid, ox0[i], oy0[i], ox1[i], oy1[i]) >= clearance) { + lo = mid; + } else { + hi = mid; + } + } + clamped = std::min(clamped, lo); + } + return clamped; +} + +inline std::pair sanitize_target( + double tx, double ty, double rx, double ry, double clearance, + const double* ox0, const double* oy0, const double* ox1, const double* oy1, size_t n, + const double* exempt, size_t n_exempt +) { + std::vector repelling(n, true); + for (size_t i = 0; i < n; ++i) { + for (size_t j = 0; j < n_exempt; ++j) { + if (ox0[i] == exempt[j * 4 + 0] && oy0[i] == exempt[j * 4 + 1] && + ox1[i] == exempt[j * 4 + 2] && oy1[i] == exempt[j * 4 + 3]) { + repelling[i] = false; + break; + } + } + } + double push = clearance * 1.05; + for (int pass = 0; pass < 5; ++pass) { + bool collision_found = false; + for (size_t i = 0; i < n; ++i) { + if (!repelling[i]) continue; + if (distance_point_to_segment(tx, ty, ox0[i], oy0[i], ox1[i], oy1[i]) < clearance) { + auto [cx, cy] = closest_point_on_segment_xy(tx, ty, ox0[i], oy0[i], ox1[i], oy1[i]); + double px = tx - cx; + double py = ty - cy; + double norm = std::hypot(px, py); + if (norm == 0.0) { + px = rx - cx; + py = ry - cy; + norm = std::hypot(px, py); + } + tx = cx + (px / norm) * push; + ty = cy + (py / norm) * push; + collision_found = true; + } + } + if (!collision_found) break; + } + return {tx, ty}; +} + +// ============================================================================ +// check_segment and smooth_path C++ implementations +// ============================================================================ + +struct Point { + double x, y; +}; + +struct Segment { + Point a, b; +}; + +inline double round_to(double val, int precision) { + double factor = std::pow(10.0, precision); + return std::round(val * factor) / factor; +} + +void check_segment_recursive( + Point seg_a, Point seg_b, + int recursion_length, int max_recursion_length, + Point target, + double fb_min_x, double fb_max_x, double fb_min_y, double fb_max_y, + int robot_id, + double clearance, double subgoal_distance, + bool has_rect, double rmin_x, double rmax_x, double rmin_y, double rmax_y, + double detour_switch_margin, double moving_ghost_min_m, int precision, + const double* ox0, const double* oy0, const double* ox1, const double* oy1, size_t n, + const bool* is_static, + py::dict last_detour_side, + py::dict last_obstacle, + std::vector& out_trajectory +) { + double seg_len = std::hypot(seg_b.x - seg_a.x, seg_b.y - seg_a.y); + int sticky_idx = -1; + py::tuple obstacle_mem_key; + if (robot_id >= 0) { + obstacle_mem_key = py::make_tuple(robot_id, recursion_length); + if (last_obstacle.contains(obstacle_mem_key)) { + py::object obs_seg_obj = last_obstacle[obstacle_mem_key]; + if (py::isinstance(obs_seg_obj)) { + py::tuple t = obs_seg_obj.cast(); + if (t.size() == 2) { + py::sequence pt_a = t[0].cast(); + py::sequence pt_b = t[1].cast(); + double sx0 = pt_a[0].cast(); + double sy0 = pt_a[1].cast(); + double sx1 = pt_b[0].cast(); + double sy1 = pt_b[1].cast(); + for (size_t i = 0; i < n; ++i) { + if (ox0[i] == sx0 && oy0[i] == sy0 && ox1[i] == sx1 && oy1[i] == sy1) { + sticky_idx = static_cast(i); + break; + } + } + } + } + } + } + + auto [closest_idx, from_sticky, px, py] = collides( + seg_a.x, seg_a.y, seg_b.x, seg_b.y, + clearance, ox0, oy0, ox1, oy1, n, + sticky_idx, detour_switch_margin + ); + + if (closest_idx < 0 || recursion_length >= max_recursion_length) { + if (robot_id >= 0 && last_obstacle.contains(obstacle_mem_key)) { + py::dict(last_obstacle).attr("pop")(obstacle_mem_key, py::none()); + } + out_trajectory.push_back({seg_a, seg_b}); + return; + } + + // Save sticky obstacle segment into last_obstacle + if (robot_id >= 0) { + py::array_t pt0(2); + pt0.mutable_at(0) = ox0[closest_idx]; + pt0.mutable_at(1) = oy0[closest_idx]; + py::array_t pt1(2); + pt1.mutable_at(0) = ox1[closest_idx]; + pt1.mutable_at(1) = oy1[closest_idx]; + last_obstacle[obstacle_mem_key] = py::make_tuple(pt0, pt1); + } + + // Direction vector from seg_a to seg_b + double dx = seg_b.x - seg_a.x; + double dy = seg_b.y - seg_a.y; + double dir_norm = std::hypot(dx, dy); + + Point subgoal_left_pt{px, py}; + Point subgoal_right_pt{px, py}; + bool left_valid = false; + bool right_valid = 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( + 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 + ); + if (found_l) { + subgoal_left_pt = {sl_x, sl_y}; + left_valid = true; + } else { + bool blocked = rect_last_l || (last_hit_l == closest_idx); + if (!blocked) { + subgoal_left_pt = {px, py}; + left_valid = true; + } + } + + // 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( + 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 + ); + if (found_r) { + subgoal_right_pt = {sr_x, sr_y}; + right_valid = true; + } else { + bool blocked = rect_last_r || (last_hit_r == closest_idx); + if (!blocked) { + subgoal_right_pt = {px, py}; + right_valid = true; + } + } + } + + if (left_valid) { + left_valid = (fb_min_x <= subgoal_left_pt.x && subgoal_left_pt.x <= fb_max_x && + fb_min_y <= subgoal_left_pt.y && subgoal_left_pt.y <= fb_max_y); + } + if (right_valid) { + right_valid = (fb_min_x <= subgoal_right_pt.x && subgoal_right_pt.x <= fb_max_x && + fb_min_y <= subgoal_right_pt.y && subgoal_right_pt.y <= fb_max_y); + } + + Point best_subgoal; + int chosen_side = -1; + + 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)); + if (last_detour_side.contains(memory_key)) { + prev_side = last_detour_side[memory_key].cast(); + } + } + + if (left_valid && right_valid) { + double dist_left = std::hypot(subgoal_left_pt.x - target.x, subgoal_left_pt.y - target.y); + double dist_right = std::hypot(subgoal_right_pt.x - target.x, subgoal_right_pt.y - target.y); + + double ghost_dx = ox1[closest_idx] - ox0[closest_idx]; + double ghost_dy = oy1[closest_idx] - oy0[closest_idx]; + double ghost_len = std::hypot(ghost_dx, ghost_dy); + + if (ghost_len > moving_ghost_min_m && (!is_static || !is_static[closest_idx])) { + double along_left = ((subgoal_left_pt.x - ox0[closest_idx]) * ghost_dx + + (subgoal_left_pt.y - oy0[closest_idx]) * ghost_dy) / ghost_len; + double along_right = ((subgoal_right_pt.x - ox0[closest_idx]) * ghost_dx + + (subgoal_right_pt.y - oy0[closest_idx]) * ghost_dy) / ghost_len; + if (std::abs(along_left - along_right) > moving_ghost_min_m) { + if (along_left < along_right) { + dist_left = 0.0; + dist_right = 1.0; + } else { + dist_left = 1.0; + dist_right = 0.0; + } + } + } + + if (prev_side.has_value() && prev_side.value() == 1 && dist_left <= dist_right + detour_switch_margin) { + best_subgoal = subgoal_left_pt; + chosen_side = 1; + } else if (prev_side.has_value() && prev_side.value() == 0 && dist_right <= dist_left + detour_switch_margin) { + best_subgoal = subgoal_right_pt; + chosen_side = 0; + } else if (dist_left < dist_right) { + best_subgoal = subgoal_left_pt; + chosen_side = 1; + } else { + best_subgoal = subgoal_right_pt; + chosen_side = 0; + } + } else if (left_valid) { + best_subgoal = subgoal_left_pt; + chosen_side = 1; + } else if (right_valid) { + best_subgoal = subgoal_right_pt; + chosen_side = 0; + } else { + out_trajectory.push_back({seg_a, seg_b}); + return; + } + + if (robot_id >= 0 && !memory_key.is_none()) { + last_detour_side[memory_key] = chosen_side; + } + + check_segment_recursive( + seg_a, best_subgoal, recursion_length + 1, max_recursion_length, + target, fb_min_x, fb_max_x, fb_min_y, fb_max_y, robot_id, + clearance, subgoal_distance, has_rect, rmin_x, rmax_x, rmin_y, rmax_y, + detour_switch_margin, moving_ghost_min_m, precision, + ox0, oy0, ox1, oy1, n, is_static, last_detour_side, last_obstacle, + out_trajectory + ); + check_segment_recursive( + best_subgoal, seg_b, recursion_length + 1, max_recursion_length, + target, fb_min_x, fb_max_x, fb_min_y, fb_max_y, robot_id, + clearance, subgoal_distance, has_rect, rmin_x, rmax_x, rmin_y, rmax_y, + detour_switch_margin, moving_ghost_min_m, precision, + ox0, oy0, ox1, oy1, n, is_static, last_detour_side, last_obstacle, + out_trajectory + ); +} + +Point smooth_path_cpp( + const std::vector& trajectory, + Point target, Point robot_pos, + double projection_distance, double clearance, + const double* ox0, const double* oy0, const double* ox1, const double* oy1, size_t n +) { + size_t start_idx = 0; + while (trajectory.size() - start_idx > 1 && + std::hypot(trajectory[start_idx].b.x - robot_pos.x, trajectory[start_idx].b.y - robot_pos.y) == 0.0) { + start_idx++; + } + size_t remaining = trajectory.size() - start_idx; + if (remaining <= 1) { + return target; + } + + Point next_pt = trajectory[start_idx].b; + double dir_x = next_pt.x - robot_pos.x; + double dir_y = next_pt.y - robot_pos.y; + double norm = std::hypot(dir_x, dir_y); + double unit_x = dir_x / norm; + double unit_y = dir_y / norm; + + double safe_dist = clamp_to_obstacle_clearance( + robot_pos.x, robot_pos.y, unit_x, unit_y, projection_distance, clearance, + ox0, oy0, ox1, oy1, n + ); + Point new_target{robot_pos.x + unit_x * safe_dist, robot_pos.y + unit_y * safe_dist}; + + double dist_new_target = std::hypot(new_target.x - robot_pos.x, new_target.y - robot_pos.y); + double dist_trajectory = std::hypot(robot_pos.x - next_pt.x, robot_pos.y - next_pt.y); + + if (dist_new_target < dist_trajectory) { + return next_pt; + } else { + Point p1_0 = trajectory[start_idx + 1].a; + Point p1_1 = trajectory[start_idx + 1].b; + auto [cx, cy] = closest_point_on_segment_xy(new_target.x, new_target.y, p1_0.x, p1_0.y, p1_1.x, p1_1.y); + return {(cx + new_target.x) / 2.0, (cy + new_target.y) / 2.0}; + } +} + +// ============================================================================ +// Pybind11 Module Wrapping +// ============================================================================ + +PYBIND11_MODULE(_fastpathplanner, m) { + m.doc() = "FastPathPlanner C++ native accelerated geometry core"; + + m.def("collides_nb", []( + double ax, double ay, double bx, double by, + double clearance, + py::array_t ox0, + py::array_t oy0, + py::array_t ox1, + py::array_t oy1, + int sticky_idx, double switch_margin + ) { + size_t n = ox0.size(); + const double* d_ox0 = ox0.data(); + const double* d_oy0 = oy0.data(); + const double* d_ox1 = ox1.data(); + const double* d_oy1 = oy1.data(); + int idx; + bool from_sticky; + double px, py; + { + py::gil_scoped_release release; + auto res = collides( + ax, ay, bx, by, clearance, + d_ox0, d_oy0, d_ox1, d_oy1, n, + sticky_idx, switch_margin + ); + idx = std::get<0>(res); + from_sticky = std::get<1>(res); + px = std::get<2>(res); + py = std::get<3>(res); + } + return py::make_tuple(idx, from_sticky, px, py); + }); + + m.def("find_subgoal_nb", []( + double opx, double opy, double ux, double uy, + double subgoal_distance, int multiple, double clearance, + py::array_t ox0, + py::array_t oy0, + py::array_t ox1, + py::array_t oy1, + bool has_rect, double rmin_x, double rmax_x, double rmin_y, double rmax_y + ) { + size_t n = ox0.size(); + const double* d_ox0 = ox0.data(); + const double* d_oy0 = oy0.data(); + const double* d_ox1 = ox1.data(); + const double* d_oy1 = oy1.data(); + bool found; + double sx, sy; + int last_hit; + bool rect_last; + { + py::gil_scoped_release release; + auto res = find_subgoal( + opx, opy, ux, uy, subgoal_distance, multiple, clearance, + d_ox0, d_oy0, d_ox1, d_oy1, n, + has_rect, rmin_x, rmax_x, rmin_y, rmax_y + ); + found = std::get<0>(res); + sx = std::get<1>(res); + sy = std::get<2>(res); + last_hit = std::get<3>(res); + rect_last = std::get<4>(res); + } + return py::make_tuple(found, sx, sy, last_hit, rect_last); + }); + + m.def("clamp_to_obstacle_clearance_nb", []( + double ox, double oy, double ux, double uy, double max_distance, double clearance, + py::array_t ox0, + py::array_t oy0, + py::array_t ox1, + py::array_t oy1 + ) { + size_t n = ox0.size(); + const double* d_ox0 = ox0.data(); + const double* d_oy0 = oy0.data(); + const double* d_ox1 = ox1.data(); + const double* d_oy1 = oy1.data(); + double res; + { + py::gil_scoped_release release; + res = clamp_to_obstacle_clearance( + ox, oy, ux, uy, max_distance, clearance, + d_ox0, d_oy0, d_ox1, d_oy1, n + ); + } + return res; + }); + + m.def("sanitize_target_nb", []( + double tx, double ty, double rx, double ry, double clearance, + py::array_t ox0, + py::array_t oy0, + py::array_t ox1, + py::array_t oy1, + py::array_t exempt + ) { + size_t n = ox0.size(); + size_t n_exempt = exempt.size() / 4; + const double* d_ox0 = ox0.data(); + const double* d_oy0 = oy0.data(); + const double* d_ox1 = ox1.data(); + const double* d_oy1 = oy1.data(); + const double* d_ex = exempt.data(); + double stx, sty; + { + py::gil_scoped_release release; + auto res = sanitize_target( + tx, ty, rx, ry, clearance, + d_ox0, d_oy0, d_ox1, d_oy1, n, + d_ex, n_exempt + ); + stx = res.first; + sty = res.second; + } + return py::make_tuple(stx, sty); + }); + + m.def("check_segment_cpp", []( + py::sequence segment, + py::array_t ox0, + py::array_t oy0, + py::array_t ox1, + py::array_t oy1, + std::optional> is_static_opt, + int recursion_length, int max_recursion_length, + py::sequence target_seq, + py::sequence bounds_seq, + std::optional robot_id_opt, + double clearance, double subgoal_distance, + std::optional forbidden_rect_opt, + double detour_switch_margin, double moving_ghost_min_m, int precision, + py::dict last_detour_side, + py::dict last_obstacle + ) { + py::sequence p_a = segment[0].cast(); + py::sequence p_b = segment[1].cast(); + Point seg_a{p_a[0].cast(), p_a[1].cast()}; + Point seg_b{p_b[0].cast(), p_b[1].cast()}; + Point target{target_seq[0].cast(), target_seq[1].cast()}; + + double fb_min_x = bounds_seq[0].cast(); + double fb_max_x = bounds_seq[1].cast(); + double fb_min_y = bounds_seq[2].cast(); + double fb_max_y = bounds_seq[3].cast(); + + int robot_id = robot_id_opt.value_or(-1); + + bool has_rect = forbidden_rect_opt.has_value() && !forbidden_rect_opt->is_none(); + double rmin_x = 0.0, rmax_x = 0.0, rmin_y = 0.0, rmax_y = 0.0; + if (has_rect) { + py::sequence r = forbidden_rect_opt.value(); + rmin_x = r[0].cast(); + rmax_x = r[1].cast(); + rmin_y = r[2].cast(); + rmax_y = r[3].cast(); + } + + size_t n = ox0.size(); + const bool* is_static_ptr = nullptr; + if (is_static_opt.has_value()) { + is_static_ptr = is_static_opt.value().data(); + } + + std::vector out_trajectory; + check_segment_recursive( + seg_a, seg_b, recursion_length, max_recursion_length, + target, fb_min_x, fb_max_x, fb_min_y, fb_max_y, robot_id, + clearance, subgoal_distance, has_rect, rmin_x, rmax_x, rmin_y, rmax_y, + detour_switch_margin, moving_ghost_min_m, precision, + ox0.data(), oy0.data(), ox1.data(), oy1.data(), n, + is_static_ptr, last_detour_side, last_obstacle, + out_trajectory + ); + + py::list traj_list; + double total_len = 0.0; + for (const auto& s : out_trajectory) { + py::array_t a(2); + a.mutable_at(0) = s.a.x; + a.mutable_at(1) = s.a.y; + py::array_t b(2); + b.mutable_at(0) = s.b.x; + b.mutable_at(1) = s.b.y; + traj_list.append(py::make_tuple(a, b)); + total_len += std::hypot(s.b.x - s.a.x, s.b.y - s.a.y); + } + return py::make_tuple(traj_list, total_len); + }); + + m.def("smooth_path_cpp", []( + py::sequence trajectory_seq, + py::sequence target_seq, + py::sequence robot_pos_seq, + double projection_distance, double clearance, + py::array_t ox0, + py::array_t oy0, + py::array_t ox1, + py::array_t oy1 + ) { + std::vector trajectory; + for (auto item : trajectory_seq) { + py::sequence s = item.cast(); + py::sequence sa = s[0].cast(); + py::sequence sb = s[1].cast(); + trajectory.push_back({ + {sa[0].cast(), sa[1].cast()}, + {sb[0].cast(), sb[1].cast()} + }); + } + Point target{target_seq[0].cast(), target_seq[1].cast()}; + Point robot_pos{robot_pos_seq[0].cast(), robot_pos_seq[1].cast()}; + + size_t n = ox0.size(); + Point res = smooth_path_cpp( + trajectory, target, robot_pos, projection_distance, clearance, + ox0.data(), oy0.data(), ox1.data(), oy1.data(), n + ); + py::array_t out(2); + out.mutable_at(0) = res.x; + out.mutable_at(1) = res.y; + return out; + }); +} diff --git a/utama_core/motion_planning/src/fastpathplanning/numba_kernels.py b/utama_core/motion_planning/src/fastpathplanning/numba_kernels.py index 9a7d517b..1969dd70 100644 --- a/utama_core/motion_planning/src/fastpathplanning/numba_kernels.py +++ b/utama_core/motion_planning/src/fastpathplanning/numba_kernels.py @@ -102,15 +102,17 @@ def _segments_intersect_nb( @njit(cache=True, fastmath=True) def _distance_between_segments_nb( ax0: float, ay0: float, ax1: float, ay1: float, bx0: float, by0: float, bx1: float, by1: float -) -> float: - """Exact port of `math_utils.distance_between_line_segments`.""" +) -> tuple: + """Computes distance between line segments and distance from point b0 to segment a. + Returns (min_dist, d_b0_to_a). + """ + d3 = distance_point_to_segment_nb(bx0, by0, ax0, ay0, ax1, ay1) if _segments_intersect_nb(ax0, ay0, ax1, ay1, bx0, by0, bx1, by1): - return 0.0 + return 0.0, d3 d1 = distance_point_to_segment_nb(ax0, ay0, bx0, by0, bx1, by1) d2 = distance_point_to_segment_nb(ax1, ay1, bx0, by0, bx1, by1) - d3 = distance_point_to_segment_nb(bx0, by0, ax0, ay0, ax1, ay1) d4 = distance_point_to_segment_nb(bx1, by1, ax0, ay0, ax1, ay1) - return min(d1, d2, d3, d4) + return min(d1, d2, d3, d4), d3 @njit(cache=True, fastmath=True) @@ -159,9 +161,10 @@ def scan_collides_nb( if o_max_y < seg_min_y or o_min_y > seg_max_y: continue - dist_between_segs = _distance_between_segments_nb(ox0i, oy0i, ox1i, oy1i, seg_x0, seg_y0, seg_x1, seg_y1) + dist_between_segs, dist_to_robot = _distance_between_segments_nb( + ox0i, oy0i, ox1i, oy1i, seg_x0, seg_y0, seg_x1, seg_y1 + ) if dist_between_segs < clearance: - dist_to_robot = distance_point_to_segment_nb(seg_x0, seg_y0, ox0i, oy0i, ox1i, oy1i) if i == sticky_idx: sticky_dist_to_robot = dist_to_robot if dist_to_robot < min_dist_to_robot: diff --git a/utama_core/motion_planning/src/fastpathplanning/planner.py b/utama_core/motion_planning/src/fastpathplanning/planner.py index 5f883807..0f8e8ece 100644 --- a/utama_core/motion_planning/src/fastpathplanning/planner.py +++ b/utama_core/motion_planning/src/fastpathplanning/planner.py @@ -34,6 +34,14 @@ ) from utama_core.rsoccer_simulator.src.ssl.envs.standard_ssl import SSLStandardEnv +try: + from utama_core.motion_planning.src.fastpathplanning import _fastpathplanner + + HAS_CPP = True +except ImportError: + _fastpathplanner = None + HAS_CPP = False + def _same_segment(a, b) -> bool: """`np.array_equal(a[0], b[0]) and np.array_equal(a[1], b[1])` for the @@ -186,9 +194,12 @@ def __init__(self, env: SSLStandardEnv | None): # allocations were a real, repeated cost, not just the collision # checks that consume its output. self._obstacle_cache_ts: float | None = None - self._obstacle_cache_moving: List[Tuple[bool, int, np.ndarray, np.ndarray]] = [] + self._obstacle_cache_moving: List[Tuple] = [] self._obstacle_cache_static: List[Tuple[np.ndarray, np.ndarray]] = [] self._obstacle_cache_static_keys: set = set() + self._static_cache_key: tuple | None = None + self._obstacle_cache_static_arrays: tuple | None = None + self._field_bounds_exempt_cache: dict[tuple, np.ndarray] = {} # Per-robot memory of the last full replan's sanitized target and the # resulting detour trajectory (the geometric waypoint list @@ -214,12 +225,15 @@ def __init__(self, env: SSLStandardEnv | None): # OBSTACLE_CLEARANCE/SUBGOAL_DISTANCE so it can never mask a real # target change large enough to matter for routing. self._REPLAN_TARGET_TOLERANCE = 0.02 # metres + self._REPLAN_TARGET_TOLERANCE_SQ = self._REPLAN_TARGET_TOLERANCE * self._REPLAN_TARGET_TOLERANCE # The robot's own position must also still be close to where the # cached trajectory was planned from — see the skip check's comment # in `_path_to` for why. A fraction of `SUBGOAL_DISTANCE` (itself # derived from robot diameter/clearance), not a bare constant, so it # scales sensibly if those config values ever change. self._REPLAN_POSITION_TOLERANCE = 0.25 * self.SUBGOAL_DISTANCE + self._REPLAN_POSITION_TOLERANCE_SQ = self._REPLAN_POSITION_TOLERANCE * self._REPLAN_POSITION_TOLERANCE + self.LOOK_AHEAD_RANGE_SQ = self.LOOK_AHEAD_RANGE * self.LOOK_AHEAD_RANGE # `_enemy_defense_rect` result cache, keyed by `margin` (the only # thing that varies across its call sites — two fixed constants, 0.0 @@ -263,12 +277,11 @@ def _should_draw(self) -> bool: return self._env is not None and self._env.render_mode == "human" def is_point_in_field(self, point, field_bounds: FieldBounds) -> bool: - x, y = float(point[0]), float(point[1]) - min_x = min(field_bounds.top_left[0], field_bounds.bottom_right[0]) - max_x = max(field_bounds.top_left[0], field_bounds.bottom_right[0]) - min_y = min(field_bounds.top_left[1], field_bounds.bottom_right[1]) - max_y = max(field_bounds.top_left[1], field_bounds.bottom_right[1]) - return min_x <= x <= max_x and min_y <= y <= max_y + tl_x, tl_y = field_bounds.top_left + br_x, br_y = field_bounds.bottom_right + min_x, max_x = (tl_x, br_x) if tl_x <= br_x else (br_x, tl_x) + min_y, max_y = (br_y, tl_y) if br_y <= tl_y else (tl_y, br_y) + return min_x <= point[0] <= max_x and min_y <= point[1] <= max_y def _enemy_defense_rect(self, game: Game, margin: float) -> Tuple[float, float, float, float]: """(min_x, max_x, min_y, max_y) of the opponent's defense area, inflated @@ -391,50 +404,50 @@ def _refresh_obstacle_cache(self, game: Game, field_bounds: FieldBounds) -> None segments) once per tick, keyed on `game.ts`. `_get_obstacles` then just filters/excludes per robot instead of reallocating from scratch. """ + # Static field bounds and defense-area segments only depend on field + # bounds and team orientation, which are constant for a match. + static_key = ( + field_bounds.top_left[0], + field_bounds.top_left[1], + field_bounds.bottom_right[0], + field_bounds.bottom_right[1], + game.field.my_team_is_right, + ) + if self._static_cache_key != static_key: + tl = np.array(field_bounds.top_left, dtype=np.float64) + br = np.array(field_bounds.bottom_right, dtype=np.float64) + tr = np.array([field_bounds.bottom_right[0], field_bounds.top_left[1]], dtype=np.float64) + bl = np.array([field_bounds.top_left[0], field_bounds.bottom_right[1]], dtype=np.float64) + + static = [(tl, tr), (tr, br), (br, bl), (bl, tl)] + + min_x, max_x, min_y, max_y = self._enemy_defense_rect(game, margin=OPPONENT_DEFENSE_AREA_KEEP_DISTANCE) + c0 = np.array([min_x, max_y], dtype=np.float64) + c1 = np.array([max_x, max_y], dtype=np.float64) + c2 = np.array([max_x, min_y], dtype=np.float64) + c3 = np.array([min_x, min_y], dtype=np.float64) + static.extend([(c0, c1), (c1, c2), (c2, c3), (c3, c0)]) + + self._obstacle_cache_static = static + self._obstacle_cache_static_keys = {_segment_key(o) for o in static} + self._obstacle_cache_static_arrays = flatten_obstacles(static) + self._static_cache_key = static_key + if self._obstacle_cache_ts == game.ts: return - # Tag each entry with whether it's a friendly robot, not just its raw - # `id` — friendly and enemy robots occupy independent id spaces, so a - # bare `r.id == robot_id` filter (applied to both lists combined) - # would wrongly exclude an enemy robot that happens to share the - # querying friendly robot's numeric id. Only friendly robots are ever - # excluded by id (the original semantics: `robot.id != robot_id` was - # only ever applied to `game.friendly_robots`). - moving: List[Tuple[bool, int, np.ndarray, np.ndarray]] = [] + moving: List[Tuple] = [] + proj_factor = self.PROJECTEDFRAMES / CONTROL_FREQUENCY for is_friendly, robots in ((True, game.friendly_robots), (False, game.enemy_robots)): for r in robots.values(): - robot_pos = np.array([r.p.x, r.p.y]) - velocity = np.array([r.v.x, r.v.y]) - # Project the "Ghost Wall" based on current velocity - point = robot_pos + velocity * (self.PROJECTEDFRAMES / CONTROL_FREQUENCY) - moving.append((is_friendly, r.id, robot_pos, point)) - - # Field bounds as obstacles (static, usually not drawn to keep screen clean) - tl, br = np.array(field_bounds.top_left), np.array(field_bounds.bottom_right) - tr = np.array([field_bounds.bottom_right[0], field_bounds.top_left[1]]) - bl = np.array([field_bounds.top_left[0], field_bounds.bottom_right[1]]) - - static = [(tl, tr), (tr, br), (br, bl), (bl, tl)] - - # Opponent's defense area is off-limits to every non-goalkeeper robot - # at all times during live play (SSL rules) — no per-tactic exception - # exists, so this belongs at the planner level rather than something - # every tactic has to remember to avoid itself. Our own defense area - # is deliberately NOT added here: our own goalkeeper (and, briefly, - # any outfield robot legally retrieving the ball from it) needs to be - # able to enter it, and this planner has no notion of "except the - # goalkeeper" to carve that out safely. - min_x, max_x, min_y, max_y = self._enemy_defense_rect(game, margin=OPPONENT_DEFENSE_AREA_KEEP_DISTANCE) - c0 = np.array([min_x, max_y]) - c1 = np.array([max_x, max_y]) - c2 = np.array([max_x, min_y]) - c3 = np.array([min_x, min_y]) - static.extend([(c0, c1), (c1, c2), (c2, c3), (c3, c0)]) + rx, ry = float(r.p.x), float(r.p.y) + px = rx + float(r.v.x) * proj_factor + py = ry + float(r.v.y) * proj_factor + robot_pos = np.array([rx, ry], dtype=np.float64) + point = np.array([px, py], dtype=np.float64) + moving.append((is_friendly, r.id, rx, ry, px, py, robot_pos, point)) self._obstacle_cache_moving = moving - self._obstacle_cache_static = static - self._obstacle_cache_static_keys = {_segment_key(o) for o in static} self._obstacle_cache_ts = game.ts def _get_obstacles( @@ -451,13 +464,20 @@ def _get_obstacles( """ self._refresh_obstacle_cache(game, field_bounds) + our_x, our_y = float(our_pos[0]), float(our_pos[1]) + look_ahead_sq = self.LOOK_AHEAD_RANGE_SQ + obstacle_list = [] - for is_friendly, r_id, robot_pos, point in self._obstacle_cache_moving: + moving_coords = [] + for is_friendly, r_id, rx, ry, px, py, robot_pos, point in self._obstacle_cache_moving: if is_friendly and r_id == robot_id: continue - if distance(our_pos, robot_pos) < self.LOOK_AHEAD_RANGE: + dx = our_x - rx + dy = our_y - ry + if dx * dx + dy * dy < look_ahead_sq: obstacle_segment = (robot_pos, point) obstacle_list.append(obstacle_segment) + moving_coords.append((rx, ry, px, py)) # DRAWING: Show the projected velocity line in Red when an RSim renderer is available. if self._should_draw: @@ -472,6 +492,29 @@ def _get_obstacles( clearance, subgoal_distance = self._effective_clearance(len(obstacle_list)) obstacle_list.extend(self._obstacle_cache_static) + + # Seed _obstacle_arrays_cache with pre-built arrays for obstacle_list + if self._obstacle_cache_static_arrays is not None: + m = len(moving_coords) + s_ox0, s_oy0, s_ox1, s_oy1 = self._obstacle_cache_static_arrays + s = len(s_ox0) + n = m + s + ox0 = np.empty(n, dtype=np.float64) + oy0 = np.empty(n, dtype=np.float64) + ox1 = np.empty(n, dtype=np.float64) + oy1 = np.empty(n, dtype=np.float64) + for i in range(m): + c = moving_coords[i] + ox0[i] = c[0] + oy0[i] = c[1] + ox1[i] = c[2] + oy1[i] = c[3] + ox0[m:] = s_ox0 + oy0[m:] = s_oy0 + ox1[m:] = s_ox1 + oy1[m:] = s_oy1 + self._obstacle_arrays_cache.append((obstacle_list, (ox0, oy0, ox1, oy1))) + return obstacle_list, clearance, subgoal_distance def _effective_clearance(self, nearby_obstacle_count: int) -> Tuple[float, float]: @@ -683,7 +726,8 @@ def _find_subgoal_fast( ox0, oy0, ox1, oy1 = self._obstacle_arrays(obstacles) has_rect = forbidden_rect is not None rmin_x, rmax_x, rmin_y, rmax_y = forbidden_rect if has_rect else (0.0, 0.0, 0.0, 0.0) - found, sub_x, sub_y, last_hit_idx, rect_last = find_subgoal_nb( + subgoal_fn = _fastpathplanner.find_subgoal_nb if HAS_CPP else find_subgoal_nb + found, sub_x, sub_y, last_hit_idx, rect_last = subgoal_fn( float(obstacle_pos[0]), float(obstacle_pos[1]), ux, @@ -717,10 +761,19 @@ def _collides_fast( clearance: Optional[float] = None, ): """`_collides_exact` with everything after the cache lookup in one native - call (`numba_kernels.collides_nb`); see that method for the behaviour.""" + call (`numba_kernels.collides_nb` or C++); see that method for the behaviour.""" clearance = self.OBSTACLE_CLEARANCE if clearance is None else clearance - sticky_key = _segment_key(sticky_obstacle) if sticky_obstacle is not None else None - seg_key = (_segment_key(segment), sticky_key, clearance) + seg_a, seg_b = segment + sax, say = float(seg_a[0]), float(seg_a[1]) + sbx, sby = float(seg_b[0]), float(seg_b[1]) + if sticky_obstacle is not None: + stk_a, stk_b = sticky_obstacle + stk_ax, stk_ay = float(stk_a[0]), float(stk_a[1]) + stk_bx, stk_by = float(stk_b[0]), float(stk_b[1]) + sticky_key = (stk_ax, stk_ay, stk_bx, stk_by) + else: + sticky_key = None + seg_key = ((sax, say, sbx, sby), sticky_key, clearance) cached = self._collision_cache.get(seg_key) if cached is not None: return cached @@ -733,16 +786,17 @@ def _collides_fast( oy0, ox1, oy1, - float(sticky_obstacle[0][0]), - float(sticky_obstacle[0][1]), - float(sticky_obstacle[1][0]), - float(sticky_obstacle[1][1]), + stk_ax, + stk_ay, + stk_bx, + stk_by, ) - idx, from_sticky, px, py = collides_nb( - float(segment[0][0]), - float(segment[0][1]), - float(segment[1][0]), - float(segment[1][1]), + collides_fn = _fastpathplanner.collides_nb if HAS_CPP else collides_nb + idx, from_sticky, px, py = collides_fn( + sax, + say, + sbx, + sby, clearance, ox0, oy0, @@ -929,6 +983,42 @@ def check_segment( clearance = self.OBSTACLE_CLEARANCE if clearance is None else clearance subgoal_distance = self.SUBGOAL_DISTANCE if subgoal_distance is None else subgoal_distance + if HAS_CPP and not EXACT_MATH: + ox0, oy0, ox1, oy1 = self._obstacle_arrays(obstacles) + tl_x, tl_y = field_bounds.top_left + br_x, br_y = field_bounds.bottom_right + min_x, max_x = (tl_x, br_x) if tl_x <= br_x else (br_x, tl_x) + min_y, max_y = (br_y, tl_y) if br_y <= tl_y else (tl_y, br_y) + bounds_seq = (min_x, max_x, min_y, max_y) + static_keys = self._obstacle_cache_static_keys + if static_keys: + is_static = np.fromiter( + (_segment_key(o) in static_keys for o in obstacles), dtype=bool, count=len(obstacles) + ) + else: + is_static = None + return _fastpathplanner.check_segment_cpp( + segment, + ox0, + oy0, + ox1, + oy1, + is_static, + recursion_length, + self.MAXRECURSIONLENGTH, + target, + bounds_seq, + robot_id, + clearance, + subgoal_distance, + forbidden_rect, + self.DETOUR_SWITCH_MARGIN, + self._MOVING_GHOST_MIN_M, + self._DETOUR_MEMORY_PRECISION, + self._last_detour_side, + self._last_obstacle, + ) + obstacle_memory_key = (robot_id, recursion_length) if robot_id is not None else None sticky_obstacle_segment = ( self._last_obstacle.get(obstacle_memory_key) if obstacle_memory_key is not None else None @@ -1086,7 +1176,8 @@ def _clamp_to_obstacle_clearance_fast( (`numba_kernels.clamp_to_obstacle_clearance_nb`).""" clearance = self.OBSTACLE_CLEARANCE if clearance is None else clearance ox0, oy0, ox1, oy1 = self._obstacle_arrays(obstacles) - return clamp_to_obstacle_clearance_nb( + clamp_fn = _fastpathplanner.clamp_to_obstacle_clearance_nb if HAS_CPP else clamp_to_obstacle_clearance_nb + return clamp_fn( float(origin[0]), float(origin[1]), float(unit_vec[0]), @@ -1144,6 +1235,22 @@ def _clamp_to_obstacle_clearance_exact( def smooth_path( self, trajectory, target, robot_position, obstacles: List, clearance: Optional[float] = None ) -> np.ndarray: + if len(trajectory) == 1: + return target + if not EXACT_MATH and HAS_CPP: + ox0, oy0, ox1, oy1 = self._obstacle_arrays(obstacles) + clearance = self.OBSTACLE_CLEARANCE if clearance is None else clearance + return _fastpathplanner.smooth_path_cpp( + trajectory, + target, + robot_position, + self.PROJECTION_DISTANCE, + clearance, + ox0, + oy0, + ox1, + oy1, + ) # A first subgoal sitting exactly on the robot (the detour recursion can # produce one) has no direction to normalise: dividing by its zero length # returned a NaN carrot. Skip such segments. Found by @@ -1183,12 +1290,34 @@ def _sanitize_target_fast( """`_sanitize_target_exact` with its push-out passes in one native call (`numba_kernels.sanitize_target_nb`); see that method for the behaviour.""" clearance = self.OBSTACLE_CLEARANCE if clearance is None else clearance - exempt = [] if exempt_obstacles is None else list(exempt_obstacles) - if field_bounds is not None: - (x0, y0), (x1, y1) = field_bounds.top_left, field_bounds.bottom_right - exempt += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)] + if not exempt_obstacles: + if field_bounds is not None: + bounds_key = ( + field_bounds.top_left[0], + field_bounds.top_left[1], + field_bounds.bottom_right[0], + field_bounds.bottom_right[1], + ) + exempt_arr = self._field_bounds_exempt_cache.get(bounds_key) + if exempt_arr is None: + (x0, y0), (x1, y1) = field_bounds.top_left, field_bounds.bottom_right + exempt_arr = np.array( + [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)], + dtype=np.float64, + ) + self._field_bounds_exempt_cache[bounds_key] = exempt_arr + else: + exempt_arr = np.empty((0, 4), dtype=np.float64) + else: + exempt = list(exempt_obstacles) + if field_bounds is not None: + (x0, y0), (x1, y1) = field_bounds.top_left, field_bounds.bottom_right + exempt += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)] + exempt_arr = np.array(exempt, dtype=np.float64).reshape(-1, 4) + ox0, oy0, ox1, oy1 = self._obstacle_arrays(obstacles) - tx, ty = sanitize_target_nb( + sanitize_fn = _fastpathplanner.sanitize_target_nb if HAS_CPP else sanitize_target_nb + tx, ty = sanitize_fn( float(target[0]), float(target[1]), float(robot_pos[0]), @@ -1198,7 +1327,7 @@ def _sanitize_target_fast( oy0, ox1, oy1, - np.array(exempt, dtype=np.float64).reshape(-1, 4), + exempt_arr, ) return np.array([tx, ty]) @@ -1402,9 +1531,11 @@ def _path_to( # up near the ball. ball_adjacent_obstacles = set() if game.ball is not None: - ball_pos = np.array([game.ball.p.x, game.ball.p.y]) - diff = raw_target - ball_pos - if math.hypot(diff[0], diff[1]) < clearance: + ball_x, ball_y = float(game.ball.p.x), float(game.ball.p.y) + dx = float(raw_target[0]) - ball_x + dy = float(raw_target[1]) - ball_y + if dx * dx + dy * dy < clearance * clearance: + ball_pos = np.array([ball_x, ball_y]) for o in obstacles: if distance_point_to_segment(ball_pos, o[0], o[1]) < clearance: ball_adjacent_obstacles.add(_segment_key(o)) @@ -1482,9 +1613,13 @@ def _path_to( final_trajectory = None if cached is not None: cached_origin, cached_target, cached_trajectory = cached + d_target_x = float(safe_target[0]) - float(cached_target[0]) + d_target_y = float(safe_target[1]) - float(cached_target[1]) + d_pos_x = float(our_pos[0]) - float(cached_origin[0]) + d_pos_y = float(our_pos[1]) - float(cached_origin[1]) if ( - distance(safe_target, cached_target) <= self._REPLAN_TARGET_TOLERANCE - and distance(our_pos, cached_origin) <= self._REPLAN_POSITION_TOLERANCE + (d_target_x * d_target_x + d_target_y * d_target_y) <= self._REPLAN_TARGET_TOLERANCE_SQ + and (d_pos_x * d_pos_x + d_pos_y * d_pos_y) <= self._REPLAN_POSITION_TOLERANCE_SQ and all( self.collides(seg, routing_obstacles, clearance=clearance)[0] is None for seg in cached_trajectory ) diff --git a/utama_core/motion_planning/src/trajsampling/bang_bang.py b/utama_core/motion_planning/src/trajsampling/bang_bang.py index 7a0f72a0..a041acb1 100644 --- a/utama_core/motion_planning/src/trajsampling/bang_bang.py +++ b/utama_core/motion_planning/src/trajsampling/bang_bang.py @@ -19,7 +19,7 @@ from dataclasses import dataclass -@dataclass(frozen=True) +@dataclass(frozen=True, slots=True) class BangBang1D: """Time-optimal 1D trajectory from (p0, v0) to (p1, v=0) respecting |velocity| <= v_max and |acceleration| <= a_max. @@ -171,7 +171,7 @@ def state_at(self, t: float) -> tuple[float, float]: return self.p0 + sign * d_signed, v_signed * sign -@dataclass(frozen=True) +@dataclass(frozen=True, slots=True) class Trajectory2D: """A single `BangBang1D` profile along the straight line from p0 to p1, mapped back onto x/y by the direction unit vector. diff --git a/utama_core/motion_planning/src/trajsampling/collision_numba.py b/utama_core/motion_planning/src/trajsampling/collision_numba.py index b8962f67..b355b3f6 100644 --- a/utama_core/motion_planning/src/trajsampling/collision_numba.py +++ b/utama_core/motion_planning/src/trajsampling/collision_numba.py @@ -282,21 +282,32 @@ def _enemy_distance_at(row: np.ndarray, t: float, px: float, py: float) -> float tc = 0.0 elif tc > t_max: tc = t_max - fp, _ = _bangbang_state_at(f_plus, tc) - fm, _ = _bangbang_state_at(f_minus, tc) - r_dyn = abs(fp - fm) / 2.0 if speed < 1e-6: - cx, cy = p0x, p0y - else: - offset = fm + r_dyn - cx = p0x + dirx * offset - cy = p0y + diry * offset - return math.hypot(px - cx, py - cy) - (radius + r_dyn) + return math.hypot(px - p0x, py - p0y) - radius + + fp, _ = _bangbang_state_at(f_plus, tc) + fm, _ = _bangbang_state_at(f_minus, tc) + length = fp - fm + if length < 1e-9: + return math.hypot(px - p0x, py - p0y) - radius + + # Tube (capsule) from fm to fp along direction unit vector: + # ax, ay is the rear reachability bound, bx, by is the forward bound + ax = p0x + dirx * fm + ay = p0y + diry * fm + s = (px - ax) * dirx + (py - ay) * diry + if s < 0.0: + s = 0.0 + elif s > length: + s = length + cx = ax + dirx * s + cy = ay + diry * s + return math.hypot(px - cx, py - cy) - radius @njit(cache=True, fastmath=True) -def first_collision_numba( +def first_collision_with_kind_numba( leg1: np.ndarray, ux1: float, uy1: float, @@ -321,12 +332,10 @@ def first_collision_numba( traj_obs: np.ndarray, # shape (n_traj, TRAJ_ROW_LEN) cv_obs: np.ndarray, # shape (n_cv, CV_ROW_LEN) enemy_obs: np.ndarray, # shape (n_enemy, ENEMY_ROW_LEN) -) -> float: +) -> Tuple[float, int]: """Native port of `TrajectorySamplingPlanner._first_collision`'s scan - loop -- see that method's docstring for the exact semantics being - preserved (margin formula, own-radius subtraction, adaptive stepping, - `MAX_LOOKAHEAD_TIME` clamp). Returns the absolute collision time, or - -1.0 if clear (Python wrapper converts to `Optional[float]`). + loop, returning (collision_time, obstacle_kind): + kind: 0 = static, 1 = traj, 2 = cv, 3 = enemy, -1 = no collision. """ t = start_t duration = min(query_duration, max_lookahead_time) @@ -335,24 +344,6 @@ def first_collision_numba( n_cv = cv_obs.shape[0] n_enemy = enemy_obs.shape[0] - # A robot can start a plan already inside another (moving) obstacle's - # clearance envelope -- e.g. GiveAndGoTactic's abandoned-receiver robot, - # left standing right where it was waiting to catch a pass, replanning a - # fresh route the instant the ball becomes a real obstacle again. Without - # this, `d < margin` at the very first sample (t == start_t) fires for - # EVERY candidate direction regardless of where it's headed -- the - # robot's own starting position is the collision, not anything about the - # trajectory -- so the planner can never find an escape and the robot - # stays pinned near-zero velocity indefinitely (found live, 2026-09-03: - # a robot 0.088m from the ball, inside the 0.09+0.0215=0.1115m combined - # radius, reporting collision_time~=0 against every sampled direction). - # Fixed by giving each obstacle a one-time "still escaping" grace at the - # start of the scan: an obstacle already violating clearance at t == - # start_t doesn't veto the trajectory outright until the robot has first - # cleared it (`d >= margin`) at some later sample -- from that point on - # it's checked normally, including re-triggering if the trajectory turns - # back into it. An obstacle that ISN'T penetrated at start_t is never - # granted this grace, so a normal approaching trajectory is unaffected. static_escaping = np.ones(n_static, dtype=np.bool_) traj_escaping = np.ones(n_traj, dtype=np.bool_) cv_escaping = np.ones(n_cv, dtype=np.bool_) @@ -366,6 +357,7 @@ def first_collision_numba( closest = math.inf collided = False + collided_kind = -1 for i in range(n_static): d = _static_distance_at(static_obs[i], px, py) - robot_radius @@ -376,6 +368,7 @@ def first_collision_numba( static_escaping[i] = still_penetrating if still_penetrating and not static_escaping[i]: collided = True + collided_kind = 0 break if d < closest: closest = d @@ -389,6 +382,7 @@ def first_collision_numba( traj_escaping[i] = still_penetrating if still_penetrating and not traj_escaping[i]: collided = True + collided_kind = 1 break if d < closest: closest = d @@ -402,6 +396,7 @@ def first_collision_numba( cv_escaping[i] = still_penetrating if still_penetrating and not cv_escaping[i]: collided = True + collided_kind = 2 break if d < closest: closest = d @@ -415,6 +410,7 @@ def first_collision_numba( enemy_escaping[i] = still_penetrating if still_penetrating and not enemy_escaping[i]: collided = True + collided_kind = 3 break if d < closest: closest = d @@ -422,7 +418,7 @@ def first_collision_numba( first_sample = False if collided: - return t + return t, collided_kind if closest == math.inf: step = max_time_step @@ -437,7 +433,64 @@ def first_collision_numba( break t = min(t + step, duration) - return -1.0 + return -1.0, -1 + + +@njit(fastmath=True) +def first_collision_numba( + leg1: np.ndarray, + ux1: float, + uy1: float, + p0x1: float, + p0y1: float, + switch_t: float, + leg2: np.ndarray, + ux2: float, + uy2: float, + p0x2: float, + p0y2: float, + query_duration: float, + start_t: float, + max_lookahead_time: float, + robot_radius: float, + margin_v_max: float, + margin_base: float, + max_time_step: float, + min_time_step: float, + step_distance_ratio: float, + static_obs: np.ndarray, + traj_obs: np.ndarray, + cv_obs: np.ndarray, + enemy_obs: np.ndarray, +) -> float: + """Wrapper around `first_collision_with_kind_numba` returning only the collision time.""" + t_col, _ = first_collision_with_kind_numba( + leg1, + ux1, + uy1, + p0x1, + p0y1, + switch_t, + leg2, + ux2, + uy2, + p0x2, + p0y2, + query_duration, + start_t, + max_lookahead_time, + robot_radius, + margin_v_max, + margin_base, + max_time_step, + min_time_step, + step_distance_ratio, + static_obs, + traj_obs, + cv_obs, + enemy_obs, + ) + return t_col def empty_row_array(row_len: int) -> np.ndarray: diff --git a/utama_core/motion_planning/src/trajsampling/config.py b/utama_core/motion_planning/src/trajsampling/config.py index 38dab484..55f26630 100644 --- a/utama_core/motion_planning/src/trajsampling/config.py +++ b/utama_core/motion_planning/src/trajsampling/config.py @@ -4,10 +4,9 @@ class trajsamplingconfig: ROBOT_RADIUS = ROBOT_RADIUS - # Number of random intermediate targets tried when the direct trajectory - # collides, per TIGERs 2024 champion paper section 2.2 ("Currently, we - # use the random-based implementation with five targets"). - N_INTERMEDIATE_TARGETS = 5 + # Number of intermediate targets tried when the direct trajectory collides. + # Systematic fan around the target direction (e.g. +-15 deg, +-30 deg, +-45 deg ...). + N_INTERMEDIATE_TARGETS = 11 # How far (metres) an intermediate target is placed from the robot's # current position -- large enough to route around a robot-sized diff --git a/utama_core/motion_planning/src/trajsampling/obstacles.py b/utama_core/motion_planning/src/trajsampling/obstacles.py index 6312da18..6c39ed5a 100644 --- a/utama_core/motion_planning/src/trajsampling/obstacles.py +++ b/utama_core/motion_planning/src/trajsampling/obstacles.py @@ -130,21 +130,23 @@ def __post_init__(self): object.__setattr__(self, "_f_minus", BangBang1D.compute(0.0, self.speed, -far, self.v_max, self.a_max)) def distance_at(self, t: float, point: Tuple[float, float]) -> float: + if self.speed < 1e-6: + return math.hypot(point[0] - self.p0[0], point[1] - self.p0[1]) - self.radius tc = max(0.0, min(t, self.t_max)) f_plus, _ = self._f_plus.state_at(tc) f_minus, _ = self._f_minus.state_at(tc) - r_dyn = abs(f_plus - f_minus) / 2.0 - # Stationary opponent: direction is undefined and r_dyn's f+/f- - # split is meaningless as a directional offset -- just grow a plain - # circle from p0, matching the paper's explicit tweak ("for - # non-moving robots, the dynamic radius is set to zero, effectively - # reducing the obstacle to a small circle"). - if self.speed < 1e-6: - center = self.p0 - else: - offset = f_minus + r_dyn - center = (self.p0[0] + self.direction[0] * offset, self.p0[1] + self.direction[1] * offset) - return math.hypot(point[0] - center[0], point[1] - center[1]) - (self.radius + r_dyn) + length = f_plus - f_minus + if length < 1e-9: + return math.hypot(point[0] - self.p0[0], point[1] - self.p0[1]) - self.radius + # Tube (capsule) from f_minus to f_plus along direction unit vector + ax = self.p0[0] + self.direction[0] * f_minus + ay = self.p0[1] + self.direction[1] * f_minus + px, py = point + s = (px - ax) * self.direction[0] + (py - ay) * self.direction[1] + s = max(0.0, min(length, s)) + cx = ax + self.direction[0] * s + cy = ay + self.direction[1] * s + return math.hypot(px - cx, py - cy) - self.radius def enemy_obstacle_from_robot( diff --git a/utama_core/motion_planning/src/trajsampling/planner.py b/utama_core/motion_planning/src/trajsampling/planner.py index cc262721..1d2df46e 100644 --- a/utama_core/motion_planning/src/trajsampling/planner.py +++ b/utama_core/motion_planning/src/trajsampling/planner.py @@ -41,7 +41,7 @@ import random from dataclasses import dataclass from dataclasses import replace as dataclasses_replace -from typing import List, Optional, Tuple +from typing import List, Optional, Tuple, Union import numpy as np @@ -65,7 +65,7 @@ ) -@dataclass(frozen=True) +@dataclass(frozen=True, slots=True) class PlanResult: trajectory: Trajectory2D has_collision: bool @@ -265,16 +265,24 @@ def _flatten_obstacles( return _stack_rows(_flatten_obstacle_rows(obstacles)) -def _has_priority(robot_id: int, other_id: int) -> bool: +def _has_priority(robot_id: int, other_id: int, game: Optional[Game] = None) -> bool: """True if `robot_id` outranks `other_id` and so may plan through its - path -- `other_id` must yield instead. Mirrors TIGERs' - `PathFinderPrioMap.mapByBotId`'s default ("higher botIds are - preferred"): a plain, fixed ordering by ID, not dynamically computed - from role or task. Good enough to break the symmetry that let two - equally-"locally correct" robots both plan through each other; a - role-aware ordering (e.g. attacker outranks support) is a refinement - for later, not needed to fix the observed collision. + path -- `other_id` must yield instead. + + Role-aware priority: + If `game` is provided and exactly one of the two teammates possesses the + ball (`has_ball == True`), the ball carrier takes absolute priority over + off-ball runners. Otherwise breaks symmetry by fixed robot ID + (`robot_id > other_id`), matching TIGERs' `PathFinderPrioMap.mapByBotId`. """ + if game is not None: + r_this = game.friendly_robots.get(robot_id) + r_other = game.friendly_robots.get(other_id) + if r_this is not None and r_other is not None: + if r_this.has_ball and not r_other.has_ball: + return True + if r_other.has_ball and not r_this.has_ball: + return False return robot_id > other_id @@ -415,6 +423,19 @@ def _priority_blocking_enabled(game: Game) -> bool: # so a jittered variant is never itself stale merely for being tried. _NEAR_LAST_JITTER_STEP_RAD = math.radians(11) +# The moving-obstacle arrays left empty, to check a candidate against static +# obstacles alone (`_fallback_rank`). +_NO_MOVING_OBSTACLES = ( + _cn.empty_row_array(_cn.TRAJ_ROW_LEN), + _cn.empty_row_array(_cn.CV_ROW_LEN), + _cn.empty_row_array(_cn.ENEMY_ROW_LEN), +) + +# Fallback rank of a candidate whose first collision is static: below every +# moving collision (ranked by minus squared distance to the target, at most +# tens of m^2), above a priority-blocked one (-inf). +_STATIC_COLLISION_RANK = -1e18 + class TrajectorySamplingPlanner: def __init__(self, v_max: float, a_max: float): @@ -600,12 +621,14 @@ def plan( # See `_first_collision`'s docstring for why this matters. obstacle_arrays = _stack_rows(combined_rows) - reused = self._try_reuse(robot_id, game.ts, p0, target_pos, obstacles, obstacle_arrays, priority_enabled) + reused = self._try_reuse( + robot_id, game.ts, p0, target_pos, obstacles, obstacle_arrays, priority_enabled, game=game + ) if reused is not None: return self._with_current_clearance(reused, p0, obstacles) direct = Trajectory2D.compute(p0, v0, target_pos, self.v_max, self.a_max) - collision_time = self._first_collision(direct, obstacle_arrays) + collision_time, direct_kind = self._check_collision(direct, obstacle_arrays) if collision_time is None: self._commit(robot_id, game.ts, direct, target_pos) result = PlanResult(trajectory=direct, has_collision=False, collision_time=None) @@ -645,11 +668,11 @@ def plan( # ranked down by raw survival time exactly like a genuine head-on # collision, with the planner never actually being allowed to # complete a totally normal, low-speed arrival next to another robot. - def fallback_rank(t_col: float, blocked: bool) -> float: - return -math.inf if blocked else t_col + def fallback_rank(trajectory, t_col: float, blocked: bool, obs_kind: int = -1) -> float: + return self._fallback_rank(trajectory, t_col, blocked, obstacle_arrays, target_pos, obs_kind=obs_kind) direct_blocked = self._blocked_by_priority_obstacle( - robot_id, direct, obstacles, collision_time, priority_enabled + robot_id, direct, obstacles, collision_time, priority_enabled, game=game ) if not direct_blocked and self._collision_leniency_accepts( direct, collision_time, v0, target_pos, priority_enabled @@ -658,45 +681,54 @@ def fallback_rank(t_col: float, blocked: bool) -> float: result = PlanResult(trajectory=direct, has_collision=True, collision_time=collision_time) return self._with_current_clearance(result, p0, obstacles) best_fallback = PlanResult(trajectory=direct, has_collision=True, collision_time=collision_time) - best_rank = fallback_rank(collision_time, direct_blocked) - - for candidate_target in self._intermediate_targets(robot_id, p0, target_pos): - # Try every switch-time variant for this direction before moving - # on to the next sampled direction -- see - # `_two_segment_candidates`'s docstring for why a single fixed - # switch time isn't enough. - # - # Tried splitting this into a once-per-direction shared-prefix - # check plus a per-variant tail check, to avoid re-scanning each - # variant's common `first_leg` prefix from t=0. Measured: no - # speedup (107.9s vs 106.5s self-time on the same real-match - # profile, i.e. a wash within noise) and `_first_collision`'s own - # call count went UP (115k vs 77k), because most sampled - # directions' first leg doesn't collide at all, so the - # early-abandon path rarely fires -- the split then just adds a - # second Python-level call per candidate without saving any - # scanning. Reverted; see `profile_real_match_opt.log`. The real - # fix for the per-timestep obstacle loop's cost turned out to be - # `_first_collision`'s own numba rewrite (see its docstring), - # not reducing candidate/call count. + 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 + + last_dir = self._last_intermediate_direction.get(robot_id) + + for candidate_target in cand_targets: + is_last = False + if last_dir is not None: + cdx, cdy = candidate_target[0] - p0[0], candidate_target[1] - p0[1] + cdist = math.hypot(cdx, cdy) + if cdist > 1e-6: + if abs(cdx / cdist - last_dir[0]) < 1e-4 and abs(cdy / cdist - last_dir[1]) < 1e-4: + is_last = True + for candidate in self._two_segment_candidates(p0, v0, candidate_target, target_pos): - t_col = self._first_collision(candidate, obstacle_arrays) + t_col, cand_kind = self._check_collision(candidate, obstacle_arrays) if t_col is None: - self._commit(robot_id, game.ts, candidate, target_pos) - self._last_intermediate_direction[robot_id] = _unit_direction(p0, candidate_target) - result = PlanResult(trajectory=candidate, has_collision=False, collision_time=None) - return self._with_current_clearance(result, p0, obstacles) - blocked = self._blocked_by_priority_obstacle(robot_id, candidate, obstacles, t_col, priority_enabled) + eff_dur = candidate.duration - (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) + best_accepted_target = candidate_target + continue + blocked = self._blocked_by_priority_obstacle( + 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): - self._commit(robot_id, game.ts, candidate, target_pos) - self._last_intermediate_direction[robot_id] = _unit_direction(p0, candidate_target) - result = PlanResult(trajectory=candidate, has_collision=True, collision_time=t_col) - return self._with_current_clearance(result, p0, obstacles) - rank = fallback_rank(t_col, blocked) + eff_dur = candidate.duration - (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) + best_accepted_target = candidate_target + continue + rank = fallback_rank(candidate, t_col, blocked, obs_kind=cand_kind) if rank > best_rank: best_fallback = PlanResult(trajectory=candidate, has_collision=True, collision_time=t_col) best_rank = rank + if best_accepted is not None and best_accepted_target is not None: + self._commit(robot_id, game.ts, best_accepted.trajectory, target_pos) + self._last_intermediate_direction[robot_id] = _unit_direction(p0, best_accepted_target) + return self._with_current_clearance(best_accepted, p0, obstacles) + # Nothing collision-free was found within the sample budget: return # whichever candidate survives longest before its first collision # (and isn't blocked by a higher-priority teammate -- see @@ -747,11 +779,25 @@ def _with_current_clearance( _, (evx, evy) = result.trajectory.state_at(result.elapsed) speed = math.hypot(evx, evy) margin = (min(speed, config.MARGIN_V_MAX) / config.MARGIN_V_MAX) ** 2 * config.MARGIN_BASE - clear_obstacles = [o for o in obstacles if o.distance_at(result.elapsed, p0) - config.ROBOT_RADIUS >= margin] - candidates = clear_obstacles or obstacles - nearest_obstacle = min(candidates, key=lambda o: o.distance_at(result.elapsed, p0)) - nearest = nearest_obstacle.distance_at(result.elapsed, p0) - config.ROBOT_RADIUS + best_cand_d = math.inf + best_cand_obs = None + best_all_d = math.inf + best_all_obs = None + + for o in obstacles: + d = o.distance_at(result.elapsed, p0) + if d < best_all_d: + best_all_d = d + best_all_obs = o + if d - config.ROBOT_RADIUS >= margin: + if d < best_cand_d: + best_cand_d = d + best_cand_obs = o + + nearest_obstacle = best_cand_obs if best_cand_obs is not None else best_all_obs + nearest_d = best_cand_d if best_cand_obs is not None else best_all_d + nearest = nearest_d - config.ROBOT_RADIUS # Closing speed: finite-difference the gap to THIS SAME obstacle a # short step further along the robot's own committed trajectory, @@ -779,6 +825,7 @@ def _try_reuse( obstacles: List[TimedObstacle], obstacle_arrays: Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray], priority_enabled: bool = True, + game: Optional[Game] = None, ) -> Optional[PlanResult]: """Keep executing the already-committed trajectory, evaluated at its real elapsed time, instead of silently restarting it from t=0 every @@ -837,7 +884,7 @@ def _try_reuse( recheck_end = min(elapsed + _PRIORITY_RECHECK_LOOKAHEAD_TIME, trajectory.duration) t = elapsed while True: - if self._blocked_by_priority_obstacle(robot_id, trajectory, obstacles, t, priority_enabled): + if self._blocked_by_priority_obstacle(robot_id, trajectory, obstacles, t, priority_enabled, game=game): return None if t >= recheck_end: break @@ -845,6 +892,36 @@ def _try_reuse( return PlanResult(trajectory=trajectory, has_collision=False, collision_time=None, elapsed=elapsed) + def _fallback_rank( + self, + trajectory, + t_col: float, + blocked: bool, + obstacle_arrays: Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray], + target_pos: Tuple[float, float], + obs_kind: int = -1, + ) -> float: + """How good a colliding candidate is as `plan()`'s fallback: higher is better. + + A candidate blocked by a higher-priority teammate is never eligible (see + `plan()`). Among the rest, the best is the one whose first collision is + closest to the target (TIGERs' `getBestRejectedResult`): it makes the + most progress before it has to deal with the obstacle. Ranking by + survival time instead preferred a candidate that wanders off and + collides late, far from the target, over one that gets nearly there. A + candidate whose first collision is with a static obstacle (field + boundary, defence area) won't clear by waiting, so it ranks below every + moving collision, as in Sumatra. + """ + if blocked: + return -math.inf + if obs_kind == 0 or ( + obs_kind == -1 and self._first_collision(trajectory, (obstacle_arrays[0],) + _NO_MOVING_OBSTACLES) == t_col + ): + return _STATIC_COLLISION_RANK + (cx, cy), _ = trajectory.state_at(t_col) + return -((cx - target_pos[0]) ** 2 + (cy - target_pos[1]) ** 2) + def _two_segment_candidates( self, p0: Tuple[float, float], @@ -978,7 +1055,8 @@ def angular_distance_of_angle(cand_angle: float) -> float: diff = abs(cand_angle - final_angle) return min(diff, 2 * math.pi - diff) - r = config.INTERMEDIATE_TARGET_RADIUS + dist_to_target = math.hypot(fx, fy) + r = min(config.INTERMEDIATE_TARGET_RADIUS, max(0.4, 0.7 * dist_to_target)) last = None near_last: List[Tuple[float, float]] = [] @@ -1020,9 +1098,24 @@ def angular_distance(t: Tuple[float, float]) -> float: return angular_distance_of_angle(cand_angle) fresh: List[Tuple[float, float]] = [] - for _ in range(config.N_INTERMEDIATE_TARGETS): - angle = self._rng.uniform(0, 2 * math.pi) - fresh.append((p0[0] + r * math.cos(angle), p0[1] + r * math.sin(angle))) + n = config.N_INTERMEDIATE_TARGETS + # Generate n candidate directions symmetrically fanning out from final_angle. + # Tight angles use compact radius for efficient obstacle hugs; wider angles + # expand to the full configured radius for large obstacle clusters. + step = math.radians(15) + for i in range(1, (n // 2) + 1): + offset = step * i + # 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 + fresh.append( + (p0[0] + cand_r * math.cos(final_angle + offset), p0[1] + cand_r * math.sin(final_angle + offset)) + ) + fresh.append( + (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))) # Sort by angle between (p0 -> final_target) and (p0 -> candidate), # smallest first -- paper section 2.2: "This favors paths pointing @@ -1030,15 +1123,34 @@ def angular_distance(t: Tuple[float, float]) -> float: fresh.sort(key=angular_distance) return ([last] if last is not None else []) + near_last + fresh + def _check_collision( + self, + trajectory, + obstacle_arrays: Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray], + start_t: float = 0.0, + ) -> Tuple[Optional[float], int]: + """Wrapper around `_first_collision` that supports mocks/subclasses returning either + `(t_col, kind)` or just `t_col`.""" + try: + res = self._first_collision(trajectory, obstacle_arrays, start_t=start_t, return_kind=True) + if isinstance(res, tuple): + return res + return res, -1 + except TypeError: + return self._first_collision(trajectory, obstacle_arrays, start_t=start_t), -1 + def _first_collision( self, trajectory, obstacle_arrays: Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray], start_t: float = 0.0, - ) -> Optional[float]: + return_kind: bool = False, + ) -> Union[Optional[float], Tuple[Optional[float], int]]: """Returns the absolute trajectory time of the first collision at or after `start_t` (not a time-since-`start_t` offset), or None if clear through `min(trajectory.duration, MAX_LOOKAHEAD_TIME)`. + If `return_kind` is True, returns `(collision_time, obstacle_kind)` + where obstacle_kind is 0=static, 1=traj, 2=cv, 3=enemy, -1=none. `start_t` lets `_try_reuse` revalidate only the remaining portion of an already-committed trajectory instead of re-checking time this robot has already safely passed through. @@ -1049,7 +1161,7 @@ def _first_collision( `_flatten_obstacles` per candidate. Delegates the actual timestep x obstacle scan to - `collision_numba.first_collision_numba`, a native port of what used + `collision_numba.first_collision_with_kind_numba`, a native port of what used to be this method's own loop. Profiling (see `planner.py`'s module docstring) found this loop -- ~18 adaptive timesteps x ~20 obstacles per call, tens of thousands of calls per match -- was the @@ -1076,7 +1188,7 @@ def _first_collision( """ leg1_args, switch_t, leg2_args, duration = _flatten_query_trajectory(trajectory) static_arr, traj_arr, cv_arr, enemy_arr = obstacle_arrays - t_col = _cn.first_collision_numba( + t_col, obs_kind = _cn.first_collision_with_kind_numba( leg1_args[0], leg1_args[1], leg1_args[2], @@ -1102,7 +1214,10 @@ def _first_collision( cv_arr, enemy_arr, ) - return None if t_col < 0.0 else t_col + res_t = None if t_col < 0.0 else t_col + if return_kind: + return res_t, obs_kind + return res_t def _collision_leniency_accepts( self, @@ -1192,6 +1307,7 @@ def _blocked_by_priority_obstacle( obstacles: List[TimedObstacle], collision_time: float, priority_enabled: bool = True, + game: Optional[Game] = None, ) -> bool: """True if, at `collision_time`, this trajectory collides with a teammate that outranks `robot_id` (see `_has_priority`). Ported from @@ -1214,7 +1330,7 @@ def _blocked_by_priority_obstacle( point, _ = trajectory.state_at(collision_time) for obstacle in obstacles: owner_id = getattr(obstacle, "owner_id", None) - if owner_id is None or not _has_priority(owner_id, robot_id): + if owner_id is None or not _has_priority(owner_id, robot_id, game=game): continue d = obstacle.distance_at(collision_time, point) - config.ROBOT_RADIUS if d < config.MARGIN_BASE: @@ -1339,7 +1455,7 @@ def distance_at(self, t: float, point: Tuple[float, float]) -> float: return math.hypot(point[0] - ox, point[1] - oy) - self.radius -@dataclass(frozen=True) +@dataclass(frozen=True, slots=True) class TwoSegmentTrajectory: """A trajectory to an intermediate target, switched at `switch_t` for a fresh trajectory toward the real final target -- paper section 2.2. diff --git a/utama_core/rsoccer_simulator/src/Entities/Frame.py b/utama_core/rsoccer_simulator/src/Entities/Frame.py index 0a1dcbd8..ea86510e 100644 --- a/utama_core/rsoccer_simulator/src/Entities/Frame.py +++ b/utama_core/rsoccer_simulator/src/Entities/Frame.py @@ -61,8 +61,11 @@ def parse(self, state, n_blues=3, n_yellows=3): rbt_obs = 11 for i in range(n_blues): - robot = Robot() - robot.id = i + robot = self.robots_blue.get(i) + if robot is None: + robot = Robot() + robot.id = i + self.robots_blue[i] = robot robot.x = state[5 + (rbt_obs * i) + 0] robot.y = state[5 + (rbt_obs * i) + 1] robot.theta = state[5 + (rbt_obs * i) + 2] @@ -74,20 +77,22 @@ def parse(self, state, n_blues=3, n_yellows=3): robot.v_wheel1 = state[5 + (rbt_obs * i) + 8] robot.v_wheel2 = state[5 + (rbt_obs * i) + 9] robot.v_wheel3 = state[5 + (rbt_obs * i) + 10] - self.robots_blue[robot.id] = robot + offset_yellow = 5 + n_blues * rbt_obs for i in range(n_yellows): - robot = Robot() - robot.id = i - robot.x = state[5 + n_blues * rbt_obs + (rbt_obs * i) + 0] - robot.y = state[5 + n_blues * rbt_obs + (rbt_obs * i) + 1] - robot.theta = state[5 + n_blues * rbt_obs + (rbt_obs * i) + 2] - robot.v_x = state[5 + n_blues * rbt_obs + (rbt_obs * i) + 3] - robot.v_y = state[5 + n_blues * rbt_obs + (rbt_obs * i) + 4] - robot.v_theta = state[5 + n_blues * rbt_obs + (rbt_obs * i) + 5] - robot.infrared = bool(state[5 + n_blues * rbt_obs + (rbt_obs * i) + 6]) - robot.v_wheel0 = state[5 + n_blues * rbt_obs + (rbt_obs * i) + 7] - robot.v_wheel1 = state[5 + n_blues * rbt_obs + (rbt_obs * i) + 8] - robot.v_wheel2 = state[5 + n_blues * rbt_obs + (rbt_obs * i) + 9] - robot.v_wheel3 = state[5 + n_blues * rbt_obs + (rbt_obs * i) + 10] - self.robots_yellow[robot.id] = robot + robot = self.robots_yellow.get(i) + if robot is None: + robot = Robot() + robot.id = i + self.robots_yellow[i] = robot + robot.x = state[offset_yellow + (rbt_obs * i) + 0] + robot.y = state[offset_yellow + (rbt_obs * i) + 1] + robot.theta = state[offset_yellow + (rbt_obs * i) + 2] + robot.v_x = state[offset_yellow + (rbt_obs * i) + 3] + robot.v_y = state[offset_yellow + (rbt_obs * i) + 4] + robot.v_theta = state[offset_yellow + (rbt_obs * i) + 5] + robot.infrared = bool(state[offset_yellow + (rbt_obs * i) + 6]) + robot.v_wheel0 = state[offset_yellow + (rbt_obs * i) + 7] + robot.v_wheel1 = state[offset_yellow + (rbt_obs * i) + 8] + robot.v_wheel2 = state[offset_yellow + (rbt_obs * i) + 9] + robot.v_wheel3 = state[offset_yellow + (rbt_obs * i) + 10] diff --git a/utama_core/rsoccer_simulator/src/Simulators/robosim/robosim_wrapper.py b/utama_core/rsoccer_simulator/src/Simulators/robosim/robosim_wrapper.py index e230816e..95f46ecc 100644 --- a/utama_core/rsoccer_simulator/src/Simulators/robosim/robosim_wrapper.py +++ b/utama_core/rsoccer_simulator/src/Simulators/robosim/robosim_wrapper.py @@ -21,16 +21,24 @@ logger = logging.getLogger(__name__) +try: + import robosim + + _HAS_NATIVE_ROBOSIM = True +except ImportError: + _HAS_NATIVE_ROBOSIM = False + # Sim reuse (opt-in, `enable_sim_reuse`): a process that plays many short scenarios keeps its # sim subprocesses, since starting one (`pixi run`, imports, world build) costs about as much # CPU as a second of play. Idle sims wait here by their constructor arguments; `acquire` # hands one out with a new native world, so nothing of the previous play is left in it. _reuse_sims = False _idle_sims: dict[tuple, list["RSimSubprocessWrapper"]] = {} +_idle_native_sims: dict[tuple, list["RSimNativeWrapper"]] = {} def enable_sim_reuse() -> None: - """From now on, `close()` on a sim from `acquire` parks its subprocess for the next `acquire` + """From now on, `close()` on a sim from `acquire` parks its simulator for the next `acquire` (closed for good at interpreter exit) instead of terminating it.""" global _reuse_sims if not _reuse_sims: @@ -42,6 +50,102 @@ def _close_idle_sims() -> None: for sims in _idle_sims.values(): while sims: sims.pop()._terminate() + for nsims in _idle_native_sims.values(): + while nsims: + nsims.pop().close() + + +class RSimNativeWrapper: + """In-process native C++ wrapper around `robosim` for zero-IPC simulation.""" + + @classmethod + def acquire(cls, sim_type, n_blue, n_yellow, field_type, time_step_ms) -> "RSimNativeWrapper": + key = (sim_type, n_blue, n_yellow, field_type, time_step_ms) + idle = _idle_native_sims.get(key) + while idle: + sim = idle.pop() + sim.new_world() + sim._released = False + return sim + return cls(*key) + + def __init__(self, sim_type, n_blue, n_yellow, field_type, time_step_ms): + self._key = (sim_type, n_blue, n_yellow, field_type, time_step_ms) + self._released = False + self.sim_type = sim_type + self.n_blue = n_blue + self.n_yellow = n_yellow + self.field_type = field_type + self.time_step_ms = time_step_ms + self.new_world() + + def new_world(self): + blue_robots_pos = [[-0.2 * i, 0, 0] for i in range(1, self.n_blue + 1)] + yellow_robots_pos = [[0.2 * i, 0, 0] for i in range(1, self.n_yellow + 1)] + if self.sim_type == "VSS": + self.sim = robosim.VSS( + self.field_type, + self.n_blue, + self.n_yellow, + self.time_step_ms, + [0, 0, 0, 0], + blue_robots_pos, + yellow_robots_pos, + ) + else: + self.sim = robosim.SSL( + self.field_type, + self.n_blue, + self.n_yellow, + self.time_step_ms, + [0, 0, 0, 0], + blue_robots_pos, + yellow_robots_pos, + ) + + def step(self, commands: np.ndarray) -> np.ndarray: + if hasattr(self.sim, "step_fast"): + self.sim.step_fast(commands) + return self.sim.get_state_array() + self.sim.step(commands) + return np.array(self.sim.get_state(), dtype=np.float64) + + def reset(self, ball_pos, blue_robots_pos, yellow_robots_pos): + self.sim.reset( + np.array(ball_pos), + np.array(blue_robots_pos), + np.array(yellow_robots_pos), + ) + + def get_field_params(self): + return self.sim.get_field_params() + + def get_state(self): + if hasattr(self.sim, "get_state_array"): + return self.sim.get_state_array() + return np.array(self.sim.get_state(), dtype=np.float64) + + def close(self): + if self._released: + return + if _reuse_sims: + self._released = True + _idle_native_sims.setdefault(self._key, []).append(self) + return + self.sim = None + + +def acquire_simulator(sim_type, n_blue, n_yellow, field_type, time_step_ms): + """Acquire a simulator. + + Subprocess mode is preferred by default: running ODE physics in a dedicated + subprocess pipelines physics computation on a separate CPU core from Python's + strategy/tactics loop, giving significantly higher end-to-end throughput. + In-process native robosim can be explicitly selected via UTAMA_USE_NATIVE_RSIM=1. + """ + if os.environ.get("UTAMA_USE_NATIVE_RSIM") and _HAS_NATIVE_ROBOSIM: + return RSimNativeWrapper.acquire(sim_type, n_blue, n_yellow, field_type, time_step_ms) + return RSimSubprocessWrapper.acquire(sim_type, n_blue, n_yellow, field_type, time_step_ms) class RSimSubprocessWrapper: @@ -76,20 +180,25 @@ def __init__(self, sim_type, n_blue, n_yellow, field_type, time_step_ms): env["SKBUILD_CMAKE_ARGS"] = f"{env.get('SKBUILD_CMAKE_ARGS', '')} {cmake_policy_flag}".strip() project_root = Path(__file__).resolve().parents[5] robosim_env = project_root / ".pixi" / "envs" / "robosim" + robosim_python = robosim_env / "bin" / "python" include_dir = robosim_env / "include" lib_dir = robosim_env / "lib" prefix = str(robosim_env) env["CMAKE_PREFIX_PATH"] = f"{prefix}:{env.get('CMAKE_PREFIX_PATH', '')}".strip(":") env["CMAKE_LIBRARY_PATH"] = f"{lib_dir}:{env.get('CMAKE_LIBRARY_PATH', '')}".strip(":") env["CMAKE_INCLUDE_PATH"] = f"{include_dir}:{env.get('CMAKE_INCLUDE_PATH', '')}".strip(":") + env["CONDA_PREFIX"] = prefix + env["PATH"] = f"{robosim_env}/bin:{env.get('PATH', '')}" + env["LD_LIBRARY_PATH"] = f"{lib_dir}:{env.get('LD_LIBRARY_PATH', '')}" + + if robosim_python.exists(): + cmd = [str(robosim_python)] + else: + cmd = ["pixi", "run", "--environment", "robosim", "--", "python"] + self.proc = subprocess.Popen( - [ - "pixi", - "run", - "--environment", - "robosim", - "--", - "python", + cmd + + [ str(script_path), "--sim_type", sim_type, diff --git a/utama_core/rsoccer_simulator/src/Simulators/rsim.py b/utama_core/rsoccer_simulator/src/Simulators/rsim.py index 4782ac8d..d430031f 100644 --- a/utama_core/rsoccer_simulator/src/Simulators/rsim.py +++ b/utama_core/rsoccer_simulator/src/Simulators/rsim.py @@ -5,6 +5,7 @@ from utama_core.rsoccer_simulator.src.Entities import Field, Frame, FrameSSL, FrameVSS from utama_core.rsoccer_simulator.src.Simulators.robosim.robosim_wrapper import ( RSimSubprocessWrapper, + acquire_simulator, ) @@ -138,7 +139,7 @@ def _init_simulator( n_robots_yellow, time_step_ms, ) -> RSimSubprocessWrapper: - return RSimSubprocessWrapper.acquire( + return acquire_simulator( sim_type="VSS", n_blue=n_robots_blue, n_yellow=n_robots_yellow, @@ -148,8 +149,13 @@ def _init_simulator( class RSimSSL(RSim): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self._sim_cmds = np.zeros((self.n_robots_blue + self.n_robots_yellow, 8), dtype=np.float64) + def send_commands(self, commands): - sim_cmds = np.zeros((self.n_robots_blue + self.n_robots_yellow, 8), dtype=np.float64) + sim_cmds = self._sim_cmds + sim_cmds.fill(0.0) for cmd in commands: if cmd.yellow: @@ -197,7 +203,7 @@ def _init_simulator( n_robots_yellow, time_step_ms, ) -> RSimSubprocessWrapper: - return RSimSubprocessWrapper.acquire( + return acquire_simulator( sim_type="SSL", n_blue=n_robots_blue, n_yellow=n_robots_yellow, diff --git a/utama_core/run/strategy_runner.py b/utama_core/run/strategy_runner.py index 0d2bf3a0..5696a9f2 100644 --- a/utama_core/run/strategy_runner.py +++ b/utama_core/run/strategy_runner.py @@ -308,6 +308,7 @@ class SideRuntime: velocity_refiner: VelocityRefiner robot_info_refiner: RobotInfoRefiner motion_controller: type[MotionController] + control_scheme: str = "fpp" game: Optional[Game] = field(init=False, default=None) game_history: Optional[GameHistory] = field(init=False, default=None) @@ -332,8 +333,10 @@ class StrategyRunner: gRSim/SSL-Vision — a RuntimeError is raised on the first vision packet if they differ. field_bounds (FieldBounds): Bounds of the subset of the full field being used. Defaults to None (ie full field). opp_strategy (AbstractStrategy, optional): Opponent strategy for pvp. Defaults to None for single player. - control_scheme (str, optional): Name of the motion control scheme to use. - opp_control_scheme (str, optional): Name of the opponent motion control scheme to use. If not set, uses same as friendly. + control_scheme (str, optional): Name of the motion control scheme to use. If not set (or None), + defaults to strategy.control_scheme, falling back to 'fpp'. + opp_control_scheme (str, optional): Name of the opponent motion control scheme to use. If not set, + defaults to opp_strategy.control_scheme, falling back to the friendly control scheme. replay_writer_config (ReplayWriterConfig | ColumnarReplayWriterConfig, optional): Configuration for the replay writer. If unset, replay is disabled. `ReplayWriterConfig` writes the original one-pickle- per-frame `.pkl` format; `ColumnarReplayWriterConfig` writes the columnar `.npz` format, ~5x smaller @@ -396,7 +399,7 @@ def __init__( full_field_dims: FieldDimensions = STANDARD_FIELD_DIMS, field_bounds: Optional[FieldBounds] = None, opp_strategy: Optional[AbstractStrategy] = None, - control_scheme: str = "fpp", # This is also the default control scheme used in the motion planning tests + control_scheme: Optional[str] = None, # If None, resolved from strategy.control_scheme (defaulting to "fpp") opp_control_scheme: Optional[str] = None, replay_writer_config: Optional[Union[ReplayWriterConfig, ColumnarReplayWriterConfig]] = None, show_live_status: bool = False, # Turn this on for simulator debugging @@ -514,6 +517,8 @@ def __init__( allowed_yellow_ids=_allowed_yellow, allowed_blue_ids=_allowed_blue, ) + self.control_scheme = self.my.control_scheme + self.opp_control_scheme = self.opp.control_scheme if self.opp is not None else None ### functions below rely on self.my and self.opp ### @@ -800,7 +805,7 @@ def _setup_sides_data( my_strategy: AbstractStrategy, opp_strategy: Optional[AbstractStrategy], filtering: bool, - control_scheme: str, + control_scheme: Optional[str], opp_control_scheme: Optional[str], my_trusted_ir_robots: Optional[FrozenSet[int]] = None, opp_trusted_ir_robots: Optional[FrozenSet[int]] = None, @@ -812,8 +817,8 @@ def _setup_sides_data( my_strategy (AbstractStrategy): The strategy for the friendly team. opp_strategy (Optional[AbstractStrategy]): The strategy for the opponent team. If None, opponent side will be None. filtering (bool): Whether to use filtering in the position refiners. - control_scheme (str): Name of the motion control scheme to use for the friendly team. - opp_control_scheme (Optional[str]): Name of the motion control scheme to use for the opponent team. If not set, uses same as friendly. + control_scheme (Optional[str]): Name of the motion control scheme to use for the friendly team. If None, uses strategy.control_scheme or 'fpp'. + opp_control_scheme (Optional[str]): Name of the motion control scheme to use for the opponent team. If not set, uses opp_strategy.control_scheme or friendly scheme. my_trusted_ir_robots (FrozenSet[int], optional): Vision IDs of friendly robots whose IR sensor is trusted. opp_trusted_ir_robots (FrozenSet[int], optional): Vision IDs of opponent robots whose IR sensor is trusted. allowed_yellow_ids (FrozenSet[int], optional): Vision IDs of yellow robots in play; others are ignored. @@ -833,13 +838,17 @@ def _setup_sides_data( allowed_yellow_ids=allowed_yellow_ids, allowed_blue_ids=allowed_blue_ids, ) - my_motion_controller = get_control_scheme(control_scheme) + my_scheme = ( + control_scheme if control_scheme is not None else getattr(my_strategy, "control_scheme", None) or "fpp" + ) + my_motion_controller = get_control_scheme(my_scheme) my_side = SideRuntime( strategy=my_strategy, position_refiner=my_pos_ref, velocity_refiner=my_vel_ref, robot_info_refiner=my_robot_ref, motion_controller=my_motion_controller, + control_scheme=my_scheme, ) if opp_strategy is not None: @@ -851,15 +860,22 @@ def _setup_sides_data( allowed_yellow_ids=allowed_yellow_ids, allowed_blue_ids=allowed_blue_ids, ) - opp_motion_controller = ( - get_control_scheme(opp_control_scheme) if opp_control_scheme is not None else my_motion_controller - ) + if opp_control_scheme is not None: + opp_scheme = opp_control_scheme + elif getattr(opp_strategy, "control_scheme", None) is not None: + opp_scheme = opp_strategy.control_scheme + elif control_scheme is not None: + opp_scheme = my_scheme + else: + opp_scheme = my_scheme + opp_motion_controller = get_control_scheme(opp_scheme) opp_side = SideRuntime( strategy=opp_strategy, position_refiner=opp_pos_ref, velocity_refiner=opp_vel_ref, robot_info_refiner=opp_robot_ref, motion_controller=opp_motion_controller, + control_scheme=opp_scheme, ) return my_side, opp_side diff --git a/utama_core/tests/analysis/test_motion_stats.py b/utama_core/tests/analysis/test_motion_stats.py new file mode 100644 index 00000000..e77b68cd --- /dev/null +++ b/utama_core/tests/analysis/test_motion_stats.py @@ -0,0 +1,74 @@ +"""Tests for `utama_core.analysis.motion_stats`.""" + +from __future__ import annotations + +import json +from pathlib import Path + +import numpy as np +import pytest + +from utama_core.analysis.motion_stats import ( + compute_motion_metrics_from_arrays, + compute_motion_metrics_from_replay, + enrich_tournament_directory, +) + + +def test_compute_motion_metrics_from_arrays(): + # 3 ticks, 2 friendly robots, 1 enemy robot + # Tick 0: friendly overlap (dist=0.10, two_r=0.18 -> clearance=-0.08m) + # Tick 1: near miss friendly vs enemy (dist=0.20 -> clearance=0.02m) + # Tick 2: completely safe (dist >= 1.0m) + fp = np.zeros((3, 2, 2), dtype=np.float32) + ep = np.zeros((3, 1, 2), dtype=np.float32) + + fp[0, 0] = [0.0, 0.0] + fp[0, 1] = [0.10, 0.0] + ep[0, 0] = [5.0, 5.0] + + fp[1, 0] = [0.0, 0.0] + fp[1, 1] = [2.0, 2.0] + ep[1, 0] = [0.20, 0.0] + + fp[2, 0] = [0.0, 0.0] + fp[2, 1] = [2.0, 2.0] + ep[2, 0] = [5.0, 5.0] + + res = compute_motion_metrics_from_arrays(fp, ep) + assert res["robot_overlap_ticks"] == 1 + assert res["robot_overlap_incidents"] == 1 + assert res["near_miss_ticks"] == 1 + assert res["teammate_overlap_ticks"] == {"friendly": 1, "enemy": 0} + assert res["teammate_overlap_incidents"] == {"friendly": 1, "enemy": 0} + assert res["opponent_overlap_ticks"] == 0 + assert res["opponent_overlap_incidents"] == 0 + assert res["worst_clearance_m"] == pytest.approx(-0.08, abs=1e-3) + assert res["total_ticks"] == 3 + + +def test_enrich_tournament_directory(tmp_path: Path): + fp = np.zeros((2, 2, 2), dtype=np.float32) + ep = np.zeros((2, 1, 2), dtype=np.float32) + fp[0, 0] = [0.0, 0.0] + fp[0, 1] = [0.10, 0.0] + ep[0, 0] = [2.0, 2.0] + fp[1, 0] = [0.0, 0.0] + fp[1, 1] = [2.0, 2.0] + ep[1, 0] = [5.0, 5.0] + + npz_path = tmp_path / "strat_a_vs_strat_b.npz" + np.savez(npz_path, friendly_p=fp, enemy_p=ep) + + stats_path = tmp_path / "strat_a_vs_strat_b.stats.json" + with open(stats_path, "w") as f: + json.dump({"rule_event_counts": {}}, f) + + enrich_tournament_directory(tmp_path) + + with open(stats_path) as f: + data = json.load(f) + assert data["robot_overlap_ticks"] == 1 + assert data["robot_overlap_incidents"] == 1 + assert data["teammate_overlap_incidents"] == {"friendly": 1, "enemy": 0} + assert data["worst_clearance_m"] == pytest.approx(-0.08, abs=1e-3) diff --git a/utama_core/tests/engine/test_match_stats.py b/utama_core/tests/engine/test_match_stats.py index 697b0534..cb3ccc2f 100644 --- a/utama_core/tests/engine/test_match_stats.py +++ b/utama_core/tests/engine/test_match_stats.py @@ -757,3 +757,45 @@ def test_foul_log_names_the_robot_and_its_tactic(): ("excessive_dribbling", "enemy", 2, "attack", False), ("keeper_held_ball", "friendly", 3, "press", True), ] + + +def test_motion_planning_collision_and_clearance_tracking(tmp_path): + acc = MatchStatsAccumulator() + # Frame 1: friendly robots overlap (centers at (0, 0) and (0.10, 0) -> dist=0.10, clearance=0.10 - 0.18 = -0.08m) + f1 = {1: _robot(1, 0.0, 0.0, True), 2: _robot(2, 0.10, 0.0, True)} + e1 = {3: _robot(3, 2.0, 0.0, False)} + acc.record_tick(_frame(f1, e1, ball_xy=(0.5, 0.0))) + + # Frame 2: near miss between friendly and enemy (centers at (0, 0) and (0.20, 0) -> clearance=0.02m) + f2 = {1: _robot(1, 0.0, 0.0, True), 2: _robot(2, 1.0, 1.0, True)} + e2 = {3: _robot(3, 0.20, 0.0, False)} + acc.record_tick(_frame(f2, e2, ball_xy=(0.5, 0.0))) + + # Frame 3: safe separation (all robots >= 0.5m apart) + f3 = {1: _robot(1, 0.0, 0.0, True), 2: _robot(2, 1.0, 1.0, True)} + e3 = {3: _robot(3, 2.0, 2.0, False)} + acc.record_tick(_frame(f3, e3, ball_xy=(0.5, 0.0))) + + stats = acc.finalize() + assert stats.robot_overlap_ticks == 1 + assert stats.robot_overlap_incidents == 1 + assert stats.near_miss_ticks == 1 + assert stats.teammate_overlap_ticks == {"friendly": 1, "enemy": 0} + assert stats.teammate_overlap_incidents == {"friendly": 1, "enemy": 0} + assert stats.opponent_overlap_ticks == 0 + assert stats.opponent_overlap_incidents == 0 + assert stats.worst_clearance_m == pytest.approx(-0.08, abs=1e-3) + + # Test serialization to json + json_path = tmp_path / "stats.json" + stats.to_json(json_path) + with open(json_path) as f: + data = json.load(f) + assert data["robot_overlap_ticks"] == 1 + assert data["robot_overlap_incidents"] == 1 + assert data["near_miss_ticks"] == 1 + assert data["teammate_overlap_ticks"] == {"friendly": 1, "enemy": 0} + assert data["teammate_overlap_incidents"] == {"friendly": 1, "enemy": 0} + assert data["opponent_overlap_ticks"] == 0 + assert data["opponent_overlap_incidents"] == 0 + assert data["worst_clearance_m"] == pytest.approx(-0.08, abs=1e-3) diff --git a/utama_core/tests/motion_planning/fastpathplanner_equivalence_test.py b/utama_core/tests/motion_planning/fastpathplanner_equivalence_test.py index d56afa86..601e2636 100644 --- a/utama_core/tests/motion_planning/fastpathplanner_equivalence_test.py +++ b/utama_core/tests/motion_planning/fastpathplanner_equivalence_test.py @@ -379,3 +379,59 @@ def test_clamp_to_obstacle_clearance_matches_reference(seed): a = new._clamp_to_obstacle_clearance(*args, clearance=clearance) b = ref._clamp_to_obstacle_clearance(*args, clearance=clearance) assert a == b and type(a) is type(b) + + +def test_obstacle_cache_and_array_prebuild(): + """Static obstacles are cached across ticks and _get_obstacles seeds _obstacle_arrays_cache.""" + from utama_core.entities.data.vector import Vector2D + from utama_core.entities.game import Game + from utama_core.entities.game.field import FieldBounds + + planner = FastPathPlanner(env=None) + bounds = FieldBounds(top_left=(-4.5, 3.0), bottom_right=(4.5, -3.0)) + + class DummyRobot: + def __init__(self, rid, x, y, vx, vy): + self.id = rid + self.p = Vector2D(x, y) + self.v = Vector2D(vx, vy) + + class DummyField: + enemy_defense_area = np.array([[3.5, 1.0], [4.5, 1.0], [4.5, -1.0], [3.5, -1.0]]) + my_team_is_right = False + + class DummyGame: + ts = 1.0 + field = DummyField() + friendly_robots = {0: DummyRobot(0, 0.0, 0.0, 0.5, 0.0), 1: DummyRobot(1, 1.0, 1.0, 0.0, 0.0)} + enemy_robots = {0: DummyRobot(0, 2.0, 2.0, -0.5, 0.0)} + + game = DummyGame() + + # First call: builds static obstacles + planner._refresh_obstacle_cache(game, bounds) + static_1 = planner._obstacle_cache_static + static_arrays_1 = planner._obstacle_cache_static_arrays + assert len(static_1) == 8 + assert static_arrays_1 is not None + + # Second call with same bounds and field: reuses exact same static objects + game.ts = 1.1 + planner._refresh_obstacle_cache(game, bounds) + assert planner._obstacle_cache_static is static_1 + assert planner._obstacle_cache_static_arrays is static_arrays_1 + + # _get_obstacles seeds _obstacle_arrays_cache with matching arrays + planner._obstacle_arrays_cache.clear() + our_pos = np.array([0.0, 0.0]) + obstacles, clearance, subgoal_dist = planner._get_obstacles(game, 0, our_pos, bounds) + assert len(planner._obstacle_arrays_cache) == 1 + cached_obs, cached_arrs = planner._obstacle_arrays_cache[0] + assert cached_obs is obstacles + from utama_core.motion_planning.src.fastpathplanning.numba_kernels import ( + flatten_obstacles, + ) + + expected_arrs = flatten_obstacles(obstacles) + for arr, exp in zip(cached_arrs, expected_arrs): + assert np.array_equal(arr, exp) diff --git a/utama_core/tests/motion_planning/implementation/trajsampling_correctness_test.py b/utama_core/tests/motion_planning/implementation/trajsampling_correctness_test.py index 8edf7547..08723b6e 100644 --- a/utama_core/tests/motion_planning/implementation/trajsampling_correctness_test.py +++ b/utama_core/tests/motion_planning/implementation/trajsampling_correctness_test.py @@ -683,10 +683,10 @@ def test_plan_exempts_ball_as_obstacle_when_targeting_it(): first_call_cv_row_count = [] original_first_collision = planner._first_collision - def _traced_first_collision(trajectory, obstacle_arrays, start_t=0.0): + def _traced_first_collision(trajectory, obstacle_arrays, *args, **kwargs): if not first_call_cv_row_count: first_call_cv_row_count.append(obstacle_arrays[2].shape[0]) - return original_first_collision(trajectory, obstacle_arrays, start_t) + return original_first_collision(trajectory, obstacle_arrays, *args, **kwargs) planner._first_collision = _traced_first_collision @@ -1084,3 +1084,97 @@ def test_with_current_clearance_still_reports_a_genuinely_approaching_obstacle() expected = approaching.distance_at(0.0, p0) - config.ROBOT_RADIUS assert updated.nearest_obstacle_distance == pytest.approx(expected) + + +def _rank_scene(): + # Robot at the origin, target 1 m ahead. A parked robot 0.6 m ahead stops the + # direct line near the target; another 1.5 m behind stops a retreat later. + obstacles = [ + ConstantVelocityObstacle(p0=(0.6, 0.0), v=(0.0, 0.0), radius=0.09), + ConstantVelocityObstacle(p0=(-1.5, 0.0), v=(0.0, 0.0), radius=0.09), + StaticSegmentObstacle(a=(0.0, 0.5), b=(0.0, 3.0)), + ] + return TrajectorySamplingPlanner(v_max=2.0, a_max=2.0), _flatten_obstacles(obstacles), (1.0, 0.0) + + +def test_fallback_prefers_the_candidate_that_collides_nearest_the_target(): + """When every candidate collides, the fallback is the one that gets closest + to the target before its first collision (TIGERs' `getBestRejectedResult`), + not the one that survives longest: that preferred a retreat colliding late, + far from the target, over a direct run stopped just short of it.""" + planner, arrays, target = _rank_scene() + direct = Trajectory2D.compute((0.0, 0.0), (0.0, 0.0), target, 2.0, 2.0) + retreat = Trajectory2D.compute((0.0, 0.0), (0.0, 0.0), (-3.0, 0.0), 2.0, 2.0) + t_direct, t_retreat = planner._first_collision(direct, arrays), planner._first_collision(retreat, arrays) + assert t_direct is not None and t_retreat is not None and t_retreat > t_direct # survival time favours the retreat + assert planner._fallback_rank(direct, t_direct, False, arrays, target) > planner._fallback_rank( + retreat, t_retreat, False, arrays, target + ) + + +def test_fallback_ranks_a_static_collision_below_any_moving_one(): + """A candidate whose first collision is a static obstacle (a field line, a + defence area) won't clear by waiting, so it ranks below every moving + collision even when it collides nearer the target -- and above a candidate + blocked by a higher-priority teammate.""" + planner, arrays, target = _rank_scene() + into_wall = Trajectory2D.compute((0.0, 0.0), (0.0, 0.0), (0.0, 2.0), 2.0, 2.0) + retreat = Trajectory2D.compute((0.0, 0.0), (0.0, 0.0), (-3.0, 0.0), 2.0, 2.0) + t_wall, t_retreat = planner._first_collision(into_wall, arrays), planner._first_collision(retreat, arrays) + wall_rank = planner._fallback_rank(into_wall, t_wall, False, arrays, target) + assert wall_rank < planner._fallback_rank(retreat, t_retreat, False, arrays, target) + assert wall_rank > planner._fallback_rank(retreat, t_retreat, True, arrays, target) + + +def test_enemy_obstacle_lateral_clearance_is_tube_not_inflated_sphere(): + """An opponent moving along +x has reachable forward acceleration and braking + bounds along x, but its body width orthogonal to travel (in y) is strictly its + robot radius, not an inflated circle of radius (radius + r_dyn). A query point + located laterally at y=0.35m from an opponent traveling along y=0 must have + clearance equal to 0.35 - radius, without being encroached upon by r_dyn.""" + from utama_core.motion_planning.src.trajsampling.obstacles import EnemyRobotObstacle + + obs = EnemyRobotObstacle( + p0=(0.0, 0.0), + speed=1.5, + direction=(1.0, 0.0), + v_max=3.0, + a_max=3.0, + radius=0.09, + t_max=0.5, + ) + # At t=0.3s, opponent has moved forward to x in [0.315, 0.585]. + # At x=0.4, y=0.35 (directly beside the tube): + # Under tube model: distance to the segment along y=0 is exactly 0.35 - 0.09 = 0.26m. + # Under the old spherical model, r_dyn (~0.135m) was added to radius, reducing clearance to ~0.125m. + d = obs.distance_at(0.3, (0.4, 0.35)) + assert math.isclose(d, 0.35 - 0.09, abs_tol=1e-6) + + +def test_has_priority_role_aware_ball_carrier_and_fallback(): + """Ball carrier must take priority over off-ball teammates regardless of robot ID. + When neither has the ball, or game is None, falls back to ID-based tie breaking.""" + from dataclasses import replace + + from utama_core.motion_planning.src.trajsampling.planner import _has_priority + + # Standalone (no game): higher ID wins + assert _has_priority(2, 1, game=None) is True + assert _has_priority(1, 2, game=None) is False + + # With game: robot 1 has ball, robot 5 does not + game, _ = _game((0.0, 0.0), (0.0, 0.0)) + r1 = replace(_robot(1, 0.0, 0.0, True), has_ball=True) + r5 = replace(_robot(5, 1.0, 0.0, True), has_ball=False) + game.friendly_robots[1] = r1 + game.friendly_robots[5] = r5 + + # Robot 1 has lower ID than 5, but HAS BALL -> outranks robot 5! + assert _has_priority(1, 5, game=game) is True + assert _has_priority(5, 1, game=game) is False + + # Neither has ball -> falls back to higher ID (5 outranks 1) + r1_noball = replace(r1, has_ball=False) + game.friendly_robots[1] = r1_noball + assert _has_priority(1, 5, game=game) is False + assert _has_priority(5, 1, game=game) is True diff --git a/utama_core/tests/motion_planning/test_fastpathplanner_cpp.py b/utama_core/tests/motion_planning/test_fastpathplanner_cpp.py new file mode 100644 index 00000000..5e10553f --- /dev/null +++ b/utama_core/tests/motion_planning/test_fastpathplanner_cpp.py @@ -0,0 +1,157 @@ +"""Tests for the native C++ FastPathPlanner extension (_fastpathplanner). + +Verifies bit-for-bit / epsilon equivalence between the C++ Pybind11 implementation +and the reference Python/Numba implementations for all core geometric functions: +- collides +- find_subgoal +- clamp_to_obstacle_clearance +- sanitize_target +- check_segment (recursive detour tree search) +- smooth_path (lookahead carrot projection) + +The extension is built locally (`pixi run build-planner`) and is not built in CI, +so the module is skipped when it is missing; the planner then uses the numba kernels. +""" + +import numpy as np +import pytest + +_fastpathplanner = pytest.importorskip("utama_core.motion_planning.src.fastpathplanning._fastpathplanner") + +from utama_core.motion_planning.src.fastpathplanning import numba_kernels # noqa: E402 +from utama_core.motion_planning.src.fastpathplanning.planner import ( # noqa: E402 + HAS_CPP, + FastPathPlanner, +) + +_TOL = 1e-9 + + +def test_cpp_extension_is_loaded(): + assert HAS_CPP is True, "The C++ native _fastpathplanner extension must be built and importable" + assert _fastpathplanner is not None + + +@pytest.mark.parametrize("seed", [1, 2, 3, 42]) +def test_cpp_collides_matches_numba(seed): + rng = np.random.default_rng(seed) + n = 15 + ox0 = rng.uniform(-4.0, 4.0, n) + oy0 = rng.uniform(-3.0, 3.0, n) + ox1 = ox0 + rng.uniform(-0.5, 0.5, n) + oy1 = oy0 + rng.uniform(-0.5, 0.5, n) + + seg_a = rng.uniform(-3.0, 3.0, 2) + seg_b = rng.uniform(-3.0, 3.0, 2) + clearance = float(rng.uniform(0.15, 0.35)) + sticky_idx = int(rng.integers(0, n)) if rng.random() < 0.5 else -1 + + c_cpp = _fastpathplanner.collides_nb( + seg_a[0], seg_a[1], seg_b[0], seg_b[1], clearance, ox0, oy0, ox1, oy1, sticky_idx, 0.05 + ) + c_nb = numba_kernels.collides_nb( + seg_a[0], seg_a[1], seg_b[0], seg_b[1], clearance, ox0, oy0, ox1, oy1, sticky_idx, 0.05 + ) + + idx_cpp, from_stk_cpp, px_cpp, py_cpp = c_cpp + idx_nb, from_stk_nb, px_nb, py_nb = c_nb + + assert idx_cpp == idx_nb + assert from_stk_cpp == from_stk_nb + if idx_cpp >= 0: + assert np.isclose(px_cpp, px_nb, atol=_TOL) + assert np.isclose(py_cpp, py_nb, atol=_TOL) + + +@pytest.mark.parametrize("seed", [10, 20, 30]) +def test_cpp_find_subgoal_matches_numba(seed): + rng = np.random.default_rng(seed) + n = 10 + ox0 = rng.uniform(-3.0, 3.0, n) + oy0 = rng.uniform(-2.0, 2.0, n) + ox1 = ox0 + rng.uniform(-0.4, 0.4, n) + oy1 = oy0 + rng.uniform(-0.4, 0.4, n) + + op = rng.uniform(-2.0, 2.0, 2) + angle = rng.uniform(0, 2 * np.pi) + ux, uy = np.cos(angle), np.sin(angle) + clearance = 0.22 + subgoal_dist = 0.35 + + res_cpp = _fastpathplanner.find_subgoal_nb( + op[0], op[1], ux, uy, subgoal_dist, 1, clearance, ox0, oy0, ox1, oy1, False, 0.0, 0.0, 0.0, 0.0 + ) + res_nb = numba_kernels.find_subgoal_nb( + op[0], op[1], ux, uy, subgoal_dist, 1, clearance, ox0, oy0, ox1, oy1, False, 0.0, 0.0, 0.0, 0.0 + ) + + assert res_cpp[0] == res_nb[0] # found + if res_cpp[0]: + assert np.isclose(res_cpp[1], res_nb[1], atol=_TOL) + assert np.isclose(res_cpp[2], res_nb[2], atol=_TOL) + assert res_cpp[3] == res_nb[3] # last_hit_idx + assert res_cpp[4] == res_nb[4] # rect_last + + +def test_cpp_check_segment_and_smooth_path(): + planner = FastPathPlanner(env=None) + + ox0 = np.array([-1.0, 0.0, 1.0]) + oy0 = np.array([0.0, -1.0, 2.0]) + ox1 = np.array([1.0, 0.0, 2.0]) + oy1 = np.array([0.0, 1.0, 3.0]) + obstacles = [(np.array([ox0[i], oy0[i]]), np.array([ox1[i], oy1[i]])) for i in range(len(ox0))] + + class DummyBounds: + top_left = (-4.5, 3.0) + bottom_right = (4.5, -3.0) + + bounds = DummyBounds() + seg = (np.array([0.0, -2.0]), np.array([0.0, 2.0])) + target = np.array([0.0, 2.0]) + + detour_dict = {} + obs_dict = {} + traj_cpp, len_cpp = _fastpathplanner.check_segment_cpp( + seg, + ox0, + oy0, + ox1, + oy1, + None, + 0, + 3, + target, + (-4.5, 4.5, -3.0, 3.0), + 0, + 0.22, + 0.35, + None, + 0.05, + 0.05, + 2, + detour_dict, + obs_dict, + ) + + # Reference Python (with HAS_CPP=False) + import utama_core.motion_planning.src.fastpathplanning.planner as planner_mod + + planner_mod.HAS_CPP = False + try: + traj_py, len_py = planner.check_segment(seg, obstacles, 0, target, bounds, 0, 0.22, 0.35, None) + carrot_py = planner.smooth_path(traj_py, target, np.array([0.0, -2.0]), obstacles, 0.22) + finally: + planner_mod.HAS_CPP = True + + assert len(traj_cpp) == len(traj_py) + assert np.isclose(len_cpp, len_py, atol=_TOL) + for (ca, cb), (pa, pb) in zip(traj_cpp, traj_py): + assert np.allclose(ca, pa, atol=_TOL) + assert np.allclose(cb, pb, atol=_TOL) + + # Smooth path + carrot_cpp = _fastpathplanner.smooth_path_cpp( + traj_cpp, target, np.array([0.0, -2.0]), 0.5, 0.22, ox0, oy0, ox1, oy1 + ) + assert np.allclose(carrot_cpp, carrot_py, atol=_TOL) diff --git a/utama_core/tests/rsoccer_simulator/test_robosim_wrapper.py b/utama_core/tests/rsoccer_simulator/test_robosim_wrapper.py index ce71dd69..9214f49e 100644 --- a/utama_core/tests/rsoccer_simulator/test_robosim_wrapper.py +++ b/utama_core/tests/rsoccer_simulator/test_robosim_wrapper.py @@ -68,18 +68,30 @@ def test_subprocess_error_is_raised_not_swallowed(): def _descendant_running(pid: int, name: str) -> int: - """`proc.pid` is the `pixi run` launcher, whose own argv names the script too: - the script is the deepest process under it with `name` in its argv.""" + """Finds the process executing `name`. When launched via `pixi run`, `proc.pid` + is the launcher process, and the script is the deepest descendant running `name`. + When launched directly via the environment's python binary, `proc.pid` itself + is the script process.""" + with open(f"/proc/{pid}/cmdline", "rb") as f: + root_cmd = f.read() + found, pending = None, [pid] while pending: p = pending.pop(0) with open(f"/proc/{p}/cmdline", "rb") as f: if name.encode() in f.read(): found = p - for task in os.listdir(f"/proc/{p}/task"): - with open(f"/proc/{p}/task/{task}/children") as f: - pending += [int(c) for c in f.read().split()] - assert found not in (None, pid), f"no process under {pid} runs {name}" + task_dir = f"/proc/{p}/task" + if os.path.isdir(task_dir): + for task in os.listdir(task_dir): + children_file = f"/proc/{p}/task/{task}/children" + if os.path.exists(children_file): + with open(children_file) as f: + pending += [int(c) for c in f.read().split()] + assert found is not None, f"no process under {pid} runs {name}" + if found == pid: + parts = root_cmd.split(b"\x00") + assert b"python" in parts[0], f"{pid} is launcher, not script: {parts[0]}" return found diff --git a/utama_core/tests/strategy_runner/test_strategy_control_scheme.py b/utama_core/tests/strategy_runner/test_strategy_control_scheme.py new file mode 100644 index 00000000..165547a2 --- /dev/null +++ b/utama_core/tests/strategy_runner/test_strategy_control_scheme.py @@ -0,0 +1,133 @@ +"""Tests for strategy-defined control scheme configuration in AbstractStrategy and StrategyRunner.""" + +from __future__ import annotations + +import pytest + +from utama_core.engine.abstract_strategy import AbstractStrategy +from utama_core.motion_planning.src.controllers import ( + DWAController, + FastPathPlanningController, + PIDController, + TrajectorySamplingController, +) +from utama_core.run.strategy_runner import StrategyRunner +from utama_core.strategy.kernel_strategy import build_default_kernel_strategy +from utama_core.tests.strategy_runner.strat_runner_test_utils import DummyStrategy + + +def _dummy_factory(motion_controller): + return build_default_kernel_strategy((1, 2))(motion_controller) + + +class _CustomSchemeStrategy(AbstractStrategy): + control_scheme = "dwa" + + +def test_abstract_strategy_control_scheme_precedence(): + # 1. Default when unset is None + s_default = AbstractStrategy(build_kernel_strategy=_dummy_factory) + assert s_default.control_scheme is None + + # 2. Inherited from factory attribute + factory_with_scheme = lambda mc: _dummy_factory(mc) # noqa: E731 + factory_with_scheme.control_scheme = "trajsample" + s_factory = AbstractStrategy(build_kernel_strategy=factory_with_scheme) + assert s_factory.control_scheme == "trajsample" + + # 3. Inherited from class attribute + s_class = _CustomSchemeStrategy(build_kernel_strategy=_dummy_factory) + assert s_class.control_scheme == "dwa" + + # 4. Constructor argument overrides class attribute and factory attribute + s_override = _CustomSchemeStrategy(build_kernel_strategy=factory_with_scheme, control_scheme="pid") + assert s_override.control_scheme == "pid" + + +def test_strategy_runner_resolves_strategy_control_scheme(): + # Friendly strategy requests 'trajsample' + strat = AbstractStrategy(build_kernel_strategy=_dummy_factory, control_scheme="trajsample") + runner = StrategyRunner( + strategy=strat, + my_team_is_yellow=True, + my_team_is_right=True, + mode="rsim", + exp_friendly=3, + exp_enemy=3, + ) + assert runner.control_scheme == "trajsample" + assert runner.my.control_scheme == "trajsample" + assert runner.my.motion_controller == TrajectorySamplingController + + +def test_strategy_runner_defaults_to_fpp_when_unspecified(): + # Neither StrategyRunner nor DummyStrategy specifies a control_scheme + runner = StrategyRunner( + strategy=DummyStrategy(), + my_team_is_yellow=True, + my_team_is_right=True, + mode="rsim", + exp_friendly=3, + exp_enemy=3, + ) + assert runner.control_scheme == "fpp" + assert runner.my.control_scheme == "fpp" + assert runner.my.motion_controller == FastPathPlanningController + + +def test_strategy_runner_explicit_override_takes_precedence(): + # Strategy declares 'trajsample', but runner explicitly passes 'dwa' + strat = AbstractStrategy(build_kernel_strategy=_dummy_factory, control_scheme="trajsample") + runner = StrategyRunner( + strategy=strat, + my_team_is_yellow=True, + my_team_is_right=True, + mode="rsim", + exp_friendly=3, + exp_enemy=3, + control_scheme="dwa", + ) + assert runner.control_scheme == "dwa" + assert runner.my.control_scheme == "dwa" + assert runner.my.motion_controller == DWAController + + +def test_strategy_runner_asymmetric_pvp_control_schemes(): + # Friendly uses 'trajsample', opponent uses 'dwa' + strat_a = AbstractStrategy(build_kernel_strategy=_dummy_factory, control_scheme="trajsample") + strat_b = AbstractStrategy(build_kernel_strategy=_dummy_factory, control_scheme="dwa") + + runner = StrategyRunner( + strategy=strat_a, + opp_strategy=strat_b, + my_team_is_yellow=True, + my_team_is_right=True, + mode="rsim", + exp_friendly=3, + exp_enemy=3, + ) + assert runner.control_scheme == "trajsample" + assert runner.opp_control_scheme == "dwa" + assert runner.my.motion_controller == TrajectorySamplingController + assert runner.opp.motion_controller == DWAController + + +def test_strategy_runner_opp_explicit_override(): + # Friendly uses 'trajsample', opponent strategy declares 'dwa', but opp_control_scheme overrides to 'pid' + strat_a = AbstractStrategy(build_kernel_strategy=_dummy_factory, control_scheme="trajsample") + strat_b = AbstractStrategy(build_kernel_strategy=_dummy_factory, control_scheme="dwa") + + runner = StrategyRunner( + strategy=strat_a, + opp_strategy=strat_b, + my_team_is_yellow=True, + my_team_is_right=True, + mode="rsim", + exp_friendly=3, + exp_enemy=3, + opp_control_scheme="pid", + ) + assert runner.control_scheme == "trajsample" + assert runner.opp_control_scheme == "pid" + assert runner.my.motion_controller == TrajectorySamplingController + assert runner.opp.motion_controller == PIDController diff --git a/vendor/rSim/CMakeLists.txt b/vendor/rSim/CMakeLists.txt index ff00f975..7d83eb00 100644 --- a/vendor/rSim/CMakeLists.txt +++ b/vendor/rSim/CMakeLists.txt @@ -3,17 +3,29 @@ cmake_minimum_required(VERSION 3.14) project(robosim) set(CMAKE_CXX_STANDARD 11) +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -march=native -DNDEBUG") + # Define CMAKE_INSTALL_xxx: LIBDIR, INCLUDEDIR include(GNUInstallDirs) include(FindPackageHandleStandardArgs) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}) + +if(DEFINED PYTHON_EXECUTABLE) + set(PYTHON_EXECUTABLE ${PYTHON_EXECUTABLE} CACHE FILEPATH "Path to Python executable" FORCE) +endif() +find_package(PythonInterp REQUIRED) +find_package(PythonLibs REQUIRED) + # Fetch pybind11 include(FetchContent) FetchContent_Declare( pybind11 - URL https://github.com/pybind/pybind11/archive/refs/tags/v2.6.2.tar.gz - URL_HASH SHA256=8ff2fff22df038f5cd02cea8af56622bc67f5b64534f1b83b9f133b8366acff2 + URL https://github.com/pybind/pybind11/archive/refs/tags/v2.13.6.tar.gz + URL_HASH SHA256=e08cb87f4773da97fa7b5f035de8763abc656d87d5773e62f6da0587d1f0ec20 ) FetchContent_MakeAvailable(pybind11) diff --git a/vendor/rSim/src/robosim/robosim_py.cpp b/vendor/rSim/src/robosim/robosim_py.cpp index 01bec937..014bf0a4 100644 --- a/vendor/rSim/src/robosim/robosim_py.cpp +++ b/vendor/rSim/src/robosim/robosim_py.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include "vssworld.h" #include "sslworld.h" @@ -23,8 +24,35 @@ struct VSS { void step(vvd actions) const { m_world->step(std::move(actions)); } + void step_fast(py::array_t actions_arr) const { + auto buf = actions_arr.request(); + if (buf.ndim != 2) { + throw std::runtime_error("actions must be a 2D array"); + } + int rows = buf.shape[0]; + int cols = buf.shape[1]; + const double *ptr = static_cast(buf.ptr); + + vvd actions(rows, vd(cols)); + for (int i = 0; i < rows; ++i) { + for (int j = 0; j < cols; ++j) { + actions[i][j] = ptr[i * cols + j]; + } + } + py::gil_scoped_release release; + m_world->step(std::move(actions)); + } + vd getState() const { return m_world->getState(); } + py::array_t getStateArray() const { + const vd &s = m_world->getState(); + py::array_t res(s.size()); + auto buf = res.request(); + std::memcpy(buf.ptr, s.data(), s.size() * sizeof(double)); + return res; + } + void reset(const vd &ballPos, const vvd &blueRobotsPos, const vvd &yellowRobotsPos) { delete m_world; m_world = new VSSWorld(m_fieldType, m_nRobotsBlue, m_nRobotsYellow, m_timeStep_ms / 1000.0, @@ -51,8 +79,35 @@ struct SSL { void step(vvd actions) const { m_world->step(std::move(actions)); } + void step_fast(py::array_t actions_arr) const { + auto buf = actions_arr.request(); + if (buf.ndim != 2) { + throw std::runtime_error("actions must be a 2D array"); + } + int rows = buf.shape[0]; + int cols = buf.shape[1]; + const double *ptr = static_cast(buf.ptr); + + vvd actions(rows, vd(cols)); + for (int i = 0; i < rows; ++i) { + for (int j = 0; j < cols; ++j) { + actions[i][j] = ptr[i * cols + j]; + } + } + py::gil_scoped_release release; + m_world->step(std::move(actions)); + } + vd getState() const { return m_world->getState(); } + py::array_t getStateArray() const { + const vd &s = m_world->getState(); + py::array_t res(s.size()); + auto buf = res.request(); + std::memcpy(buf.ptr, s.data(), s.size() * sizeof(double)); + return res; + } + void reset(const vd &ballPos, const vvd &blueRobotsPos, const vvd &yellowRobotsPos) { delete m_world; m_world = new SSLWorld(m_fieldType, m_nRobotsBlue, m_nRobotsYellow, m_timeStep_ms / 1000.0, @@ -70,14 +125,18 @@ PYBIND11_MODULE(_robosim, m) { py::class_(m, "VSS") .def(py::init()) .def("step", &VSS::step) + .def("step_fast", &VSS::step_fast) .def("get_state", &VSS::getState) + .def("get_state_array", &VSS::getStateArray) .def("reset", &VSS::reset) .def("get_field_params", &VSS::getFieldParams); py::class_(m, "SSL") .def(py::init()) .def("step", &SSL::step) + .def("step_fast", &SSL::step_fast) .def("get_state", &SSL::getState) + .def("get_state_array", &SSL::getStateArray) .def("reset", &SSL::reset) .def("get_field_params", &SSL::getFieldParams); }