[sim] 整理后期Sim2Sim与比赛Rough策略

2026-07-27 13:20:41 +08:00
parent c05c1cb162
commit 4ee4af028c
20 changed files with 5161 additions and 15 deletions
@@ -2,7 +2,7 @@

 `rc_mjlab/` 保存 16DOF 轮足机器人的当前训练与 Sim2Sim 工程。历史快照由 Git Tag 保留，不在目录中复制 `old`、`new` 或 `final` 版本。

-当前内容对应 `v0.7.0`：训练代码保持 `v0.6.0` 的比赛架构，新增后期 MuJoCo 姿态、IK、动力学和 MPC 工具。训练过程可能先获得基模，再调整奖励、课程和环境参数继续训练；模型 checkpoint 的变化不等同于软件架构变化。
+当前内容对应 `v0.8.0`：训练代码保持 `v0.6.0` 的比赛架构，包含 `v0.7.0` 的 MuJoCo 独立工具，并新增后期 Sim2Sim、路线检查和比赛最终 Rough ONNX 策略。训练过程可能先获得基模，再调整奖励、课程和环境参数继续训练；模型 checkpoint 的变化不等同于软件架构变化。

 ## 内容

@@ -12,6 +12,7 @@
 - `mujoco_sim`：不依赖训练循环的姿态、IK、动力学和 MPC 分析
 - `mjlab`：固定版本的本地训练框架依赖
 - `model_rough.pt`：本阶段 Rough 策略权重
+- `model_6800.onnx`：比赛最终使用的 Rough 策略
 - `pyproject.toml`、`uv.lock`：Python 环境与依赖锁定

 与 `v0.3.0` 相比，本版本更新了 MJCF 质量和惯性参数，并将 mjlab 上游基准从 `00409797` 更新到 `40f8d93e`。机械 CAD 未发生变化。
@@ -22,4 +23,6 @@

 `v0.7.0` 不修改比赛训练架构，增加独立 MuJoCo 工具；入口和参数边界见 [`rc_mjlab/mujoco_sim/README.md`](rc_mjlab/mujoco_sim/README.md)。

+`v0.8.0` 继续保持训练架构和 MJCF 不变，归档后期 Sim2Sim 增量与比赛 Rough ONNX 策略；入口和归档边界见 [`rc_mjlab/sim2sim/README.md`](rc_mjlab/sim2sim/README.md)。
+
 工程命令和任务说明见 [`rc_mjlab/README.md`](rc_mjlab/README.md)，本地依赖来源见 [`rc_mjlab/DEPENDENCIES.md`](rc_mjlab/DEPENDENCIES.md)。
@@ -8,6 +8,7 @@
 - `mjlab[cu128]`
 - PyTorch CUDA 12.8 环境
 - `pynput`
+- 后期 Sim2Sim 可选依赖：Pygame、ONNX Runtime

 精确解析结果保存在 `uv.lock`。项目使用本地可编辑的 `mjlab`：

@@ -39,4 +40,10 @@ uv run train Robot-Flat-v0
 uv run play Robot-Rough-v0
 ```

+根 `uv.lock` 保留比赛训练环境的历史解析结果。后期 Sim2Sim 新增依赖单独保存在 `sim2sim/requirements.txt`，运行时叠加，避免重新锁定时升级历史 MuJoCo nightly：
+
+```bash
+uv run --with-requirements sim2sim/requirements.txt python sim2sim/nav_sim2sim.py
+```
+
 GPU、CUDA、MuJoCo development wheel 和驱动版本必须满足 `pyproject.toml` 与 `uv.lock` 的约束。
@@ -2,7 +2,7 @@

 基于 [mjlab](https://github.com/google-deepmind/mjlab) 框架的四轮腿混合机器人强化学习训练与部署部署项目，面向机器人竞赛场景（如越障、匍匐、斜坡、台阶等复合任务）。

-> 当前目录对应 `v0.7.0`：保留 `v0.6.0` 的比赛训练架构，并加入后期 MuJoCo 独立工具集。当前目录中的 `model_rough.pt` 是早期参考权重；比赛最终使用的 `model_6800.onnx` 将随最终部署版本归档。
+> 当前目录对应 `v0.8.0`：保留 `v0.6.0` 的比赛训练架构与 `v0.7.0` 的 MuJoCo 独立工具集，加入后期 Sim2Sim 工具和比赛最终 Rough 策略 `model_6800.onnx`。`model_rough.pt` 仍作为早期参考权重保留。

 ---

@@ -41,6 +41,10 @@ rc_mjlab/
 │       └── competition_terrains.py   # 竞赛自定义地形（高墙障碍、低杆障碍）
 ├── sim2sim/                      # Sim2Sim 物理部署与高精度交互回放工具
 │   ├── nav_sim2sim.py            # 主程序：2D Pygame 交互面板 + 全自动多地形导航追踪
+│   ├── nav_route_sim2sim_check.py # ONNX 策略批量路线检查
+│   ├── ik_slalom_sim2sim.py      # 纯 IK、路径跟踪与绕桩验证
+│   ├── ik_compensation_sweep.py  # IK 补偿参数扫描
+│   ├── export_onnx.py            # PT actor 导出与 ONNX 一致性检查
 │   ├── sim2sim.py                # 简易版键盘调试工具
 │   ├── interface/
 │   │   └── mujoco_io.py          # MuJoCo 输入输出与传感器、低通滤波器接口
@@ -54,7 +58,9 @@ rc_mjlab/
 │   ├── scene.xml                 # mjlab 场景入口文件
 │   └── meshes/                   # STL/OBJ 碰撞与外观网格
 ├── mujoco_sim/                   # 姿态、IK、动力学和 MPC 独立工具
-├── model_rough.pt                # 本阶段用于回放和 Sim2Sim 的 Rough 策略
+├── tools/nav_tools/              # 路线安全检查公共模块
+├── model_rough.pt                # 早期 Rough 参考 checkpoint
+├── model_6800.onnx               # 比赛最终 Rough 策略
 ├── pyproject.toml                # 项目依赖（uv 管理，含清华镜像源加速）
 └── uv.lock                       # 精确依赖锁定文件
 ```
@@ -89,6 +95,8 @@ cd sim2sim
 uv run python nav_sim2sim.py
 ```

+后期 Sim2Sim 的入口、模型边界和批量检查命令见 [`sim2sim/README.md`](sim2sim/README.md)。
+
 ---

 ## 🖥️ 交互式自动导航平台 (sim2sim/nav_sim2sim.py)
@@ -0,0 +1,58 @@
+# 后期 Sim2Sim 工具
+
+本目录保存比赛训练架构之后形成的 MuJoCo 策略验证工具。`v0.8.0` 在早期 Sim2Sim 基础上增加 ONNX 策略加载、IK 参数扫描、纯 IK 绕桩验证和批量路线检查；训练任务与 MJCF 不在本阶段修改。
+
+## 主要入口
+
+- `nav_sim2sim.py`：Pygame 面板与 MuJoCo 多任务导航，Rough 策略优先加载根目录的 `model_6800.onnx`。
+- `sim2sim.py`：较轻量的键盘控制与策略回放入口，优先加载 `model_6800.onnx`，缺失时回退到早期 `model_rough.pt`。
+- `ik_slalom_sim2sim.py`：不依赖 RL 策略的 IK、差速轮、路径跟踪和绕桩测试。
+- `ik_compensation_sweep.py`：批量扫描 IK 补偿参数并输出排序结果。
+- `nav_route_sim2sim_check.py`：使用 ONNX 策略批量检查内置任务或外部航点路线。
+- `export_onnx.py`：将兼容的 PyTorch actor checkpoint 导出并核对为 ONNX。
+- `interface/mujoco_io.py`：MuJoCo 模型、传感器和执行器接口。
+- `policy/policy_runner.py`：PT/ONNX 策略加载与历史观测缓存。
+
+## 环境
+
+主训练环境继续由根目录的 `uv.lock` 管理。后期 Sim2Sim 新增的 Pygame 与 ONNX Runtime 单独记录在 `sim2sim/requirements.txt`，运行时叠加，避免重新解析时改变已归档的 MuJoCo nightly 版本：
+
+```powershell
+uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\nav_sim2sim.py
+```
+
+训练工程提供 MuJoCo、NumPy、PyTorch、Matplotlib 和 `pynput`；专用 requirements 显式补充 Pygame 与 ONNX Runtime。下面其他命令同样使用 `--with-requirements .\sim2sim\requirements.txt`。
+
+## 常用命令
+
+```powershell
+# 比赛 Rough 策略交互回放
+uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\nav_sim2sim.py
+
+# 轻量策略回放
+uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\sim2sim.py
+
+# 纯 IK 绕桩验证
+uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\ik_slalom_sim2sim.py --test slalom
+
+# IK 补偿参数扫描
+uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\ik_compensation_sweep.py --top 12
+
+# 使用内置绕桩任务做批量 Sim2Sim 路线检查
+uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\nav_route_sim2sim_check.py `
+  --terrain-xml .\sim2sim\terrain\scene_terrain.xml `
+  --mission slalom `
+  --onnx .\model_6800.onnx
+
+# 导出早期参考 PT 权重；也可用 --pt-path 指定其他 checkpoint
+uv run --with-requirements .\sim2sim\requirements.txt python .\sim2sim\export_onnx.py
+```
+
+## 模型与边界
+
+- `../model_6800.onnx` 是 `last_not_slalom_1050` 最终真机工程使用的比赛 Rough 策略，SHA-256 为 `3C994BDD3434AD15770A52AC0E8D229F502F00D6511CDD42C2E2C742301AEF13`。
+- `../model_rough.pt` 是较早阶段的参考 checkpoint，两者不是同一版本的权重。
+- Crawl 模型未在本阶段归档；需要 Crawl 策略的入口会查找 `model_crawl.onnx` 或 `model_crawl.pt`。
+- `nav_route_sim2sim_check.py` 依赖 `../tools/nav_tools/route_safety_check.py` 的航点和避障几何定义。
+
+运行时生成的日志、临时 XML 和 `route_check_runs/` 不纳入版本库。源目录中的大量路线试验结果也未复制；它们包含重复轨迹和本机绝对路径，不属于可复用程序源码。
@@ -0,0 +1,110 @@
+import argparse
+import torch
+import torch.nn as nn
+import numpy as np
+from pathlib import Path
+import onnxruntime as ort
+
+
+PROJECT_ROOT = Path(__file__).resolve().parents[1]
+
+class PolicyMLP(nn.Module):
+    def __init__(self, obs_dim=53, action_dim=16):
+        super().__init__()
+        self.register_buffer("obs_mean", torch.zeros(obs_dim))
+        self.register_buffer("obs_std", torch.ones(obs_dim))
+        self.net = nn.Sequential(
+            nn.Linear(obs_dim, 512), nn.ELU(),
+            nn.Linear(512, 256), nn.ELU(),
+            nn.Linear(256, 128), nn.ELU(),
+            nn.Linear(128, action_dim),
+        )
+
+    def forward(self, x):
+        x = (x - self.obs_mean) / torch.clamp(self.obs_std, min=1e-6)
+        return self.net(x)
+
+def load_policy(model_path, device):
+    ckpt = torch.load(model_path, map_location=device, weights_only=False)
+    state_dict = ckpt["actor_state_dict"]
+    weight_key = "mlp.0.weight" if "mlp.0.weight" in state_dict else "net.0.weight"
+    obs_dim = state_dict[weight_key].shape[1]
+
+    output_key = "mlp.6.weight" if "mlp.6.weight" in state_dict else "net.6.weight"
+    action_dim = state_dict[output_key].shape[0]
+
+    model = PolicyMLP(obs_dim=obs_dim, action_dim=action_dim)
+    my_sd = {}
+    for k, v in state_dict.items():
+        if k.startswith("mlp."):
+            my_sd[k.replace("mlp.", "net.")] = v
+        elif k.startswith("net."):
+            my_sd[k] = v
+        elif k == "obs_normalizer._mean":
+            my_sd["obs_mean"] = v.squeeze()
+        elif k == "obs_normalizer._var":
+            my_sd["obs_std"] = torch.sqrt(v.squeeze() + 1e-5)
+
+    model.load_state_dict(my_sd, strict=False)
+    model.eval()
+    model.to(device)
+    return model, obs_dim
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--pt-path",
+        "--pt_path",
+        dest="pt_path",
+        type=Path,
+        default=PROJECT_ROOT / "model_rough.pt",
+        help="PyTorch checkpoint to export (default: ../model_rough.pt).",
+    )
+    args = parser.parse_args()
+
+    pt_path = args.pt_path.expanduser().resolve()
+    if not pt_path.exists():
+        print(f"File not found: {pt_path}")
+        return
+
+    device = torch.device("cpu")
+    print(f"Loading {pt_path}...")
+    model, obs_dim = load_policy(pt_path, device)
+
+    onnx_path = pt_path.with_suffix(".onnx")
+
+    dummy_input = torch.randn(1, obs_dim, device=device)
+
+    print(f"Exporting to {onnx_path}...")
+    torch.onnx.export(
+        model,
+        dummy_input,
+        str(onnx_path),
+        export_params=True,
+        opset_version=14,
+        do_constant_folding=True,
+        input_names=["obs"],
+        output_names=["action"],
+        dynamic_axes={"obs": {0: "batch_size"}, "action": {0: "batch_size"}}
+    )
+
+    print("Verifying ONNX export...")
+    try:
+        session = ort.InferenceSession(str(onnx_path))
+        with torch.no_grad():
+            pt_out = model(dummy_input).numpy()
+        onnx_out = session.run(["action"], {"obs": dummy_input.numpy()})[0]
+
+        max_diff = np.max(np.abs(pt_out - onnx_out))
+        mean_diff = np.mean(np.abs(pt_out - onnx_out))
+        print(f"ONNX vs PyTorch - max_diff: {max_diff:.6f}, mean_diff: {mean_diff:.6f}")
+
+        if max_diff < 1e-4:
+            print("ONNX export verified OK.")
+        else:
+            print("WARNING: ONNX export has significant divergence from PyTorch model.")
+    except ImportError:
+        print("onnxruntime not installed. Skipping verification. Install with: pip install onnxruntime")
+
+if __name__ == "__main__":
+    main()
@@ -0,0 +1,238 @@
+#!/usr/bin/env python3
+"""Sweep IK compensation parameters in the standalone sim2sim scene."""
+
+from __future__ import annotations
+
+import argparse
+import importlib.util
+import json
+import sys
+from pathlib import Path
+from typing import Any
+
+THIS_DIR = Path(__file__).resolve().parent
+PROJECT_ROOT = THIS_DIR.parent
+if str(THIS_DIR) not in sys.path:
+    sys.path.insert(0, str(THIS_DIR))
+
+SIM_PATH = THIS_DIR / "ik_slalom_sim2sim.py"
+spec = importlib.util.spec_from_file_location("ik_slalom_sim2sim", SIM_PATH)
+if spec is None or spec.loader is None:
+    raise RuntimeError(f"Cannot load {SIM_PATH}")
+sim = importlib.util.module_from_spec(spec)
+sys.modules["ik_slalom_sim2sim"] = sim
+spec.loader.exec_module(sim)
+
+
+TRIALS = [
+    {"name": "forward", "speed": 1.0, "yaw": 0.0, "target_vx": 1.0, "target_yaw": 0.0},
+    {"name": "yaw", "speed": 0.0, "yaw": 1.0, "target_vx": 0.0, "target_yaw": 1.0},
+    {"name": "arc", "speed": 1.0, "yaw": 1.0, "target_vx": 1.0, "target_yaw": 1.0},
+]
+
+
+def parse_float_list(text: str) -> list[float]:
+    return [float(x.strip()) for x in text.split(",") if x.strip()]
+
+
+def parse_bool_list(text: str) -> list[bool]:
+    out: list[bool] = []
+    for item in text.split(","):
+        key = item.strip().lower()
+        if not key:
+            continue
+        if key in {"1", "true", "on", "yes"}:
+            out.append(True)
+        elif key in {"0", "false", "off", "no"}:
+            out.append(False)
+        else:
+            raise argparse.ArgumentTypeError(f"Invalid bool item: {item}")
+    return out
+
+
+def make_sim_args(args: argparse.Namespace, trial: dict[str, float | str], cfg: dict[str, Any]) -> argparse.Namespace:
+    argv = [
+        "ik_slalom_sim2sim.py",
+        "--test",
+        str(trial["name"]),
+        "--duration",
+        str(args.duration),
+        "--settle",
+        str(args.settle),
+        "--speed",
+        str(trial["speed"]),
+        "--yaw-rate",
+        str(trial["yaw"]),
+        "--posture",
+        "custom",
+        "--custom-abduction",
+        str(args.custom_abduction),
+        "--custom-hip",
+        str(args.custom_hip),
+        "--custom-knee",
+        str(args.custom_knee),
+        "--wheel-model",
+        "direct",
+        "--linear-wheel-gain",
+        str(args.linear_wheel_gain),
+        "--direct-yaw-wheel-gain",
+        str(args.direct_yaw_wheel_gain),
+        "--max-wheel-speed",
+        str(args.max_wheel_speed),
+        "--wheel-accel-limit",
+        str(args.wheel_accel_limit),
+        "--yaw-rate-kp",
+        str(cfg["yaw_rate_kp"]),
+        "--encoder-posture-kp",
+        str(cfg["encoder_posture_kp"]),
+        "--encoder-posture-max",
+        str(cfg["encoder_posture_max"]),
+        "--roll-comp-gain",
+        str(cfg["roll_comp_gain"]),
+        "--pitch-comp-gain",
+        str(cfg["pitch_comp_gain"]),
+        "--no-realtime",
+    ]
+    argv.append("--imu-posture" if cfg["imu_posture"] else "--no-imu-posture")
+    argv.append("--encoder-guard" if cfg["encoder_guard"] else "--no-encoder-guard")
+    argv.append("--imu-guard" if cfg["imu_guard"] else "--no-imu-guard")
+    old_argv = sys.argv
+    try:
+        sys.argv = argv
+        return sim.parse_args()
+    finally:
+        sys.argv = old_argv
+
+
+def score_trial(out: dict[str, Any], trial: dict[str, float | str]) -> dict[str, float]:
+    vx = float(out["mean_body_vx_mps"])
+    yaw = float(out["mean_yaw_rate_rad_s"])
+    vx_err = abs(vx - float(trial["target_vx"]))
+    yaw_err = abs(yaw - float(trial["target_yaw"]))
+    return {
+        "vx": vx,
+        "yaw": yaw,
+        "imu_gyro_z": float(out["mean_imu_gyro_z_rad_s"]),
+        "vx_err": vx_err,
+        "yaw_err": yaw_err,
+        "err": vx_err + yaw_err,
+    }
+
+
+def run_sweep(args: argparse.Namespace) -> list[dict[str, Any]]:
+    rows: list[dict[str, Any]] = []
+    for imu_posture in args.imu_posture_values:
+        for encoder_guard in args.encoder_guard_values:
+            for imu_guard in args.imu_guard_values:
+                for encoder_posture_kp in args.encoder_posture_kps:
+                    for encoder_posture_max in args.encoder_posture_maxs:
+                        for yaw_rate_kp in args.yaw_rate_kps:
+                            for roll_comp_gain in args.roll_comp_gains:
+                                for pitch_comp_gain in args.pitch_comp_gains:
+                                    cfg = {
+                                        "imu_posture": imu_posture,
+                                        "encoder_guard": encoder_guard,
+                                        "imu_guard": imu_guard,
+                                        "encoder_posture_kp": encoder_posture_kp,
+                                        "encoder_posture_max": encoder_posture_max,
+                                        "yaw_rate_kp": yaw_rate_kp,
+                                        "roll_comp_gain": roll_comp_gain,
+                                        "pitch_comp_gain": pitch_comp_gain,
+                                    }
+                                    detail: list[dict[str, Any]] = []
+                                    speed_error = 0.0
+                                    max_tilt = 0.0
+                                    max_leg = 0.0
+                                    mean_wheel_err = 0.0
+                                    stable_all = True
+                                    for trial in TRIALS:
+                                        sim_args = make_sim_args(args, trial, cfg)
+                                        out = sim.run_one(str(trial["name"]), sim_args)
+                                        trial_score = score_trial(out, trial)
+                                        trial_score["test"] = str(trial["name"])
+                                        detail.append(trial_score)
+                                        speed_error += trial_score["err"]
+                                        max_tilt = max(max_tilt, float(out["max_tilt_deg"]))
+                                        max_leg = max(max_leg, float(out["max_leg_encoder_error_rad"]))
+                                        mean_wheel_err += float(out["mean_wheel_speed_error_rad_s"])
+                                        stable_all = stable_all and bool(out["stable"])
+
+                                    score = (
+                                        speed_error
+                                        + args.tilt_weight * max_tilt
+                                        + args.leg_error_weight * max_leg
+                                        + args.wheel_error_weight * (mean_wheel_err / len(TRIALS))
+                                    )
+                                    row = {
+                                        **cfg,
+                                        "score": round(score, 6),
+                                        "speed_error_sum": round(speed_error, 6),
+                                        "max_tilt_deg": round(max_tilt, 5),
+                                        "max_leg_encoder_error_rad": round(max_leg, 6),
+                                        "mean_wheel_speed_error_rad_s": round(mean_wheel_err / len(TRIALS), 6),
+                                        "stable_all": stable_all,
+                                        "detail": detail,
+                                    }
+                                    rows.append(row)
+                                    print(
+                                        "DONE "
+                                        + json.dumps(
+                                            {k: v for k, v in row.items() if k != "detail"},
+                                            ensure_ascii=False,
+                                        ),
+                                        flush=True,
+                                    )
+    rows.sort(key=lambda r: float(r["score"]))
+    return rows
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--duration", type=float, default=3.0)
+    parser.add_argument("--settle", type=float, default=1.5)
+    parser.add_argument("--custom-abduction", type=float, default=0.2)
+    parser.add_argument("--custom-hip", type=float, default=1.697)
+    parser.add_argument("--custom-knee", type=float, default=-2.650)
+    parser.add_argument("--linear-wheel-gain", type=float, default=12.5)
+    parser.add_argument("--direct-yaw-wheel-gain", type=float, default=8.0)
+    parser.add_argument("--max-wheel-speed", type=float, default=12.0)
+    parser.add_argument("--wheel-accel-limit", type=float, default=35.0)
+    parser.add_argument("--imu-posture-values", type=parse_bool_list, default=[True, False])
+    parser.add_argument("--encoder-guard-values", type=parse_bool_list, default=[True])
+    parser.add_argument("--imu-guard-values", type=parse_bool_list, default=[True])
+    parser.add_argument("--encoder-posture-kps", type=parse_float_list, default=[0.0, 0.05, 0.15, 0.30])
+    parser.add_argument("--encoder-posture-maxs", type=parse_float_list, default=[0.03])
+    parser.add_argument("--yaw-rate-kps", type=parse_float_list, default=[0.0, 0.4, 0.8])
+    parser.add_argument("--roll-comp-gains", type=parse_float_list, default=[0.35])
+    parser.add_argument("--pitch-comp-gains", type=parse_float_list, default=[0.35])
+    parser.add_argument("--tilt-weight", type=float, default=0.02)
+    parser.add_argument("--leg-error-weight", type=float, default=0.5)
+    parser.add_argument("--wheel-error-weight", type=float, default=0.0)
+    parser.add_argument("--top", type=int, default=12)
+    parser.add_argument("--json", type=Path, default=None)
+    return parser.parse_args()
+
+
+def main() -> int:
+    args = parse_args()
+    rows = run_sweep(args)
+    if args.json:
+        args.json.write_text(json.dumps(rows, indent=2), encoding="utf-8")
+
+    print("\nTop compensation parameter sets")
+    print("rank score speed_err tilt leg_err wheel_err imu enc_kp yaw_kp enc_guard imu_guard")
+    for i, row in enumerate(rows[: args.top], 1):
+        print(
+            f"{i:2d} {row['score']:7.4f} {row['speed_error_sum']:7.4f} "
+            f"{row['max_tilt_deg']:5.2f} {row['max_leg_encoder_error_rad']:7.4f} "
+            f"{row['mean_wheel_speed_error_rad_s']:7.4f} "
+            f"{int(row['imu_posture'])} {row['encoder_posture_kp']:6.3f} "
+            f"{row['yaw_rate_kp']:6.3f} {int(row['encoder_guard'])} {int(row['imu_guard'])}"
+        )
+        for d in row["detail"]:
+            print(f"   {d['test']:<7} vx={d['vx']:+.3f} yaw={d['yaw']:+.3f} err={d['err']:.3f}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
@@ -87,9 +87,14 @@ class MuJoCoIO:
        return out_xml_path

    def _rebuild_actuators(self, spec):
-        actuators_to_delete = list(spec.actuators)
-        for act in actuators_to_delete:
-            spec.delete(act)
+        if hasattr(spec, "delete"):
+            actuators_to_delete = list(spec.actuators)
+            for act in actuators_to_delete:
+                spec.delete(act)
+        else:
+            actuators_to_delete = list(spec.actuators)
+            for act in actuators_to_delete:
+                act.delete()
        
        # Keep sim2sim aligned with the training robot config and sim2real runtime:
        # leg position PD = (50.0, 1.5), wheel velocity damping = 1.0.
@@ -324,9 +324,11 @@ def main():
    terrain_dir = Path(__file__).parent / "terrain"
    terrain_xml = terrain_dir / "scene_terrain.xml"
    robot_xml = project_root / "mjcf" / "wheelleg.xml"
+    rough_onnx = project_root / "model_6800.onnx"
+    crawl_onnx = project_root / "model_crawl.onnx"
    policy_path = {
-        "rough": project_root / "model_rough.pt",
-        "crawl": project_root / "model_crawl.pt"
+        "rough": rough_onnx if rough_onnx.exists() else project_root / "model_rough.pt",
+        "crawl": crawl_onnx if crawl_onnx.exists() else project_root / "model_crawl.pt"
    }

    # 1. 解析 XML 地图障碍物，实现 100% 可视化精准对应
@@ -3,7 +3,10 @@ import torch.nn as nn
 import numpy as np
 from collections import deque
 from pathlib import Path
-from pynput import keyboard
+try:
+    from pynput import keyboard
+except ImportError:
+    keyboard = None

 # ============================================================
 # Policy Model
@@ -26,6 +29,28 @@ class PolicyMLP(nn.Module):


 def load_policy(model_path, device):
+    if str(model_path).endswith('.onnx'):
+        import onnxruntime as ort
+        session = ort.InferenceSession(str(model_path))
+        class OnnxWrapper:
+            def __init__(self, session):
+                self.session = session
+                self.obs_dim = session.get_inputs()[0].shape[1]
+                if isinstance(self.obs_dim, str):
+                    self.obs_dim = 53
+                class MockMean:
+                    def __init__(self, d):
+                        self.d = d
+                    def numel(self):
+                        return self.d
+                self.obs_mean = MockMean(self.obs_dim)
+
+            def __call__(self, x):
+                inputs = {self.session.get_inputs()[0].name: x.cpu().numpy()}
+                out = self.session.run(None, inputs)[0]
+                return torch.tensor(out, device=x.device)
+        return OnnxWrapper(session)
+
    ckpt = torch.load(model_path, map_location=device, weights_only=False)
    state_dict = ckpt["actor_state_dict"]
    
@@ -129,9 +154,13 @@ class PolicyRunner:
        ], dtype=np.float32)

        # Background keyboard listener for seamless switcher keys ('1' and '2')
-        self.listener = keyboard.Listener(on_press=self._on_press)
-        self.listener.start()
-        print("[PolicyRunner] Background Keyboard Switcher active: Press '1' for ROUGH, '2' for CRAWL")
+        self.listener = None
+        if keyboard is not None:
+            self.listener = keyboard.Listener(on_press=self._on_press)
+            self.listener.start()
+            print("[PolicyRunner] Background Keyboard Switcher active: Press '1' for ROUGH, '2' for CRAWL")
+        else:
+            print("[PolicyRunner] pynput not installed; background keyboard switcher disabled.")

    def _on_press(self, key):
        try:
@@ -0,0 +1,3 @@
+# Additional runtime dependencies for the post-training Sim2Sim tools.
+onnxruntime>=1.19.0
+pygame>=2.6.1
@@ -41,6 +41,19 @@ class PolicyMLP(nn.Module):


 def load_policy(model_path, device):
+    if str(model_path).endswith('.onnx'):
+        import onnxruntime as ort
+        session = ort.InferenceSession(str(model_path))
+        class OnnxWrapper:
+            def __init__(self, session):
+                self.session = session
+            def __call__(self, x):
+                inputs = {self.session.get_inputs()[0].name: x.cpu().numpy()}
+                out = self.session.run(None, inputs)[0]
+                return torch.tensor(out, device=x.device)
+        return OnnxWrapper(session)
+
+
    ckpt = torch.load(model_path, map_location=device, weights_only=False)
    state_dict = ckpt["actor_state_dict"]
    model = PolicyMLP()
@@ -131,7 +144,9 @@ def main():
    terrain_dir = Path(__file__).parent / "terrain"
    terrain_xml = terrain_dir / "scene_terrain.xml"
    robot_xml = Path(__file__).parent.parent / "mjcf" / "wheelleg.xml"
-    policy_path = Path(__file__).parent.parent / "model_1700.pt"
+    policy_path = Path(__file__).parent.parent / "model_6800.onnx"
+    if not policy_path.exists():
+        policy_path = Path(__file__).parent.parent / "model_rough.pt"
    hfield_dir = terrain_dir

    temp_xml = project_root / "mjcf" / "sim2sim_temp.xml"
@@ -0,0 +1,7 @@
+# 路线检查公共模块
+
+`route_safety_check.py` 提供航点、避障区域、机器人平面包络和几何距离计算，供 `sim2sim/nav_route_sim2sim_check.py` 复用。
+
+该模块只依赖 Python 标准库。原开发目录中的地图编辑器、PCD、比赛路线 JSON、备份和批量实验结果不属于本次后期 Sim2Sim 里程碑，未在这里复制。
+
+外部路线文件需要包含 `waypoints`（或 `segments[].waypoints`）以及 `regions` / `avoid_regions`。也可以不提供路线文件，直接使用 Sim2Sim 检查器的内置任务。
@@ -0,0 +1,638 @@
+#!/usr/bin/env python3
+"""Offline route safety checker for nav_tools waypoint JSON files.
+
+The checker treats avoid regions as hard no-go polygons and validates the
+route centerline with a circular robot footprint. It is intentionally light on
+dependencies so it can run on the robot laptop without ROS, pygame, or shapely.
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import math
+import sys
+import xml.etree.ElementTree as ET
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any
+
+
+ROBOT_BODY_LENGTH = 0.356
+ROBOT_BODY_WIDTH = 0.235
+ROBOT_BODY_CENTER_X = 0.1518
+ROBOT_ORIGIN_FROM_FRONT = 0.105
+ROBOT_WHEEL_VIS_LENGTH = 0.16
+ROBOT_WHEEL_VIS_WIDTH = 0.055
+ROBOT_POSE_HIP = 0.550
+ROBOT_POSE_KNEE = -1.125
+PCD_ROBOT_RADIUS = 0.18
+ROBOT_FOOTPRINT_PADDING = 0.03
+
+
+@dataclass(frozen=True)
+class Waypoint:
+    index: int
+    id: str
+    x: float
+    y: float
+    yaw_deg: float
+    speed: float | None
+    policy: str
+    tolerance: float | None
+    slalom_straight: bool = False
+    slalom_script_break: bool = False
+    slalom_script_pos_tolerance: float | None = None
+    exact_reach: bool = False
+    precision_follow: bool = False
+    require_yaw: bool = False
+    yaw_tolerance_deg: float | None = None
+    stable_cycles: int | None = None
+    mandatory_cross: bool = False
+    mandatory_radius: float | None = None
+    mandatory_center_x: float | None = None
+    mandatory_center_y: float | None = None
+
+
+@dataclass(frozen=True)
+class AvoidRegion:
+    name: str
+    kind: str
+    polygon: tuple[tuple[float, float], ...]
+
+
+@dataclass(frozen=True)
+class SegmentRisk:
+    start_id: str
+    end_id: str
+    region: str
+    clearance_m: float
+    required_m: float
+    margin_m: float
+    length_m: float
+    centerline_intersects: bool
+
+    @property
+    def status(self) -> str:
+        if self.centerline_intersects:
+            return "INTERSECT"
+        if self.margin_m < 0.0:
+            return "VIOLATION"
+        if self.margin_m < 0.05:
+            return "TIGHT"
+        return "OK"
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description="Check nav_tools waypoint routes against avoid/no-go polygons."
+    )
+    parser.add_argument(
+        "--points",
+        type=Path,
+        default=Path("tools/nav_tools/points/points_20260705_174627.json"),
+        help="Route JSON exported by nav_map_viewer.",
+    )
+    parser.add_argument(
+        "--xml",
+        type=Path,
+        default=Path("tools/nav_tools/xml/A.xml"),
+        help="Optional MuJoCo terrain XML used for metadata checks.",
+    )
+    parser.add_argument(
+        "--onnx",
+        type=Path,
+        default=Path("model_6800.onnx"),
+        help="Optional ONNX policy path used for input/output shape reporting.",
+    )
+    parser.add_argument(
+        "--footprint-radius",
+        type=float,
+        default=None,
+        help="Robot circular footprint radius in meters. Defaults to sim2real lateral footprint.",
+    )
+    parser.add_argument(
+        "--avoid-margin",
+        type=float,
+        default=0.05,
+        help="Extra clearance added outside the robot footprint.",
+    )
+    parser.add_argument(
+        "--warn-margin",
+        type=float,
+        default=0.05,
+        help="Report a TIGHT warning when spare margin is below this value.",
+    )
+    parser.add_argument(
+        "--top",
+        type=int,
+        default=20,
+        help="Number of closest segment-region pairs to print.",
+    )
+    parser.add_argument(
+        "--json-out",
+        type=Path,
+        default=None,
+        help="Optional machine-readable report path.",
+    )
+    parser.add_argument(
+        "--allow-violations",
+        action="store_true",
+        help="Exit with code 0 even when violations are detected.",
+    )
+    return parser.parse_args()
+
+
+def load_json(path: Path) -> dict[str, Any]:
+    with path.open("r", encoding="utf-8") as f:
+        data = json.load(f)
+    if not isinstance(data, dict):
+        raise ValueError(f"JSON root must be an object: {path}")
+    return data
+
+
+def _optional_float(value: Any) -> float | None:
+    if value is None:
+        return None
+    try:
+        return float(value)
+    except (TypeError, ValueError):
+        return None
+
+
+def load_waypoints(payload: dict[str, Any]) -> list[Waypoint]:
+    rows = None
+    if isinstance(payload.get("segments"), list) and payload["segments"]:
+        rows = []
+        for segment in payload["segments"]:
+            if isinstance(segment, dict) and isinstance(segment.get("waypoints"), list):
+                rows.extend(segment["waypoints"])
+    if rows is None:
+        rows = payload.get("waypoints")
+    if not isinstance(rows, list):
+        raise ValueError("Route JSON has no top-level waypoints or segments[].waypoints")
+
+    waypoints: list[Waypoint] = []
+    for index, row in enumerate(rows, start=1):
+        if not isinstance(row, dict):
+            continue
+        x = float(row.get("world_x", row.get("x", 0.0)))
+        y = float(row.get("world_y", row.get("y", 0.0)))
+        yaw = float(row.get("yawDeg", row.get("yaw_deg", row.get("yaw", 0.0))))
+        speed = row.get("speed")
+        tolerance = row.get("tolerance")
+        waypoints.append(
+            Waypoint(
+                index=index,
+                id=str(row.get("id", index)),
+                x=x,
+                y=y,
+                yaw_deg=yaw,
+                speed=float(speed) if speed is not None else None,
+                policy=str(row.get("policy", "")),
+                tolerance=float(tolerance) if tolerance is not None else None,
+                slalom_straight=bool(row.get("slalom_straight", row.get("slalomStraight", False))),
+                slalom_script_break=bool(
+                    row.get("slalom_script_break", row.get("slalomScriptBreak", False))
+                ),
+                slalom_script_pos_tolerance=_optional_float(
+                    row.get(
+                        "slalom_script_pos_tolerance",
+                        row.get("slalomScriptPosTolerance", row.get("scriptTolerance")),
+                    )
+                ),
+                exact_reach=bool(row.get("exact_reach", row.get("exactReach", False))),
+                precision_follow=bool(row.get("precision_follow", row.get("precisionFollow", False))),
+                require_yaw=bool(row.get("require_yaw", row.get("requireYaw", False))),
+                yaw_tolerance_deg=_optional_float(
+                    row.get("yaw_tolerance_deg", row.get("yawToleranceDeg"))
+                ),
+                stable_cycles=(
+                    int(row.get("stable_cycles", row.get("stableCycles")))
+                    if row.get("stable_cycles", row.get("stableCycles")) is not None
+                    else None
+                ),
+                mandatory_cross=bool(row.get("mandatory_cross", row.get("mandatoryCross", False))),
+                mandatory_radius=_optional_float(
+                    row.get("mandatory_radius", row.get("mandatoryRadius"))
+                ),
+                mandatory_center_x=_optional_float(
+                    row.get("mandatory_center_x", row.get("mandatoryCenterX"))
+                ),
+                mandatory_center_y=_optional_float(
+                    row.get("mandatory_center_y", row.get("mandatoryCenterY"))
+                ),
+            )
+        )
+    if len(waypoints) < 2:
+        raise ValueError("Route must contain at least two waypoints")
+    return waypoints
+
+
+def load_regions(payload: dict[str, Any]) -> list[AvoidRegion]:
+    rows = payload.get("regions", payload.get("avoid_regions", []))
+    if not isinstance(rows, list):
+        return []
+
+    regions: list[AvoidRegion] = []
+    for index, row in enumerate(rows, start=1):
+        if not isinstance(row, dict):
+            continue
+        polygon_rows = row.get("polygon", row.get("points", []))
+        if not isinstance(polygon_rows, list):
+            continue
+        polygon: list[tuple[float, float]] = []
+        for point in polygon_rows:
+            if isinstance(point, dict):
+                polygon.append((float(point.get("x", 0.0)), float(point.get("y", 0.0))))
+            elif isinstance(point, (list, tuple)) and len(point) >= 2:
+                polygon.append((float(point[0]), float(point[1])))
+        if len(polygon) >= 3:
+            regions.append(
+                AvoidRegion(
+                    name=str(row.get("name", f"avoid_{index}")),
+                    kind=str(row.get("kind", "avoid")),
+                    polygon=tuple(polygon),
+                )
+            )
+    return regions
+
+
+def robot_wheel_local_points(body_center_offset_x: float) -> list[tuple[float, float]]:
+    thigh_dx = -0.25 * math.sin(ROBOT_POSE_HIP)
+    shank_dx = -0.2 * math.sin(ROBOT_POSE_HIP + ROBOT_POSE_KNEE)
+    wheel_positions = (
+        ((0.32826 + 0.06389) - ROBOT_BODY_CENTER_X, 0.066172 - 0.027344, 0.1035, 0.014699, 0.04074, 0.0),
+        ((0.32826 + 0.06389) - ROBOT_BODY_CENTER_X, -0.065853 + 0.027311, -0.1035, -0.018447, -0.040735, -0.00075079),
+        ((-0.024743 - 0.06389) - ROBOT_BODY_CENTER_X, 0.066141 - 0.027309, 0.099459, 0.012475, 0.040737, 0.0),
+        ((-0.024743 - 0.06389) - ROBOT_BODY_CENTER_X, -0.065884 + 0.027341, -0.099408, -0.012435, -0.040737, -0.00075079),
+    )
+    return [
+        (
+            body_center_offset_x + pitch_x + knee_x + thigh_dx + shank_dx,
+            pitch_y + knee_y + wheel_y + wheel_geom_y,
+        )
+        for pitch_x, pitch_y, knee_y, wheel_y, wheel_geom_y, knee_x in wheel_positions
+    ]
+
+
+def default_lateral_footprint_radius() -> float:
+    half_width = ROBOT_BODY_WIDTH * 0.5
+    radius = max(PCD_ROBOT_RADIUS, half_width)
+    body_center_offset_x = ROBOT_ORIGIN_FROM_FRONT - ROBOT_BODY_LENGTH * 0.5
+    for _, wheel_y in robot_wheel_local_points(body_center_offset_x):
+        radius = max(radius, abs(wheel_y) + ROBOT_WHEEL_VIS_WIDTH * 0.5)
+    return radius + ROBOT_FOOTPRINT_PADDING
+
+
+def point_segment_distance(
+    px: float,
+    py: float,
+    ax: float,
+    ay: float,
+    bx: float,
+    by: float,
+) -> float:
+    dx = bx - ax
+    dy = by - ay
+    length_sq = dx * dx + dy * dy
+    if length_sq <= 1.0e-12:
+        return math.hypot(px - ax, py - ay)
+    t = ((px - ax) * dx + (py - ay) * dy) / length_sq
+    t = max(0.0, min(1.0, t))
+    qx = ax + t * dx
+    qy = ay + t * dy
+    return math.hypot(px - qx, py - qy)
+
+
+def orientation(
+    ax: float,
+    ay: float,
+    bx: float,
+    by: float,
+    cx: float,
+    cy: float,
+) -> float:
+    return (bx - ax) * (cy - ay) - (by - ay) * (cx - ax)
+
+
+def on_segment(
+    ax: float,
+    ay: float,
+    bx: float,
+    by: float,
+    cx: float,
+    cy: float,
+) -> bool:
+    return (
+        min(ax, bx) - 1.0e-9 <= cx <= max(ax, bx) + 1.0e-9
+        and min(ay, by) - 1.0e-9 <= cy <= max(ay, by) + 1.0e-9
+        and abs(orientation(ax, ay, bx, by, cx, cy)) <= 1.0e-9
+    )
+
+
+def segments_intersect(
+    a: tuple[float, float],
+    b: tuple[float, float],
+    c: tuple[float, float],
+    d: tuple[float, float],
+) -> bool:
+    ax, ay = a
+    bx, by = b
+    cx, cy = c
+    dx, dy = d
+    o1 = orientation(ax, ay, bx, by, cx, cy)
+    o2 = orientation(ax, ay, bx, by, dx, dy)
+    o3 = orientation(cx, cy, dx, dy, ax, ay)
+    o4 = orientation(cx, cy, dx, dy, bx, by)
+    if o1 * o2 < 0.0 and o3 * o4 < 0.0:
+        return True
+    return (
+        on_segment(ax, ay, bx, by, cx, cy)
+        or on_segment(ax, ay, bx, by, dx, dy)
+        or on_segment(cx, cy, dx, dy, ax, ay)
+        or on_segment(cx, cy, dx, dy, bx, by)
+    )
+
+
+def point_in_polygon(x: float, y: float, polygon: tuple[tuple[float, float], ...]) -> bool:
+    inside = False
+    for index, (ax, ay) in enumerate(polygon):
+        bx, by = polygon[(index + 1) % len(polygon)]
+        if point_segment_distance(x, y, ax, ay, bx, by) <= 1.0e-9:
+            return True
+        if (ay > y) != (by > y):
+            x_cross = (bx - ax) * (y - ay) / (by - ay) + ax
+            if x < x_cross:
+                inside = not inside
+    return inside
+
+
+def segment_polygon_intersects(
+    a: tuple[float, float],
+    b: tuple[float, float],
+    polygon: tuple[tuple[float, float], ...],
+) -> bool:
+    if point_in_polygon(a[0], a[1], polygon) or point_in_polygon(b[0], b[1], polygon):
+        return True
+    return any(
+        segments_intersect(a, b, polygon[index], polygon[(index + 1) % len(polygon)])
+        for index in range(len(polygon))
+    )
+
+
+def segment_polygon_distance(
+    a: tuple[float, float],
+    b: tuple[float, float],
+    polygon: tuple[tuple[float, float], ...],
+) -> float:
+    if segment_polygon_intersects(a, b, polygon):
+        return 0.0
+    distances = [point_segment_distance(px, py, a[0], a[1], b[0], b[1]) for px, py in polygon]
+    for index, (ax, ay) in enumerate(polygon):
+        bx, by = polygon[(index + 1) % len(polygon)]
+        distances.append(point_segment_distance(a[0], a[1], ax, ay, bx, by))
+        distances.append(point_segment_distance(b[0], b[1], ax, ay, bx, by))
+    return min(distances)
+
+
+def analyze_route(
+    waypoints: list[Waypoint],
+    regions: list[AvoidRegion],
+    required_clearance: float,
+) -> list[SegmentRisk]:
+    risks: list[SegmentRisk] = []
+    for start, end in zip(waypoints, waypoints[1:]):
+        a = (start.x, start.y)
+        b = (end.x, end.y)
+        length = math.hypot(end.x - start.x, end.y - start.y)
+        for region in regions:
+            intersects = segment_polygon_intersects(a, b, region.polygon)
+            clearance = 0.0 if intersects else segment_polygon_distance(a, b, region.polygon)
+            risks.append(
+                SegmentRisk(
+                    start_id=start.id,
+                    end_id=end.id,
+                    region=region.name,
+                    clearance_m=clearance,
+                    required_m=required_clearance,
+                    margin_m=clearance - required_clearance,
+                    length_m=length,
+                    centerline_intersects=intersects,
+                )
+            )
+    risks.sort(key=lambda item: (item.margin_m, item.clearance_m))
+    return risks
+
+
+def parse_xml_summary(path: Path) -> dict[str, Any]:
+    if not path.exists():
+        return {"path": str(path), "exists": False}
+    root = ET.parse(path).getroot()
+    geoms = [geom for geom in root.iter("geom")]
+    collidable = [
+        geom for geom in geoms
+        if geom.get("name") != "floor"
+        and geom.get("contype", "1") != "0"
+        and geom.get("conaffinity", "1") != "0"
+    ]
+    return {
+        "path": str(path),
+        "exists": True,
+        "model": root.get("model", ""),
+        "geom_count": len(geoms),
+        "collidable_geom_count": len(collidable),
+    }
+
+
+def parse_onnx_summary(path: Path) -> dict[str, Any]:
+    if not path.exists():
+        return {"path": str(path), "exists": False}
+    try:
+        import onnx  # type: ignore
+    except Exception as exc:  # pragma: no cover - depends on local env
+        return {"path": str(path), "exists": True, "error": f"onnx import failed: {exc}"}
+
+    model = onnx.load(str(path))
+    inputs = [
+        {
+            "name": item.name,
+            "shape": [
+                dim.dim_value if dim.dim_value else dim.dim_param
+                for dim in item.type.tensor_type.shape.dim
+            ],
+        }
+        for item in model.graph.input
+    ]
+    outputs = [
+        {
+            "name": item.name,
+            "shape": [
+                dim.dim_value if dim.dim_value else dim.dim_param
+                for dim in item.type.tensor_type.shape.dim
+            ],
+        }
+        for item in model.graph.output
+    ]
+    return {
+        "path": str(path),
+        "exists": True,
+        "inputs": inputs,
+        "outputs": outputs,
+        "metadata_keys": [prop.key for prop in model.metadata_props],
+    }
+
+
+def risk_to_dict(risk: SegmentRisk) -> dict[str, Any]:
+    return {
+        "start_id": risk.start_id,
+        "end_id": risk.end_id,
+        "region": risk.region,
+        "clearance_m": round(risk.clearance_m, 6),
+        "required_m": round(risk.required_m, 6),
+        "margin_m": round(risk.margin_m, 6),
+        "length_m": round(risk.length_m, 6),
+        "centerline_intersects": risk.centerline_intersects,
+        "status": risk.status,
+    }
+
+
+def print_report(
+    points_path: Path,
+    xml_summary: dict[str, Any],
+    onnx_summary: dict[str, Any],
+    waypoints: list[Waypoint],
+    regions: list[AvoidRegion],
+    footprint_radius: float,
+    avoid_margin: float,
+    warn_margin: float,
+    risks: list[SegmentRisk],
+    top: int,
+) -> None:
+    required_clearance = footprint_radius + avoid_margin
+    violations = [risk for risk in risks if risk.margin_m < 0.0 or risk.centerline_intersects]
+    tight = [
+        risk for risk in risks
+        if risk.margin_m >= 0.0 and risk.margin_m < warn_margin
+    ]
+    route_len = sum(
+        math.hypot(b.x - a.x, b.y - a.y)
+        for a, b in zip(waypoints, waypoints[1:])
+    )
+
+    print("Route safety check")
+    print(f"  points: {points_path}")
+    print(f"  waypoints: {len(waypoints)}, regions: {len(regions)}, path_length: {route_len:.3f} m")
+    print(
+        "  clearance: "
+        f"footprint={footprint_radius:.3f} m + avoid_margin={avoid_margin:.3f} m "
+        f"=> required={required_clearance:.3f} m"
+    )
+    if xml_summary.get("exists"):
+        print(
+            "  xml: "
+            f"{xml_summary.get('path')} "
+            f"(model={xml_summary.get('model')}, geoms={xml_summary.get('geom_count')}, "
+            f"collidable={xml_summary.get('collidable_geom_count')})"
+        )
+    else:
+        print(f"  xml: missing ({xml_summary.get('path')})")
+    if onnx_summary.get("exists") and not onnx_summary.get("error"):
+        print(f"  onnx: {onnx_summary.get('path')}")
+        print(f"    inputs: {onnx_summary.get('inputs')}")
+        print(f"    outputs: {onnx_summary.get('outputs')}")
+    elif onnx_summary.get("exists"):
+        print(f"  onnx: {onnx_summary.get('error')}")
+    else:
+        print(f"  onnx: missing ({onnx_summary.get('path')})")
+
+    print("")
+    if violations:
+        print(f"FAIL: {len(violations)} segment-region pairs are inside required clearance.")
+    elif tight:
+        print(f"WARN: no violations, but {len(tight)} segment-region pairs are tight.")
+    else:
+        print("PASS: all segment-region pairs satisfy the requested clearance.")
+
+    print("")
+    print(f"Closest {min(top, len(risks))} segment-region pairs:")
+    print("  status      wp_start->wp_end  region       clear   req     spare")
+    for risk in risks[:top]:
+        print(
+            f"  {risk.status:<10}  "
+            f"{risk.start_id:>4}->{risk.end_id:<4}      "
+            f"{risk.region:<10}  "
+            f"{risk.clearance_m:>6.3f}  "
+            f"{risk.required_m:>6.3f}  "
+            f"{risk.margin_m:>7.3f}"
+        )
+
+
+def main() -> int:
+    args = parse_args()
+    points_path = args.points.resolve()
+    xml_path = args.xml.resolve()
+    onnx_path = args.onnx.resolve()
+
+    payload = load_json(points_path)
+    waypoints = load_waypoints(payload)
+    regions = load_regions(payload)
+    if not regions:
+        raise ValueError(f"No avoid regions found in {points_path}")
+
+    footprint_radius = (
+        float(args.footprint_radius)
+        if args.footprint_radius is not None
+        else default_lateral_footprint_radius()
+    )
+    required_clearance = footprint_radius + float(args.avoid_margin)
+    risks = analyze_route(waypoints, regions, required_clearance)
+    xml_summary = parse_xml_summary(xml_path)
+    onnx_summary = parse_onnx_summary(onnx_path)
+
+    print_report(
+        points_path,
+        xml_summary,
+        onnx_summary,
+        waypoints,
+        regions,
+        footprint_radius,
+        float(args.avoid_margin),
+        float(args.warn_margin),
+        risks,
+        max(0, int(args.top)),
+    )
+
+    violations = [risk for risk in risks if risk.margin_m < 0.0 or risk.centerline_intersects]
+    tight = [
+        risk for risk in risks
+        if risk.margin_m >= 0.0 and risk.margin_m < float(args.warn_margin)
+    ]
+    report = {
+        "points": str(points_path),
+        "waypoint_count": len(waypoints),
+        "region_count": len(regions),
+        "footprint_radius_m": round(footprint_radius, 6),
+        "avoid_margin_m": round(float(args.avoid_margin), 6),
+        "required_clearance_m": round(required_clearance, 6),
+        "violations": [risk_to_dict(risk) for risk in violations],
+        "tight": [risk_to_dict(risk) for risk in tight],
+        "closest": [risk_to_dict(risk) for risk in risks[: max(0, int(args.top))]],
+        "xml": xml_summary,
+        "onnx": onnx_summary,
+    }
+    if args.json_out:
+        args.json_out.parent.mkdir(parents=True, exist_ok=True)
+        args.json_out.write_text(json.dumps(report, ensure_ascii=False, indent=2), encoding="utf-8")
+
+    if violations and not args.allow_violations:
+        return 2
+    return 0
+
+
+if __name__ == "__main__":
+    try:
+        raise SystemExit(main())
+    except Exception as exc:
+        print(f"ERROR: {exc}", file=sys.stderr)
+        raise SystemExit(1)