[tools] 整理导航打点工具与抽样PCD

.gitignore

@@ -36,6 +36,9 @@ checkpoints/
 wandb/
 sim2sim_log_*.txt
 **/sim2sim_temp.xml
+**/route_check_runs/
+**/route_experiments/suite_*/
+**/tools/nav_tools/points/auto_candidates/
 
 # IDE and operating system files
 .idea/

01_doc/version_history.md

@@ -12,6 +12,8 @@
 | `v0.5.0` | 随机化增强 | 扩大观测、延迟和动力学随机化，加入持续外力扰动 |
 | `v0.6.0` | 比赛训练架构 | 分轴奖励、自适应指令课程、障碍释放课程和比赛站姿 |
 | `v0.7.0` | MuJoCo 工具 | 姿态优化、IK 扫描、动力学、MPC 和 GUI 调试工具 |
+| `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
+| `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
 
 ## `v0.4.0` 的模型变化
 
@@ -51,3 +53,21 @@
 - 增加 IK/差速轮参数扫描，可导出 JSON 结果。
 - 增加 Robot、Controller、Dynamics、MPCController 和 GUI 调试链路。
 - 记录历史工具常量与新版 MJCF 质量、比赛默认站姿之间的参数边界，避免将分析结果直接当作已校准真机参数。
+
+## `v0.8.0` 的后期 Sim2Sim
+
+- 比赛训练任务、MJCF 和 `v0.7.0` 的 MuJoCo 工具保持不变。
+- 策略运行器增加 ONNX 加载，并允许在缺少 `pynput` 时关闭后台键盘监听继续运行。
+- MuJoCo 执行器重建同时兼容新旧 Spec 删除接口。
+- 增加 PT→ONNX 导出、IK 补偿扫描、纯 IK 绕桩和 ONNX 批量路线检查入口。
+- 归档比赛最终 Rough 策略 `model_6800.onnx`；其 SHA-256 为 `3C994BDD3434AD15770A52AC0E8D229F502F00D6511CDD42C2E2C742301AEF13`。
+- Crawl 权重、运行日志、临时 XML 和大量重复路线实验不在本阶段归档。
+
+## `v0.8.1` 的导航打点工具
+
+- 补充 Pygame 地图/PCD/航点综合编辑器、避障区域编辑器和坐标变换工具。
+- 补充路线安全检查、候选航点优化、XML/航点镜像和批量 Sim2Sim 实验入口。
+- 按源文件时间保留 14 份比赛路线 JSON，不将开发期文件名误解释为正式版本号。
+- 补充 `1hao.xml`、`2hao.xml`、`A_C.xml`，并为 `1B_FF.json` 补齐其引用的 `B_C.xml`。
+- 将两份约 915 MiB 的原始 ASCII PCD 确定性抽样为各小于 10 MB 的预览点云；抽样参数、点数和哈希记录在工具 README。
+- 训练代码、MJCF、比赛策略和历史依赖锁保持 `v0.8.0` 状态不变。

05_software/README.md

@@ -28,7 +28,7 @@ MJCF + mjlab task
 IK real --------------------------------> 电机
 ```
 
-`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim 和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`。
+`rc_mjlab` 是自包含工程。训练、MJCF、MuJoCo、Sim2Sim、导航工具和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.0`，后期 MuJoCo 工具集见 `v0.7.0`，后期 Sim2Sim 与比赛 Rough 策略见 `v0.8.0`，完整导航打点工具见 `v0.8.1`。
 
 详细说明见：
 

05_software/train/README.md

@@ -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.1`：训练代码保持 `v0.6.0` 的比赛架构，包含后期 MuJoCo、Sim2Sim、比赛最终 Rough ONNX 策略，并补充完整导航打点工具、路线迭代和抽样 PCD。训练过程可能先获得基模，再调整奖励、课程和环境参数继续训练；模型 checkpoint 的变化不等同于软件架构变化。
 
 ## 内容
 
@@ -10,8 +10,10 @@
 - `mjcf`：轮足机器人 MuJoCo 模型和网格
 - `sim2sim`：策略加载、交互控制和比赛地形验证
 - `mujoco_sim`：不依赖训练循环的姿态、IK、动力学和 MPC 分析
+- `tools/nav_tools`：地图/PCD 查看、航点编辑、路线检查和比赛路线数据
 - `mjlab`：固定版本的本地训练框架依赖
 - `model_rough.pt`：本阶段 Rough 策略权重
+- `model_6800.onnx`：比赛最终使用的 Rough 策略
 - `pyproject.toml`、`uv.lock`：Python 环境与依赖锁定
 
 与 `v0.3.0` 相比，本版本更新了 MJCF 质量和惯性参数，并将 mjlab 上游基准从 `00409797` 更新到 `40f8d93e`。机械 CAD 未发生变化。
@@ -22,4 +24,8 @@
 
 `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)。
+
+`v0.8.1` 补充完整导航打点工具和小体积预览点云；入口、路线清单和抽样边界见 [`rc_mjlab/tools/nav_tools/README.md`](rc_mjlab/tools/nav_tools/README.md)。
+
 工程命令和任务说明见 [`rc_mjlab/README.md`](rc_mjlab/README.md)，本地依赖来源见 [`rc_mjlab/DEPENDENCIES.md`](rc_mjlab/DEPENDENCIES.md)。

05_software/train/rc_mjlab/DEPENDENCIES.md

@@ -8,6 +8,8 @@
 - `mjlab[cu128]`
 - PyTorch CUDA 12.8 环境
 - `pynput`
+- 后期 Sim2Sim 可选依赖：Pygame、ONNX Runtime
+- 导航打点可选依赖：Pygame、Pillow
 
 精确解析结果保存在 `uv.lock`。项目使用本地可编辑的 `mjlab`：
 
@@ -39,4 +41,16 @@ 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
+```
+
+导航打点工具同样不修改历史锁文件：
+
+```bash
+uv run --with-requirements tools/nav_tools/requirements.txt python tools/nav_tools/nav_map_viewer.py
+```
+
 GPU、CUDA、MuJoCo development wheel 和驱动版本必须满足 `pyproject.toml` 与 `uv.lock` 的约束。

05_software/train/rc_mjlab/README.md

@@ -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.1`：保留比赛训练架构、后期 MuJoCo/Sim2Sim 和比赛最终 Rough 策略，并补充导航地图、打点工具、路线迭代与抽样 PCD。`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/              # 地图/PCD/航点编辑、路线检查与比赛路线数据
+├── model_rough.pt                # 早期 Rough 参考 checkpoint
+├── model_6800.onnx               # 比赛最终 Rough 策略
 ├── pyproject.toml                # 项目依赖（uv 管理，含清华镜像源加速）
 └── uv.lock                       # 精确依赖锁定文件
 ```
@@ -89,6 +95,10 @@ cd sim2sim
 uv run python nav_sim2sim.py
 ```
 
+后期 Sim2Sim 的入口、模型边界和批量检查命令见 [`sim2sim/README.md`](sim2sim/README.md)。
+
+导航打点工具、路线快照和抽样点云说明见 [`tools/nav_tools/README.md`](tools/nav_tools/README.md)。
+
 ---
 
 ## 🖥️ 交互式自动导航平台 (sim2sim/nav_sim2sim.py)

05_software/train/rc_mjlab/sim2sim/README.md

@@ -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` 的航点和避障几何定义；默认使用 `points_20260715_120154.json` 与 `1hao.xml`。
+
+运行时生成的日志、临时 XML、`route_check_runs/` 和批量实验输出不纳入版本库。人工打点形成的路线快照保存在 `../tools/nav_tools/points/`，大量重复仿真轨迹仍不复制。

05_software/train/rc_mjlab/sim2sim/export_onnx.py

@@ -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()

05_software/train/rc_mjlab/sim2sim/ik_compensation_sweep.py

@@ -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())

05_software/train/rc_mjlab/sim2sim/interface/mujoco_io.py

@@ -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.

05_software/train/rc_mjlab/sim2sim/nav_sim2sim.py

@@ -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% 可视化精准对应

05_software/train/rc_mjlab/sim2sim/policy/policy_runner.py

@@ -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:

05_software/train/rc_mjlab/sim2sim/requirements.txt

@@ -0,0 +1,3 @@
+# Additional runtime dependencies for the post-training Sim2Sim tools.
+onnxruntime>=1.19.0
+pygame>=2.6.1

05_software/train/rc_mjlab/sim2sim/sim2sim.py

@@ -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"

05_software/train/rc_mjlab/tools/nav_tools/README.md

@@ -0,0 +1,93 @@
+# 导航地图与打点工具
+
+本目录保存比赛后期使用的独立导航地图、PCD 查看、航点编辑、避障区域编辑和路线验证工具。它同时为 `sim2sim/nav_route_sim2sim_check.py` 提供航点与避障几何定义。
+
+## 目录
+
+```text
+nav_tools/
+├─ nav_map_viewer.py               # 地图、PCD、XML、航点和避障区综合编辑器
+├─ avoid_region_tool.py            # 独立避障多边形编辑器
+├─ route_safety_check.py           # 离线路线净空检查
+├─ route_candidate_optimizer.py    # 航点候选优化
+├─ run_route_experiments.py        # 安全检查与 Sim2Sim 批量实验
+├─ mirror_nav_xml_points.py        # XML 与航点镜像
+├─ pcd_transform_tool.py           # PCD 平移和旋转
+├─ transform_pcd_xy.py             # PCD 坐标原点变换
+├─ downsample_ascii_pcd.py         # ASCII PCD 确定性抽样
+├─ annotate_odin1_relocalization_frame.py
+├─ pcd/                            # 小于 10 MB 的打点预览点云
+├─ points/                         # 比赛期间的路线迭代 JSON
+├─ xml/                            # 1 号、2 号和 A/B 场地 XML
+├─ regions/                        # 独立避障区域输出目录
+└─ assets/                         # 坐标和机构示意图
+```
+
+## 运行
+
+在 `rc_mjlab` 根目录执行：
+
+```powershell
+# 地图与航点综合编辑器
+uv run --with-requirements .\tools\nav_tools\requirements.txt `
+  python .\tools\nav_tools\nav_map_viewer.py
+
+# 指定 PCD 的独立避障区域编辑器
+uv run --with-requirements .\tools\nav_tools\requirements.txt `
+  python .\tools\nav_tools\avoid_region_tool.py --pcd 1hao.pcd
+
+# 默认检查 points_20260715_120154.json 与 1hao.xml
+uv run python .\tools\nav_tools\route_safety_check.py
+
+# 使用最终 Rough 策略进行默认路线 Sim2Sim 检查
+uv run --with-requirements .\sim2sim\requirements.txt `
+  python .\sim2sim\nav_route_sim2sim_check.py
+```
+
+候选路线和批量实验必须显式保存为新文件，不应覆盖历史路线：
+
+```powershell
+uv run python .\tools\nav_tools\route_candidate_optimizer.py
+
+uv run --with-requirements .\sim2sim\requirements.txt `
+  python .\tools\nav_tools\run_route_experiments.py `
+  --points .\tools\nav_tools\points\points_20260715_120154.json
+```
+
+## PCD 抽样
+
+仓库中的两份 PCD 从原始 ASCII 点云按固定步长均匀抽样，字段、坐标和 PCD 头结构保持不变。它们面向地图显示和人工打点，不替代原始高密度点云用于建图、定位精度评估或点云算法基准。
+
+| 文件 | 原始点数 | 抽样步长 | 仓库点数 | 仓库大小 | SHA-256 |
+| --- | ---: | ---: | ---: | ---: | --- |
+| `pcd/1hao.pcd` | 9,163,893 | 46 | 199,215 | 9,876,010 B | `48B231C52BECA51316F352300C8B2046133E92359E0855227D93DEB0D927AD34` |
+| `pcd/2hao.pcd` | 10,043,048 | 52 | 193,136 | 9,719,736 B | `714516A7A726D46311A58507149FBC93D622616BE79C59FFED622274D6526B1F` |
+
+复现抽样：
+
+```powershell
+python .\tools\nav_tools\downsample_ascii_pcd.py `
+  <原始PCD> <输出PCD> --max-bytes 9900000
+```
+
+## 航点数据
+
+`points/` 保留原文件名和时间顺序，没有把多个路线重命名成 `old`、`new` 或 `final`。各文件的航点数、避障区和 XML 绑定见 [`points/README.md`](points/README.md)。
+
+- 请求目录中的 `route_safety_check.py` 是较早版本，因此保留 `v0.8.0` 已归档的后期兼容版本。
+- `1B_FF.json` 引用了请求目录中缺失的 `xml/B_C.xml`，本次从后期整合目录补齐该文件。
+- `.uv-cache`、`__pycache__`、自动候选、实验日志和重复轨迹输出不归档。
+
+整理时使用默认圆形包络和 `0.05 m` 额外净空检查 `points_20260715_120154.json`，报告了 3 组线段—避障区净空不足。该结果按原样保留，未自动移动航点；它表示保守几何检查仍有待复核，不等同于路线没有经过实机使用。
+
+## 主要操作
+
+- `Pan`：拖动地图；滚轮缩放；`F` 适应窗口。
+- `Point`：添加、插入、选择和编辑航点；`Backspace` 删除。
+- `Terrain`：选择并调整 XML 障碍组的位置和偏航。
+- `Avoid`：绘制避障多边形；`Enter` 闭合；`Delete` 删除。
+- `Load JSON`：载入 `waypoints` 或 `segments[].waypoints`。
+- `Save JSON` / `Save All JSON`：同时保存航点和避障区域。
+- `M`：切换 XML；`O`：切换坐标原点；`Esc`：退出。
+
+任务字段约定：`none`、`slalom`、`gravel`、`wall`、`low_bar`、`stairs`、`ramp_bridge`、`spawn` 和 `return`。

05_software/train/rc_mjlab/tools/nav_tools/annotate_odin1_relocalization_frame.py

@@ -0,0 +1,145 @@
+#!/usr/bin/env python3
+"""Generate an annotated Odin1 relocalization-frame PNG.
+
+The wiki defines Odin1 frames as:
+  I: IMU frame
+  L: LiDAR / point-cloud frame
+  C = camera frame
+
+Relocalization poses are tied to the SLAM point-cloud map, so the device pose
+should be treated as T_map_L unless a driver-specific TF remaps it.
+"""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+from PIL import Image, ImageDraw, ImageFont
+
+
+TOOL_DIR = Path(__file__).resolve().parent
+IMG_DIR = TOOL_DIR / "assets"
+OUT = TOOL_DIR / "odin1_relocalization_frame.png"
+
+
+def font(size: int, bold: bool = False) -> ImageFont.FreeTypeFont | ImageFont.ImageFont:
+    return ImageFont.load_default()
+
+
+def arrow(draw: ImageDraw.ImageDraw, start: tuple[int, int], end: tuple[int, int], color: tuple[int, int, int], width: int = 5) -> None:
+    draw.line([start, end], fill=color, width=width)
+    sx, sy = start
+    ex, ey = end
+    dx = ex - sx
+    dy = ey - sy
+    length = max((dx * dx + dy * dy) ** 0.5, 1.0)
+    ux, uy = dx / length, dy / length
+    px, py = -uy, ux
+    head = 18
+    wing = 9
+    points = [
+        (ex, ey),
+        (int(ex - ux * head + px * wing), int(ey - uy * head + py * wing)),
+        (int(ex - ux * head - px * wing), int(ey - uy * head - py * wing)),
+    ]
+    draw.polygon(points, fill=color)
+
+
+def label_box(
+    draw: ImageDraw.ImageDraw,
+    xy: tuple[int, int],
+    text: str,
+    fill: tuple[int, int, int],
+    text_color: tuple[int, int, int] = (255, 255, 255),
+    size: int = 24,
+) -> None:
+    x, y = xy
+    fnt = font(size, bold=True)
+    lines = text.splitlines()
+    widths = [draw.textbbox((0, 0), line, font=fnt)[2] for line in lines]
+    heights = [draw.textbbox((0, 0), line, font=fnt)[3] - draw.textbbox((0, 0), line, font=fnt)[1] for line in lines]
+    box_w = max(widths) + 28
+    box_h = sum(heights) + 14 * (len(lines) - 1) + 24
+    draw.rounded_rectangle((x, y, x + box_w, y + box_h), radius=10, fill=fill, outline=(255, 255, 255), width=2)
+    cy = y + 12
+    for line, h in zip(lines, heights):
+        draw.text((x + 14, cy), line, fill=text_color, font=fnt)
+        cy += h + 14
+
+
+def main() -> int:
+    coordinate = Image.open(IMG_DIR / "coordinate.png").convert("RGB")
+    structure = Image.open(IMG_DIR / "structure1.png").convert("RGB")
+
+    target_w = 1180
+    coordinate = coordinate.resize((target_w, int(coordinate.height * target_w / coordinate.width)))
+    structure = structure.resize((target_w, int(structure.height * target_w / structure.width)))
+
+    gap = 24
+    margin = 30
+    title_h = 118
+    canvas = Image.new("RGB", (target_w + margin * 2, title_h + coordinate.height + gap + structure.height + margin), (245, 247, 250))
+    draw = ImageDraw.Draw(canvas)
+
+    title_font = font(34, bold=True)
+    body_font = font(22)
+    draw.text((margin, 22), "Odin1 relocalization frame: LiDAR / point-cloud frame L", fill=(15, 23, 42), font=title_font)
+    draw.text(
+        (margin, 68),
+        "Use the relocalization pose as T_map_L. Convert to robot base with your measured T_L_base.",
+        fill=(51, 65, 85),
+        font=body_font,
+    )
+
+    top_y = title_h
+    bot_y = title_h + coordinate.height + gap
+    canvas.paste(coordinate, (margin, top_y))
+    canvas.paste(structure, (margin, bot_y))
+
+    red = (220, 38, 38)
+    blue = (37, 99, 235)
+    green = (22, 163, 74)
+    amber = (217, 119, 6)
+
+    # coordinate.png positions after scaling to 1180 px.
+    lidar_origin = (margin + 690, top_y + 255)
+    imu_origin = (margin + 286, top_y + 655)
+    camera_origin = (margin + 885, top_y + 500)
+
+    draw.ellipse((lidar_origin[0] - 12, lidar_origin[1] - 12, lidar_origin[0] + 12, lidar_origin[1] + 12), fill=red, outline=(255, 255, 255), width=3)
+    arrow(draw, (margin + 900, top_y + 150), lidar_origin, red, width=6)
+    label_box(draw, (margin + 725, top_y + 70), "Relocalization pose\nis here: frame L", red)
+
+    draw.ellipse((imu_origin[0] - 9, imu_origin[1] - 9, imu_origin[0] + 9, imu_origin[1] + 9), fill=blue)
+    arrow(draw, (margin + 160, top_y + 555), imu_origin, blue, width=4)
+    label_box(draw, (margin + 40, top_y + 460), "IMU frame I\nnot the relocalization origin", blue, size=20)
+
+    draw.ellipse((camera_origin[0] - 9, camera_origin[1] - 9, camera_origin[0] + 9, camera_origin[1] + 9), fill=green)
+    arrow(draw, (margin + 1015, top_y + 565), camera_origin, green, width=4)
+    label_box(draw, (margin + 880, top_y + 585), "Camera frame C\nseparate optical frame", green, size=20)
+
+    # structure1.png positions after scaling to 1180 px.
+    struct_imu = (margin + 398, bot_y + 357)
+    struct_lidar_hint = (margin + 677, bot_y + 350)
+    draw.ellipse((struct_imu[0] - 9, struct_imu[1] - 9, struct_imu[0] + 9, struct_imu[1] + 9), fill=blue)
+    arrow(draw, (margin + 245, bot_y + 250), struct_imu, blue, width=4)
+    label_box(draw, (margin + 45, bot_y + 165), "Wiki marks IMU separately", blue, size=20)
+
+    arrow(draw, (margin + 820, bot_y + 230), struct_lidar_hint, red, width=5)
+    label_box(draw, (margin + 830, bot_y + 145), "L is the point-cloud/LiDAR frame\nshown in the coordinate diagram", red, size=20)
+
+    note = (
+        "Fixed wiki extrinsic: T^imu_lidar translation = [-0.02663, 0.03447, 0.02174] m, rotation = identity.\n"
+        "So L and I axes are parallel, but their origins are offset. Do not use shell center as pose origin."
+    )
+    note_box = (margin, canvas.height - margin - 78, canvas.width - margin, canvas.height - margin)
+    draw.rounded_rectangle(note_box, radius=10, fill=(255, 251, 235), outline=amber, width=2)
+    draw.text((margin + 16, canvas.height - margin - 64), note, fill=(120, 53, 15), font=font(19))
+
+    canvas.save(OUT)
+    print(OUT)
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

05_software/train/rc_mjlab/tools/nav_tools/avoid_region_tool.py

@@ -0,0 +1,373 @@
+#!/usr/bin/env python3
+"""Pygame tool for marking no-go / avoid regions on a PCD map."""
+
+from __future__ import annotations
+
+import argparse
+import json
+import math
+import os
+import pkgutil
+import time
+from dataclasses import dataclass, field
+from pathlib import Path
+
+os.environ.setdefault("PYGAME_HIDE_SUPPORT_PROMPT", "1")
+
+if not hasattr(pkgutil, "ImpImporter"):
+    pkgutil.ImpImporter = pkgutil.zipimporter  # type: ignore[attr-defined]
+
+import pygame
+
+
+TOOL_DIR = Path(__file__).resolve().parent
+PCD_DIR = TOOL_DIR / "pcd"
+REGION_DIR = TOOL_DIR / "regions"
+
+COLOR_BG = (10, 15, 30)
+COLOR_GRID = (22, 29, 48)
+COLOR_PANEL = (18, 24, 42)
+COLOR_BORDER = (50, 65, 95)
+COLOR_TEXT = (248, 250, 252)
+COLOR_MUTED = (148, 163, 184)
+COLOR_PCD = (92, 160, 255)
+COLOR_REGION = (244, 63, 94)
+COLOR_CURRENT = (234, 179, 8)
+
+
+@dataclass
+class PointCloud:
+    path: Path | None = None
+    points: list[tuple[float, float, float]] = field(default_factory=list)
+    total_count: int = 0
+    sampled_count: int = 0
+    error: str = ""
+
+
+@dataclass
+class Region:
+    name: str
+    points: list[tuple[float, float]]
+    kind: str = "avoid"
+
+
+class Camera:
+    def __init__(self, width: int, height: int, panel_width: int = 330) -> None:
+        self.width = width
+        self.height = height
+        self.panel_width = panel_width
+        self.zoom = 55.0
+        self.pan_x = 0.0
+        self.pan_y = 0.0
+
+    @property
+    def map_width(self) -> int:
+        return max(200, self.width - self.panel_width)
+
+    def world_to_screen(self, x: float, y: float) -> tuple[int, int]:
+        return int(self.map_width / 2 + x * self.zoom + self.pan_x), int(self.height / 2 - y * self.zoom + self.pan_y)
+
+    def screen_to_world(self, sx: float, sy: float) -> tuple[float, float]:
+        return (sx - self.map_width / 2 - self.pan_x) / self.zoom, -(sy - self.height / 2 - self.pan_y) / self.zoom
+
+    def zoom_at(self, factor: float, pos: tuple[int, int]) -> None:
+        before = self.screen_to_world(*pos)
+        self.zoom = max(8.0, min(260.0, self.zoom * factor))
+        after = self.screen_to_world(*pos)
+        self.pan_x += (after[0] - before[0]) * self.zoom
+        self.pan_y -= (after[1] - before[1]) * self.zoom
+
+
+def get_font(size: int, bold: bool = False) -> pygame.font.Font:
+    return pygame.font.Font(None, size)
+
+
+def scan_pcd_files() -> list[Path]:
+    PCD_DIR.mkdir(parents=True, exist_ok=True)
+    return sorted(PCD_DIR.glob("*.pcd"), key=lambda item: item.name.lower())
+
+
+def iter_ascii_pcd_points(path: Path):
+    with path.open("r", encoding="utf-8", errors="ignore") as handle:
+        data = False
+        for raw in handle:
+            line = raw.strip()
+            if data and line:
+                parts = line.split()
+                if len(parts) >= 3:
+                    yield float(parts[0]), float(parts[1]), float(parts[2])
+            elif line.upper().startswith("DATA"):
+                if "ascii" not in line.lower():
+                    raise RuntimeError(f"Only ASCII PCD is supported: {path}")
+                data = True
+
+
+def load_pcd(path: Path, max_points: int = 90000, z_min: float = -5.0, z_max: float = 0.5) -> PointCloud:
+    cloud = PointCloud(path=path)
+    try:
+        filtered = []
+        for x, y, z in iter_ascii_pcd_points(path):
+            cloud.total_count += 1
+            if z_min <= z <= z_max:
+                filtered.append((x, y, z))
+        stride = max(1, math.ceil(len(filtered) / max_points)) if filtered else 1
+        cloud.points = filtered[::stride]
+        cloud.sampled_count = len(cloud.points)
+    except Exception as exc:
+        cloud.error = str(exc)
+    return cloud
+
+
+def fit_camera(camera: Camera, cloud: PointCloud, regions: list[Region]) -> None:
+    xs = [p[0] for p in cloud.points]
+    ys = [p[1] for p in cloud.points]
+    for region in regions:
+        xs.extend(p[0] for p in region.points)
+        ys.extend(p[1] for p in region.points)
+    if not xs or not ys:
+        return
+    min_x, max_x = min(xs), max(xs)
+    min_y, max_y = min(ys), max(ys)
+    span_x = max(max_x - min_x, 1.0)
+    span_y = max(max_y - min_y, 1.0)
+    margin = 70
+    camera.zoom = max(8.0, min(260.0, min((camera.map_width - margin * 2) / span_x, (camera.height - margin * 2) / span_y)))
+    camera.pan_x = -((min_x + max_x) / 2) * camera.zoom
+    camera.pan_y = ((min_y + max_y) / 2) * camera.zoom
+
+
+def draw_grid(surface: pygame.Surface, camera: Camera, font: pygame.font.Font) -> None:
+    min_x, min_y = camera.screen_to_world(0, camera.height)
+    max_x, max_y = camera.screen_to_world(camera.map_width, 0)
+    for gx in range(math.floor(min_x), math.ceil(max_x) + 1):
+        sx, _ = camera.world_to_screen(gx, 0)
+        color = (38, 50, 78) if gx == 0 else COLOR_GRID
+        pygame.draw.line(surface, color, (sx, 0), (sx, camera.height), 2 if gx == 0 else 1)
+        if gx % 2 == 0 and 0 < sx < camera.map_width - 30:
+            surface.blit(font.render(f"{gx}m", True, COLOR_MUTED), (sx + 4, camera.height - 22))
+    for gy in range(math.floor(min_y), math.ceil(max_y) + 1):
+        _, sy = camera.world_to_screen(0, gy)
+        color = (38, 50, 78) if gy == 0 else COLOR_GRID
+        pygame.draw.line(surface, color, (0, sy), (camera.map_width, sy), 2 if gy == 0 else 1)
+
+
+def draw_pcd(surface: pygame.Surface, camera: Camera, cloud: PointCloud) -> None:
+    for x, y, z in cloud.points:
+        sx, sy = camera.world_to_screen(x, y)
+        if 0 <= sx < camera.map_width and 0 <= sy < camera.height:
+            color = (148, 210, 255) if z > 0.3 else COLOR_PCD
+            surface.set_at((sx, sy), color)
+
+
+def draw_regions(surface: pygame.Surface, camera: Camera, regions: list[Region], current: list[tuple[float, float]]) -> None:
+    overlay = pygame.Surface(surface.get_size(), pygame.SRCALPHA)
+    for region in regions:
+        points = [camera.world_to_screen(x, y) for x, y in region.points]
+        if len(points) >= 3:
+            pygame.draw.polygon(overlay, (*COLOR_REGION, 75), points)
+            pygame.draw.polygon(surface, COLOR_REGION, points, width=2)
+    if current:
+        points = [camera.world_to_screen(x, y) for x, y in current]
+        for point in points:
+            pygame.draw.circle(surface, COLOR_CURRENT, point, 5)
+        if len(points) >= 2:
+            pygame.draw.lines(surface, COLOR_CURRENT, False, points, width=2)
+    surface.blit(overlay, (0, 0))
+
+
+def save_regions(regions: list[Region], cloud: PointCloud) -> Path:
+    REGION_DIR.mkdir(parents=True, exist_ok=True)
+    stamp = time.strftime("%Y%m%d_%H%M%S")
+    path = REGION_DIR / f"avoid_regions_{stamp}.json"
+    payload = {
+        "name": f"avoid_regions_{stamp}",
+        "map": cloud.path.stem if cloud.path else "",
+        "frame_id": "map",
+        "regions": [
+            {
+                "id": index,
+                "name": region.name,
+                "kind": region.kind,
+                "polygon": [{"x": x, "y": y} for x, y in region.points],
+            }
+            for index, region in enumerate(regions, start=1)
+        ],
+    }
+    path.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8")
+    return path
+
+
+def draw_panel(
+    surface: pygame.Surface,
+    camera: Camera,
+    title_font: pygame.font.Font,
+    small_font: pygame.font.Font,
+    pcd_files: list[Path],
+    selected_pcd: int,
+    cloud: PointCloud,
+    regions: list[Region],
+    current: list[tuple[float, float]],
+    saved: Path | None,
+) -> list[tuple[pygame.Rect, str]]:
+    x0 = camera.map_width
+    pygame.draw.rect(surface, COLOR_PANEL, (x0, 0, camera.panel_width, camera.height))
+    pygame.draw.line(surface, COLOR_BORDER, (x0, 0), (x0, camera.height), 2)
+    x = x0 + 18
+    y = 16
+    buttons: list[tuple[pygame.Rect, str]] = []
+
+    surface.blit(title_font.render("avoid regions", True, COLOR_TEXT), (x, y))
+    y += 34
+    label = pcd_files[selected_pcd].name if pcd_files and selected_pcd >= 0 else "put .pcd in nav_tools/pcd"
+    surface.blit(small_font.render(f"PCD: {label[:32]}", True, COLOR_MUTED), (x, y))
+    y += 24
+    if cloud.error:
+        surface.blit(small_font.render(cloud.error[:36], True, COLOR_REGION), (x, y))
+    else:
+        surface.blit(small_font.render(f"points: {cloud.sampled_count}/{cloud.total_count}", True, COLOR_MUTED), (x, y))
+    y += 34
+
+    for text, action in (("Prev PCD", "prev_pcd"), ("Next PCD", "next_pcd"), ("Fit", "fit"), ("Save JSON", "save")):
+        rect = pygame.Rect(x, y, 128, 26)
+        pygame.draw.rect(surface, (30, 41, 59), rect, border_radius=5)
+        pygame.draw.rect(surface, COLOR_BORDER, rect, width=1, border_radius=5)
+        surface.blit(small_font.render(text, True, COLOR_TEXT), (rect.x + 10, rect.y + 6))
+        buttons.append((rect, action))
+        y += 32
+
+    y += 12
+    lines = [
+        f"regions: {len(regions)}",
+        f"current vertices: {len(current)}",
+        "",
+        "Left click: add vertex",
+        "Enter: close polygon",
+        "Backspace: undo vertex",
+        "Delete: remove last region",
+        "Right/Middle drag: pan",
+        "Wheel: zoom",
+        "S: save, F: fit",
+    ]
+    for line in lines:
+        color = COLOR_TEXT if line and ":" not in line else COLOR_MUTED
+        surface.blit(small_font.render(line, True, color), (x, y))
+        y += 20
+    if saved:
+        y += 8
+        surface.blit(small_font.render(f"saved: {saved.name[:28]}", True, (16, 185, 129)), (x, y))
+    return buttons
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description="Mark avoid/no-go regions on a PCD map.")
+    parser.add_argument("--width", type=int, default=1280)
+    parser.add_argument("--height", type=int, default=820)
+    parser.add_argument("--pcd", type=Path, help="PCD file name/path. Relative paths are resolved from tools/nav_tools/pcd.")
+    return parser.parse_args()
+
+
+def main() -> int:
+    args = parse_args()
+    pcd_files = scan_pcd_files()
+    selected_pcd = 0
+    if args.pcd:
+        if args.pcd.is_absolute():
+            raise SystemExit("Use a PCD file name under tools/nav_tools/pcd, not an absolute path.")
+        requested = PCD_DIR / args.pcd
+        if requested.exists() and requested not in pcd_files:
+            pcd_files.append(requested)
+        if requested in pcd_files:
+            selected_pcd = pcd_files.index(requested)
+    cloud = load_pcd(pcd_files[selected_pcd]) if pcd_files else PointCloud()
+    regions: list[Region] = []
+    current: list[tuple[float, float]] = []
+    saved: Path | None = None
+
+    pygame.init()
+    pygame.font.init()
+    screen = pygame.display.set_mode((args.width, args.height), pygame.RESIZABLE)
+    pygame.display.set_caption("nav_tools - avoid region editor")
+    title_font = get_font(18, bold=True)
+    small_font = get_font(13)
+    map_font = get_font(12)
+    camera = Camera(args.width, args.height)
+    fit_camera(camera, cloud, regions)
+    clock = pygame.time.Clock()
+    dragging = False
+    last_mouse = (0, 0)
+    buttons: list[tuple[pygame.Rect, str]] = []
+    running = True
+
+    while running:
+        for event in pygame.event.get():
+            if event.type == pygame.QUIT:
+                running = False
+            elif event.type == pygame.VIDEORESIZE:
+                camera.width, camera.height = event.w, event.h
+                screen = pygame.display.set_mode((event.w, event.h), pygame.RESIZABLE)
+            elif event.type == pygame.KEYDOWN:
+                if event.key == pygame.K_ESCAPE:
+                    running = False
+                elif event.key in (pygame.K_RETURN, pygame.K_KP_ENTER):
+                    if len(current) >= 3:
+                        regions.append(Region(f"avoid_{len(regions) + 1}", current[:]))
+                        current.clear()
+                elif event.key == pygame.K_BACKSPACE and current:
+                    current.pop()
+                elif event.key == pygame.K_DELETE and regions:
+                    regions.pop()
+                elif event.key == pygame.K_f:
+                    fit_camera(camera, cloud, regions)
+                elif event.key == pygame.K_s:
+                    saved = save_regions(regions, cloud)
+            elif event.type == pygame.MOUSEBUTTONDOWN:
+                clicked = None
+                for rect, action in buttons:
+                    if rect.collidepoint(event.pos):
+                        clicked = action
+                        break
+                if event.button == 1 and clicked:
+                    if clicked == "prev_pcd" and pcd_files:
+                        selected_pcd = (selected_pcd - 1) % len(pcd_files)
+                        cloud = load_pcd(pcd_files[selected_pcd])
+                        fit_camera(camera, cloud, regions)
+                    elif clicked == "next_pcd" and pcd_files:
+                        selected_pcd = (selected_pcd + 1) % len(pcd_files)
+                        cloud = load_pcd(pcd_files[selected_pcd])
+                        fit_camera(camera, cloud, regions)
+                    elif clicked == "fit":
+                        fit_camera(camera, cloud, regions)
+                    elif clicked == "save":
+                        saved = save_regions(regions, cloud)
+                elif event.button == 1 and event.pos[0] < camera.map_width:
+                    current.append(camera.screen_to_world(*event.pos))
+                elif event.button in (2, 3) or (event.button == 1 and event.pos[0] >= camera.map_width):
+                    dragging = True
+                    last_mouse = event.pos
+                elif event.button == 4 and event.pos[0] < camera.map_width:
+                    camera.zoom_at(1.12, event.pos)
+                elif event.button == 5 and event.pos[0] < camera.map_width:
+                    camera.zoom_at(1 / 1.12, event.pos)
+            elif event.type == pygame.MOUSEBUTTONUP:
+                dragging = False
+            elif event.type == pygame.MOUSEMOTION and dragging:
+                dx = event.pos[0] - last_mouse[0]
+                dy = event.pos[1] - last_mouse[1]
+                camera.pan_x += dx
+                camera.pan_y += dy
+                last_mouse = event.pos
+
+        screen.fill(COLOR_BG)
+        draw_grid(screen, camera, map_font)
+        draw_pcd(screen, camera, cloud)
+        draw_regions(screen, camera, regions, current)
+        buttons = draw_panel(screen, camera, title_font, small_font, pcd_files, selected_pcd, cloud, regions, current, saved)
+        pygame.display.flip()
+        clock.tick(60)
+
+    pygame.quit()
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

05_software/train/rc_mjlab/tools/nav_tools/downsample_ascii_pcd.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+"""Deterministically downsample an ASCII PCD file to a byte-size limit."""
+
+from __future__ import annotations
+
+import argparse
+import math
+import os
+from pathlib import Path
+
+
+DEFAULT_MAX_BYTES = 9_900_000
+
+
+def read_header(path: Path) -> tuple[list[bytes], int, int]:
+    header: list[bytes] = []
+    declared_points: int | None = None
+
+    with path.open("rb") as stream:
+        while True:
+            line = stream.readline()
+            if not line:
+                raise ValueError(f"PCD header has no DATA line: {path}")
+            header.append(line)
+            fields = line.strip().split(maxsplit=1)
+            if fields and fields[0].upper() == b"POINTS" and len(fields) == 2:
+                declared_points = int(fields[1])
+            if fields and fields[0].upper() == b"DATA":
+                if len(fields) != 2 or fields[1].lower() != b"ascii":
+                    raise ValueError("Only DATA ascii PCD files are supported")
+                data_offset = stream.tell()
+                break
+
+    if declared_points is None:
+        raise ValueError(f"PCD header has no POINTS field: {path}")
+    return header, data_offset, declared_points
+
+
+def render_header(header: list[bytes], point_count: int) -> bytes:
+    rendered: list[bytes] = []
+    replaced_width = False
+    replaced_points = False
+
+    for line in header:
+        newline = b"\r\n" if line.endswith(b"\r\n") else b"\n"
+        fields = line.strip().split(maxsplit=1)
+        key = fields[0].upper() if fields else b""
+        if key == b"WIDTH":
+            rendered.append(f"WIDTH {point_count}".encode("ascii") + newline)
+            replaced_width = True
+        elif key == b"POINTS":
+            rendered.append(f"POINTS {point_count}".encode("ascii") + newline)
+            replaced_points = True
+        elif key == b"HEIGHT":
+            rendered.append(b"HEIGHT 1" + newline)
+        else:
+            rendered.append(line)
+
+    if not replaced_width or not replaced_points:
+        raise ValueError("PCD header must contain WIDTH and POINTS fields")
+    return b"".join(rendered)
+
+
+def measure_sample(
+    path: Path,
+    data_offset: int,
+    stride: int,
+) -> tuple[int, int, int]:
+    offset = stride // 2
+    source_count = 0
+    selected_count = 0
+    selected_bytes = 0
+
+    with path.open("rb") as stream:
+        stream.seek(data_offset)
+        for line in stream:
+            if not line.strip():
+                continue
+            if source_count % stride == offset:
+                selected_count += 1
+                selected_bytes += len(line)
+            source_count += 1
+
+    return source_count, selected_count, selected_bytes
+
+
+def write_sample(
+    source: Path,
+    output: Path,
+    header: bytes,
+    data_offset: int,
+    stride: int,
+) -> None:
+    offset = stride // 2
+    point_index = 0
+    temporary = output.with_suffix(output.suffix + ".tmp")
+    output.parent.mkdir(parents=True, exist_ok=True)
+
+    try:
+        with source.open("rb") as src, temporary.open("wb") as dst:
+            dst.write(header)
+            src.seek(data_offset)
+            for line in src:
+                if not line.strip():
+                    continue
+                if point_index % stride == offset:
+                    dst.write(line)
+                point_index += 1
+        os.replace(temporary, output)
+    finally:
+        if temporary.exists():
+            temporary.unlink()
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("source", type=Path, help="Source DATA ascii PCD file")
+    parser.add_argument("output", type=Path, help="Downsampled output PCD file")
+    parser.add_argument(
+        "--max-bytes",
+        type=int,
+        default=DEFAULT_MAX_BYTES,
+        help=f"Maximum output size in bytes (default: {DEFAULT_MAX_BYTES})",
+    )
+    return parser.parse_args()
+
+
+def main() -> int:
+    args = parse_args()
+    source = args.source.expanduser().resolve()
+    output = args.output.expanduser().resolve()
+    max_bytes = int(args.max_bytes)
+
+    if source == output:
+        raise ValueError("Source and output paths must be different")
+    if max_bytes <= 8192:
+        raise ValueError("--max-bytes must be greater than 8192")
+
+    header, data_offset, declared_points = read_header(source)
+    data_bytes = source.stat().st_size - data_offset
+    payload_budget = max_bytes - 8192
+    stride = max(1, math.ceil(data_bytes / payload_budget))
+
+    while True:
+        source_count, selected_count, selected_bytes = measure_sample(
+            source,
+            data_offset,
+            stride,
+        )
+        if source_count != declared_points:
+            raise ValueError(
+                f"POINTS declares {declared_points}, but {source_count} data rows were read"
+            )
+        output_header = render_header(header, selected_count)
+        if len(output_header) + selected_bytes <= max_bytes:
+            break
+        stride += 1
+
+    write_sample(source, output, output_header, data_offset, stride)
+    output_bytes = output.stat().st_size
+    if output_bytes > max_bytes:
+        raise RuntimeError(f"Generated file exceeds limit: {output_bytes} > {max_bytes}")
+
+    print(f"source={source}")
+    print(f"output={output}")
+    print(f"source_points={source_count}")
+    print(f"output_points={selected_count}")
+    print(f"stride={stride}")
+    print(f"output_bytes={output_bytes}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

05_software/train/rc_mjlab/tools/nav_tools/mirror_nav_xml_points.py

@@ -0,0 +1,215 @@
+#!/usr/bin/env python3
+"""Mirror a nav_tools terrain XML and route JSON together."""
+
+from __future__ import annotations
+
+import argparse
+import copy
+import json
+import math
+import time
+import xml.etree.ElementTree as ET
+from pathlib import Path
+from typing import Any
+
+
+TOOL_DIR = Path(__file__).resolve().parent
+XML_DIR = TOOL_DIR / "xml"
+POINTS_DIR = TOOL_DIR / "points"
+DEFAULT_XML = XML_DIR / "1hao.xml"
+DEFAULT_POINTS = POINTS_DIR / "points_20260715_120154.json"
+
+
+def format_float(value: float) -> str:
+    if abs(value) < 0.0000005:
+        value = 0.0
+    return f"{value:.6f}".rstrip("0").rstrip(".")
+
+
+def parse_float_tuple(text: str | None) -> tuple[float, ...]:
+    if not text:
+        return ()
+    return tuple(float(item) for item in text.split())
+
+
+def mirror_coord(value: float, axis_value: float) -> float:
+    return 2.0 * axis_value - value
+
+
+def normalize_yaw_deg(value: float) -> float:
+    while value > 180.0:
+        value -= 360.0
+    while value <= -180.0:
+        value += 360.0
+    return value
+
+
+def mirror_yaw_deg(value: float, axis: str) -> float:
+    if axis == "x":
+        return normalize_yaw_deg(180.0 - value)
+    return normalize_yaw_deg(-value)
+
+
+def mirror_quat(quat: tuple[float, ...], axis: str) -> tuple[float, float, float, float]:
+    if axis == "x":
+        return quat[0], quat[1], -quat[2], quat[3]
+    return quat[0], -quat[1], quat[2], quat[3]
+
+
+def mirror_xml(source: Path, target: Path, axis: str, axis_value: float) -> None:
+    parser = ET.XMLParser(target=ET.TreeBuilder(insert_comments=True))
+    tree = ET.parse(source, parser=parser)
+    mirrored = copy.deepcopy(tree)
+    root = mirrored.getroot()
+    model_name = root.get("model", source.stem)
+    if "mirror" not in model_name.lower():
+        root.set("model", f"{model_name} mirror")
+
+    for element in root.iter():
+        pos = parse_float_tuple(element.get("pos"))
+        if len(pos) >= 1:
+            mirrored_pos = list(pos)
+            coord_index = 0 if axis == "x" else 1
+            if len(mirrored_pos) > coord_index:
+                mirrored_pos[coord_index] = mirror_coord(mirrored_pos[coord_index], axis_value)
+            element.set("pos", " ".join(format_float(value) for value in mirrored_pos))
+
+        center = parse_float_tuple(element.get("center"))
+        if len(center) >= 1:
+            mirrored_center = list(center)
+            coord_index = 0 if axis == "x" else 1
+            if len(mirrored_center) > coord_index:
+                mirrored_center[coord_index] = mirror_coord(mirrored_center[coord_index], axis_value)
+            element.set("center", " ".join(format_float(value) for value in mirrored_center))
+
+        quat = parse_float_tuple(element.get("quat"))
+        if len(quat) == 4:
+            mirrored_quat = mirror_quat(quat, axis)
+            element.set("quat", " ".join(format_float(value) for value in mirrored_quat))
+
+    ET.indent(mirrored, space="  ")
+    target.parent.mkdir(parents=True, exist_ok=True)
+    mirrored.write(target, encoding="utf-8", xml_declaration=False)
+
+
+def mirror_point_fields(row: dict[str, Any], axis: str, axis_value: float) -> None:
+    coord_keys = ("x", "world_x") if axis == "x" else ("y", "world_y")
+    for key in coord_keys:
+        if key in row and isinstance(row[key], (int, float)):
+            row[key] = mirror_coord(float(row[key]), axis_value)
+    if "yawDeg" in row and isinstance(row["yawDeg"], (int, float)):
+        row["yawDeg"] = mirror_yaw_deg(float(row["yawDeg"]), axis)
+    if "yaw" in row and isinstance(row["yaw"], (int, float)):
+        row["yaw"] = mirror_yaw_deg(float(row["yaw"]), axis)
+
+
+def mirror_polygon(points: Any, axis: str, axis_value: float) -> None:
+    if not isinstance(points, list):
+        return
+    for point in points:
+        if isinstance(point, dict):
+            key = "x" if axis == "x" else "y"
+            if isinstance(point.get(key), (int, float)):
+                point[key] = mirror_coord(float(point[key]), axis_value)
+        elif isinstance(point, list):
+            coord_index = 0 if axis == "x" else 1
+            if len(point) > coord_index and isinstance(point[coord_index], (int, float)):
+                point[coord_index] = mirror_coord(float(point[coord_index]), axis_value)
+
+
+def mirror_regions(payload: dict[str, Any], axis: str, axis_value: float) -> None:
+    for key in ("regions", "avoid_regions"):
+        rows = payload.get(key)
+        if not isinstance(rows, list):
+            continue
+        for region in rows:
+            if isinstance(region, dict):
+                mirror_polygon(region.get("polygon", region.get("points")), axis, axis_value)
+
+
+def mirror_waypoint_rows(rows: Any, axis: str, axis_value: float) -> None:
+    if not isinstance(rows, list):
+        return
+    for row in rows:
+        if isinstance(row, dict):
+            mirror_point_fields(row, axis, axis_value)
+
+
+def mirror_points_json(source: Path, target: Path, axis: str, axis_value: float, mirrored_xml: Path | None) -> None:
+    payload = json.loads(source.read_text(encoding="utf-8"))
+    if not isinstance(payload, dict):
+        raise ValueError(f"Expected top-level JSON object: {source}")
+
+    payload["name"] = f"{payload.get('name', source.stem)}_mirror"
+    payload["mirroredFrom"] = source.name
+    payload["mirror"] = {
+        "axis": axis,
+        f"axis_{axis}": axis_value,
+        "createdAt": time.strftime("%Y-%m-%dT%H:%M:%S"),
+    }
+    if mirrored_xml is not None:
+        payload["xml"] = f"xml/{mirrored_xml.name}"
+
+    origin = payload.get("origin")
+    if isinstance(origin, dict):
+        coord_key = "x" if axis == "x" else "y"
+        if isinstance(origin.get(coord_key), (int, float)):
+            origin[coord_key] = mirror_coord(float(origin[coord_key]), axis_value)
+        if isinstance(origin.get("yaw_deg"), (int, float)):
+            origin["yaw_deg"] = mirror_yaw_deg(float(origin["yaw_deg"]), axis)
+            origin["yaw_rad"] = math.radians(float(origin["yaw_deg"]))
+        elif isinstance(origin.get("yaw_rad"), (int, float)):
+            origin["yaw_rad"] = math.radians(mirror_yaw_deg(math.degrees(float(origin["yaw_rad"])), axis))
+            origin["yaw_deg"] = math.degrees(float(origin["yaw_rad"]))
+
+    mirror_waypoint_rows(payload.get("waypoints"), axis, axis_value)
+    segments = payload.get("segments")
+    if isinstance(segments, list):
+        for segment in segments:
+            if isinstance(segment, dict):
+                mirror_waypoint_rows(segment.get("waypoints"), axis, axis_value)
+
+    mirror_regions(payload, axis, axis_value)
+
+    target.parent.mkdir(parents=True, exist_ok=True)
+    target.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8")
+
+
+def default_output_path(source: Path, suffix: str) -> Path:
+    return source.with_name(f"{source.stem}{suffix}{source.suffix}")
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description="Mirror a nav_tools XML and matching points JSON.")
+    parser.add_argument("--xml", type=Path, default=DEFAULT_XML, help="Input XML path.")
+    parser.add_argument("--points", type=Path, default=DEFAULT_POINTS, help="Input points JSON path.")
+    parser.add_argument("--mirror-axis", choices=("x", "y"), default="x", help="Coordinate axis to mirror.")
+    parser.add_argument("--axis-x", type=float, default=0.0, help="Mirror axis x value. Default: 0.")
+    parser.add_argument("--axis-y", type=float, default=0.0, help="Mirror axis y value.")
+    parser.add_argument("--out-xml", type=Path, help="Output XML path. Default: <input>_mirror.xml.")
+    parser.add_argument("--out-points", type=Path, help="Output JSON path. Default: <input>_mirror.json.")
+    return parser.parse_args()
+
+
+def main() -> int:
+    args = parse_args()
+    xml_path = args.xml if args.xml.is_absolute() else TOOL_DIR / args.xml
+    points_path = args.points if args.points.is_absolute() else TOOL_DIR / args.points
+    out_xml = args.out_xml if args.out_xml else default_output_path(xml_path, "_mirror")
+    out_points = args.out_points if args.out_points else default_output_path(points_path, "_mirror")
+    if not out_xml.is_absolute():
+        out_xml = TOOL_DIR / out_xml
+    if not out_points.is_absolute():
+        out_points = TOOL_DIR / out_points
+
+    axis_value = args.axis_x if args.mirror_axis == "x" else args.axis_y
+    mirror_xml(xml_path, out_xml, args.mirror_axis, axis_value)
+    mirror_points_json(points_path, out_points, args.mirror_axis, axis_value, out_xml)
+    print(f"XML: {out_xml}")
+    print(f"JSON: {out_points}")
+    print(f"axis_{args.mirror_axis}: {axis_value}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

05_software/train/rc_mjlab/tools/nav_tools/pcd/.gitkeep

@@ -0,0 +1 @@
+

05_software/train/rc_mjlab/tools/nav_tools/pcd_transform_tool.py

@@ -0,0 +1,108 @@
+#!/usr/bin/env python3
+"""Transform ASCII PCD maps by XY translation and yaw rotation.
+
+Examples:
+  python pcd_transform_tool.py pcd/1hao.pcd --dx -9.34 --dy -0.88 --yaw-deg -90 --output pcd/1hao_tf.pcd
+  python pcd_transform_tool.py pcd/1hao.pcd --yaw-deg 90 --in-place
+"""
+
+from __future__ import annotations
+
+import argparse
+import math
+import shutil
+import time
+from pathlib import Path
+
+
+TOOL_DIR = Path(__file__).resolve().parent
+
+
+def tool_relative(path: Path) -> Path:
+    if path.is_absolute():
+        raise ValueError("Use a path relative to this nav_tools folder.")
+    return TOOL_DIR / path
+
+
+def format_float(value: float) -> str:
+    if abs(value) < 5e-10:
+        value = 0.0
+    return f"{value:.8f}".rstrip("0").rstrip(".")
+
+
+def transform_xy(x: float, y: float, dx: float, dy: float, yaw_rad: float) -> tuple[float, float]:
+    tx = x + dx
+    ty = y + dy
+    c = math.cos(yaw_rad)
+    s = math.sin(yaw_rad)
+    return tx * c - ty * s, tx * s + ty * c
+
+
+def transform_ascii_pcd(source: Path, target: Path, dx: float, dy: float, yaw_deg: float) -> int:
+    yaw_rad = math.radians(yaw_deg)
+    data_started = False
+    transformed = 0
+    with source.open("r", encoding="utf-8", errors="ignore") as fin, target.open("w", encoding="utf-8", newline="\n") as fout:
+        for raw in fin:
+            line = raw.strip()
+            if data_started and line:
+                parts = line.split()
+                if len(parts) >= 2:
+                    x, y = transform_xy(float(parts[0]), float(parts[1]), dx, dy, yaw_rad)
+                    parts[0] = format_float(x)
+                    parts[1] = format_float(y)
+                    fout.write(" ".join(parts) + "\n")
+                    transformed += 1
+                else:
+                    fout.write(raw)
+            else:
+                fout.write(raw)
+                if line.upper().startswith("DATA"):
+                    if "ascii" not in line.lower():
+                        raise RuntimeError(f"Only ASCII PCD is supported: {source}")
+                    data_started = True
+    if transformed == 0:
+        raise RuntimeError(f"No points transformed: {source}")
+    return transformed
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description="Translate and yaw-rotate an ASCII PCD file.")
+    parser.add_argument("pcd", type=Path, help="Input PCD path relative to this nav_tools folder.")
+    parser.add_argument("--dx", type=float, default=0.0, help="X translation before rotation, meters.")
+    parser.add_argument("--dy", type=float, default=0.0, help="Y translation before rotation, meters.")
+    parser.add_argument("--yaw-deg", type=float, default=0.0, help="Yaw rotation after translation, degrees.")
+    parser.add_argument("--output", type=Path, help="Output PCD path relative to this nav_tools folder.")
+    parser.add_argument("--in-place", action="store_true", help="Overwrite input PCD after creating a .bak copy.")
+    return parser.parse_args()
+
+
+def main() -> int:
+    args = parse_args()
+    source = tool_relative(args.pcd)
+    if not source.exists():
+        raise FileNotFoundError(source)
+
+    if args.in_place:
+        target = source.with_suffix(source.suffix + ".tmp")
+    elif args.output:
+        target = tool_relative(args.output)
+    else:
+        target = source.with_name(f"{source.stem}_tf{source.suffix}")
+
+    target.parent.mkdir(parents=True, exist_ok=True)
+    count = transform_ascii_pcd(source, target, args.dx, args.dy, args.yaw_deg)
+
+    if args.in_place:
+        backup = source.with_suffix(source.suffix + f".bak_{time.strftime('%Y%m%d_%H%M%S')}")
+        shutil.copy2(source, backup)
+        target.replace(source)
+        print(f"Transformed {count} points in-place: {source}")
+        print(f"Backup: {backup}")
+    else:
+        print(f"Transformed {count} points: {target}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

05_software/train/rc_mjlab/tools/nav_tools/points/.gitkeep

@@ -0,0 +1 @@
+

05_software/train/rc_mjlab/tools/nav_tools/points/README.md

@@ -0,0 +1,24 @@
+# 航点路线演进
+
+本目录按原始文件名保存导航打点阶段的路线快照。文件名中的 `new`、`real`、`reall` 等是开发期原名，仅表示当时的迭代语境，不统一解释为正式版本或最终验证状态。
+
+| 源文件时间 | 文件 | 航点 | 避障区 | 绑定 XML |
+| --- | --- | ---: | ---: | --- |
+| 2026-07-10 23:15 | `1B_FF.json` | 48 | 26 | `B_C.xml` |
+| 2026-07-11 01:35 | `A_FFF.json` | 46 | 26 | `A_C.xml` |
+| 2026-07-12 23:55 | `1hao_wallspeed08_桥A适用于重定位.json` | 49 | 19 | `1hao.xml` |
+| 2026-07-13 00:10 | `1hao_not_raogan.json` | 32 | 19 | `1hao.xml` |
+| 2026-07-13 20:36 | `1hao_n.json` | 48 | 19 | `1hao.xml` |
+| 2026-07-14 09:57 | `1hao_new.json` | 47 | 19 | `1hao.xml` |
+| 2026-07-14 11:20 | `1hao_bridge_a_crawl.json` | 47 | 19 | `1hao.xml` |
+| 2026-07-14 12:01 | `1hao_crawl2.json` | 47 | 19 | `1hao.xml` |
+| 2026-07-14 12:16 | `1hao_new1.json` | 47 | 19 | `A_C.xml` |
+| 2026-07-14 12:19 | `1hao_jianyi.json` | 47 | 19 | `1hao.xml` |
+| 2026-07-15 11:42 | `1hao_real.json` | 30 | 19 | `1hao.xml` |
+| 2026-07-15 11:43 | `2hao_not_raogan.json` | 29 | 21 | `2hao.xml` |
+| 2026-07-15 12:01 | `1hao_reall.json` | 29 | 19 | `1hao.xml` |
+| 2026-07-15 12:01 | `points_20260715_120154.json` | 29 | 19 | `1hao.xml` |
+
+这些文件各有参数和路线差异，全部保留以呈现打点演进。默认工具入口使用时间戳文件 `points_20260715_120154.json`；它不是对其他文件的替代，其他路线仍可通过 `--points` 或界面下拉框载入。
+
+默认路线在当前保守圆形包络检查下存在 3 组净空不足记录，因此这里称为“默认归档路线”，不额外标记为几何安全最终版。

05_software/train/rc_mjlab/tools/nav_tools/regions/.gitkeep

@@ -0,0 +1 @@
+

05_software/train/rc_mjlab/tools/nav_tools/requirements.txt

@@ -0,0 +1,2 @@
+pygame>=2.6.1
+Pillow>=10.0.0

05_software/train/rc_mjlab/tools/nav_tools/route_candidate_optimizer.py

@@ -0,0 +1,558 @@
+#!/usr/bin/env python3
+"""Generate safer waypoint candidates for nav_tools route JSON files.
+
+The optimizer keeps the original JSON intact, adjusts copied waypoints away
+from avoid polygons, recomputes yaw, and writes candidate JSON files for
+offline/sim2sim validation.
+"""
+
+from __future__ import annotations
+
+import argparse
+import copy
+import json
+import math
+import time
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any
+
+from route_safety_check import (
+    AvoidRegion,
+    Waypoint,
+    analyze_route,
+    default_lateral_footprint_radius,
+    load_regions,
+    load_waypoints,
+    point_in_polygon,
+    point_segment_distance,
+)
+
+
+@dataclass
+class MutablePoint:
+    id: str
+    x: float
+    y: float
+    yaw_deg: float
+    speed: float | None
+    policy: str
+    tolerance: float | None
+    row_refs: list[dict[str, Any]]
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description="Generate safer route candidates.")
+    parser.add_argument("--points", type=Path, default=Path("tools/nav_tools/points/points_20260715_120154.json"))
+    parser.add_argument("--out-dir", type=Path, default=Path("tools/nav_tools/points/auto_candidates"))
+    parser.add_argument("--avoid-margin", type=float, default=0.05)
+    parser.add_argument("--extra-margin", type=float, default=0.08)
+    parser.add_argument("--footprint-radius", type=float, default=None)
+    parser.add_argument("--iterations", type=int, default=240)
+    parser.add_argument("--step-scale", type=float, default=0.45)
+    parser.add_argument("--max-move-per-iter", type=float, default=0.08)
+    parser.add_argument("--max-total-move", type=float, default=0.55)
+    parser.add_argument("--smooth-weight", type=float, default=0.12)
+    parser.add_argument("--lock-ends", action="store_true", default=True)
+    parser.add_argument("--no-lock-ends", dest="lock_ends", action="store_false")
+    parser.add_argument("--slow-near-risk", action="store_true", default=True)
+    parser.add_argument("--no-slow-near-risk", dest="slow_near_risk", action="store_false")
+    parser.add_argument("--min-speed", type=float, default=0.22)
+    parser.add_argument("--max-segment-length", type=float, default=0.65)
+    parser.add_argument(
+        "--focus-start-index",
+        type=int,
+        default=None,
+        help="1-based first waypoint index allowed to move; points outside the focus range are locked.",
+    )
+    parser.add_argument(
+        "--focus-end-index",
+        type=int,
+        default=None,
+        help="1-based last waypoint index allowed to move; points outside the focus range are locked.",
+    )
+    parser.add_argument(
+        "--policy-range",
+        action="append",
+        default=[],
+        metavar="START:END:POLICY",
+        help="Override waypoint policy by 1-based inclusive index range, e.g. 21:26:crawl. Can be repeated.",
+    )
+    parser.add_argument(
+        "--speed-range",
+        action="append",
+        default=[],
+        metavar="START:END:SPEED",
+        help="Override waypoint speed by 1-based inclusive index range, e.g. 21:26:0.22. Can be repeated.",
+    )
+    parser.add_argument(
+        "--shift-range",
+        action="append",
+        default=[],
+        metavar="START:END:DX:DY",
+        help="Shift waypoint positions by 1-based inclusive index range. Applied after optimization.",
+    )
+    parser.add_argument("--top", type=int, default=12)
+    return parser.parse_args()
+
+
+def load_payload(path: Path) -> dict[str, Any]:
+    with path.open("r", encoding="utf-8") as f:
+        payload = json.load(f)
+    if not isinstance(payload, dict):
+        raise ValueError(f"JSON root must be object: {path}")
+    return payload
+
+
+def iter_waypoint_rows(payload: dict[str, Any]) -> list[dict[str, Any]]:
+    rows: list[dict[str, Any]] = []
+    if isinstance(payload.get("waypoints"), list):
+        rows.extend(row for row in payload["waypoints"] if isinstance(row, dict))
+    if isinstance(payload.get("segments"), list):
+        for segment in payload["segments"]:
+            if isinstance(segment, dict) and isinstance(segment.get("waypoints"), list):
+                rows.extend(row for row in segment["waypoints"] if isinstance(row, dict))
+    return rows
+
+
+def load_mutable_points(payload: dict[str, Any]) -> list[MutablePoint]:
+    unique: list[MutablePoint] = []
+    refs_by_key: dict[tuple[str, float, float], MutablePoint] = {}
+    for index, row in enumerate(iter_waypoint_rows(payload), start=1):
+        x = float(row.get("world_x", row.get("x", 0.0)))
+        y = float(row.get("world_y", row.get("y", 0.0)))
+        key = (str(row.get("id", index)), round(x, 9), round(y, 9))
+        point = refs_by_key.get(key)
+        if point is None:
+            point = MutablePoint(
+                id=str(row.get("id", index)),
+                x=x,
+                y=y,
+                yaw_deg=float(row.get("yawDeg", row.get("yaw_deg", row.get("yaw", 0.0)))),
+                speed=float(row["speed"]) if row.get("speed") is not None else None,
+                policy=str(row.get("policy", "rough")),
+                tolerance=float(row["tolerance"]) if row.get("tolerance") is not None else None,
+                row_refs=[],
+            )
+            refs_by_key[key] = point
+            unique.append(point)
+        point.row_refs.append(row)
+    if len(unique) < 2:
+        raise ValueError("Route must contain at least two unique waypoints")
+    return unique
+
+
+def mutable_to_waypoints(points: list[MutablePoint]) -> list[Waypoint]:
+    return [
+        Waypoint(
+            index=i,
+            id=p.id,
+            x=p.x,
+            y=p.y,
+            yaw_deg=p.yaw_deg,
+            speed=p.speed,
+            policy=p.policy,
+            tolerance=p.tolerance,
+        )
+        for i, p in enumerate(points, start=1)
+    ]
+
+
+def closest_point_on_segment(px: float, py: float, ax: float, ay: float, bx: float, by: float) -> tuple[float, float]:
+    dx = bx - ax
+    dy = by - ay
+    length_sq = dx * dx + dy * dy
+    if length_sq <= 1.0e-12:
+        return ax, ay
+    t = ((px - ax) * dx + (py - ay) * dy) / length_sq
+    t = max(0.0, min(1.0, t))
+    return ax + t * dx, ay + t * dy
+
+
+def closest_segment_polygon_pair(
+    ax: float,
+    ay: float,
+    bx: float,
+    by: float,
+    polygon: tuple[tuple[float, float], ...],
+) -> tuple[float, float, float, float, float]:
+    best = (float("inf"), ax, ay, polygon[0][0], polygon[0][1])
+    samples = 17
+    for i in range(samples):
+        t = i / (samples - 1)
+        sx = ax + (bx - ax) * t
+        sy = ay + (by - ay) * t
+        for px, py in polygon:
+            dist = math.hypot(sx - px, sy - py)
+            if dist < best[0]:
+                best = (dist, sx, sy, px, py)
+        for (ex0, ey0), (ex1, ey1) in zip(polygon, polygon[1:] + polygon[:1]):
+            qx, qy = closest_point_on_segment(sx, sy, ex0, ey0, ex1, ey1)
+            dist = math.hypot(sx - qx, sy - qy)
+            if dist < best[0]:
+                best = (dist, sx, sy, qx, qy)
+    for sx, sy in ((ax, ay), (bx, by)):
+        for (ex0, ey0), (ex1, ey1) in zip(polygon, polygon[1:] + polygon[:1]):
+            qx, qy = closest_point_on_segment(sx, sy, ex0, ey0, ex1, ey1)
+            dist = math.hypot(sx - qx, sy - qy)
+            if dist < best[0]:
+                best = (dist, sx, sy, qx, qy)
+    return best
+
+
+def polygon_centroid(polygon: tuple[tuple[float, float], ...]) -> tuple[float, float]:
+    return (
+        sum(x for x, _ in polygon) / len(polygon),
+        sum(y for _, y in polygon) / len(polygon),
+    )
+
+
+def point_inside_any_region(x: float, y: float, regions: list[AvoidRegion]) -> bool:
+    return any(point_in_polygon(x, y, region.polygon) for region in regions)
+
+
+def nearest_region_clearance(x: float, y: float, regions: list[AvoidRegion]) -> float:
+    best = float("inf")
+    for region in regions:
+        if point_in_polygon(x, y, region.polygon):
+            return 0.0
+        polygon = region.polygon
+        for (ax, ay), (bx, by) in zip(polygon, polygon[1:] + polygon[:1]):
+            best = min(best, point_segment_distance(x, y, ax, ay, bx, by))
+    return best
+
+
+def optimize_points(
+    points: list[MutablePoint],
+    regions: list[AvoidRegion],
+    required_clearance: float,
+    args: argparse.Namespace,
+) -> tuple[list[MutablePoint], dict[str, Any]]:
+    original = [(p.x, p.y) for p in points]
+    target_clearance = required_clearance + float(args.extra_margin)
+    locked = {0, len(points) - 1} if args.lock_ends else set()
+    if args.focus_start_index is not None or args.focus_end_index is not None:
+        focus_start = max(1, int(args.focus_start_index or 1)) - 1
+        focus_end = min(len(points), int(args.focus_end_index or len(points))) - 1
+        if focus_start > focus_end:
+            raise ValueError(
+                f"Invalid focus range: start={args.focus_start_index} end={args.focus_end_index}"
+            )
+        locked.update(i for i in range(len(points)) if i < focus_start or i > focus_end)
+    risk_hit_counts = [0 for _ in points]
+
+    for _ in range(int(args.iterations)):
+        deltas = [[0.0, 0.0] for _ in points]
+        weights = [0.0 for _ in points]
+        waypoints = mutable_to_waypoints(points)
+        risks = analyze_route(waypoints, regions, target_clearance)
+        active_risks = [risk for risk in risks if risk.margin_m < 0.0]
+        if not active_risks:
+            break
+
+        id_to_index = {p.id: i for i, p in enumerate(points)}
+        for risk in active_risks:
+            ia = id_to_index.get(risk.start_id)
+            ib = id_to_index.get(risk.end_id)
+            region = next((item for item in regions if item.name == risk.region), None)
+            if ia is None or ib is None or region is None:
+                continue
+            ax, ay = points[ia].x, points[ia].y
+            bx, by = points[ib].x, points[ib].y
+            dist, sx, sy, qx, qy = closest_segment_polygon_pair(ax, ay, bx, by, region.polygon)
+            vx = sx - qx
+            vy = sy - qy
+            norm = math.hypot(vx, vy)
+            if norm < 1.0e-6 or risk.centerline_intersects:
+                cx, cy = polygon_centroid(region.polygon)
+                mx = 0.5 * (ax + bx)
+                my = 0.5 * (ay + by)
+                vx = mx - cx
+                vy = my - cy
+                norm = math.hypot(vx, vy)
+            if norm < 1.0e-6:
+                vx, vy, norm = 1.0, 0.0, 1.0
+            ux = vx / norm
+            uy = vy / norm
+            push = min(float(args.max_move_per_iter), max(0.0, target_clearance - dist) * float(args.step_scale))
+            for idx in (ia, ib):
+                if idx in locked:
+                    continue
+                deltas[idx][0] += ux * push
+                deltas[idx][1] += uy * push
+                weights[idx] += 1.0
+                risk_hit_counts[idx] += 1
+
+        if args.smooth_weight > 0.0 and len(points) > 2:
+            for i in range(1, len(points) - 1):
+                if i in locked:
+                    continue
+                avg_x = 0.5 * (points[i - 1].x + points[i + 1].x)
+                avg_y = 0.5 * (points[i - 1].y + points[i + 1].y)
+                deltas[i][0] += (avg_x - points[i].x) * float(args.smooth_weight)
+                deltas[i][1] += (avg_y - points[i].y) * float(args.smooth_weight)
+                weights[i] += 1.0
+
+        for i, point in enumerate(points):
+            if i in locked or weights[i] <= 0.0:
+                continue
+            dx = deltas[i][0] / weights[i]
+            dy = deltas[i][1] / weights[i]
+            ox, oy = original[i]
+            next_x = point.x + dx
+            next_y = point.y + dy
+            total_dx = next_x - ox
+            total_dy = next_y - oy
+            total = math.hypot(total_dx, total_dy)
+            if total > float(args.max_total_move):
+                scale = float(args.max_total_move) / total
+                next_x = ox + total_dx * scale
+                next_y = oy + total_dy * scale
+            point.x = next_x
+            point.y = next_y
+
+    for i, point in enumerate(points[:-1]):
+        nxt = points[i + 1]
+        point.yaw_deg = math.degrees(math.atan2(nxt.y - point.y, nxt.x - point.x))
+    points[-1].yaw_deg = points[-2].yaw_deg
+
+    if args.slow_near_risk:
+        for i, point in enumerate(points):
+            clearance = nearest_region_clearance(point.x, point.y, regions)
+            if clearance < target_clearance + 0.12:
+                current = point.speed if point.speed is not None else 0.35
+                point.speed = max(float(args.min_speed), min(current, 0.28))
+            if risk_hit_counts[i] > 0:
+                current = point.speed if point.speed is not None else 0.35
+                point.speed = max(float(args.min_speed), min(current, 0.25))
+
+    final_risks = analyze_route(mutable_to_waypoints(points), regions, required_clearance)
+    violations = [risk for risk in final_risks if risk.margin_m < 0.0 or risk.centerline_intersects]
+    moves = [
+        {
+            "id": point.id,
+            "dx": round(point.x - ox, 4),
+            "dy": round(point.y - oy, 4),
+            "dist": round(math.hypot(point.x - ox, point.y - oy), 4),
+            "risk_hits": risk_hit_counts[i],
+        }
+        for i, (point, (ox, oy)) in enumerate(zip(points, original))
+        if math.hypot(point.x - ox, point.y - oy) > 1.0e-4 or risk_hit_counts[i] > 0
+    ]
+    return points, {
+        "violations": len(violations),
+        "min_margin": round(final_risks[0].margin_m, 6) if final_risks else None,
+        "focus_range": [
+            args.focus_start_index,
+            args.focus_end_index,
+        ],
+        "moves": moves,
+        "top_risks": [
+            {
+                "start_id": risk.start_id,
+                "end_id": risk.end_id,
+                "region": risk.region,
+                "margin_m": round(risk.margin_m, 6),
+                "clearance_m": round(risk.clearance_m, 6),
+                "centerline_intersects": risk.centerline_intersects,
+            }
+            for risk in final_risks[: int(args.top)]
+        ],
+    }
+
+
+def apply_policy_ranges(points: list[MutablePoint], ranges: list[str]) -> list[dict[str, Any]]:
+    applied: list[dict[str, Any]] = []
+    for item in ranges:
+        parts = [part.strip() for part in str(item).split(":")]
+        if len(parts) != 3 or not parts[2]:
+            raise ValueError(f"Invalid --policy-range {item!r}; expected START:END:POLICY")
+        start = max(1, int(parts[0]))
+        end = min(len(points), int(parts[1]))
+        if start > end:
+            raise ValueError(f"Invalid --policy-range {item!r}; start is after end")
+        policy = parts[2]
+        for index in range(start - 1, end):
+            points[index].policy = policy
+        applied.append({"start": start, "end": end, "policy": policy})
+    return applied
+
+
+def apply_speed_ranges(points: list[MutablePoint], ranges: list[str]) -> list[dict[str, Any]]:
+    applied: list[dict[str, Any]] = []
+    for item in ranges:
+        parts = [part.strip() for part in str(item).split(":")]
+        if len(parts) != 3:
+            raise ValueError(f"Invalid --speed-range {item!r}; expected START:END:SPEED")
+        start = max(1, int(parts[0]))
+        end = min(len(points), int(parts[1]))
+        if start > end:
+            raise ValueError(f"Invalid --speed-range {item!r}; start is after end")
+        speed = float(parts[2])
+        for index in range(start - 1, end):
+            points[index].speed = speed
+        applied.append({"start": start, "end": end, "speed": speed})
+    return applied
+
+
+def apply_shift_ranges(points: list[MutablePoint], ranges: list[str]) -> list[dict[str, Any]]:
+    applied: list[dict[str, Any]] = []
+    for item in ranges:
+        parts = [part.strip() for part in str(item).split(":")]
+        if len(parts) != 4:
+            raise ValueError(f"Invalid --shift-range {item!r}; expected START:END:DX:DY")
+        start = max(1, int(parts[0]))
+        end = min(len(points), int(parts[1]))
+        if start > end:
+            raise ValueError(f"Invalid --shift-range {item!r}; start is after end")
+        dx = float(parts[2])
+        dy = float(parts[3])
+        for index in range(start - 1, end):
+            points[index].x += dx
+            points[index].y += dy
+        applied.append({"start": start, "end": end, "dx": dx, "dy": dy})
+    if applied:
+        for i, point in enumerate(points[:-1]):
+            nxt = points[i + 1]
+            point.yaw_deg = math.degrees(math.atan2(nxt.y - point.y, nxt.x - point.x))
+        points[-1].yaw_deg = points[-2].yaw_deg
+    return applied
+
+
+def refresh_safety_summary(
+    summary: dict[str, Any],
+    points: list[MutablePoint],
+    regions: list[AvoidRegion],
+    required_clearance: float,
+    args: argparse.Namespace,
+) -> None:
+    final_risks = analyze_route(mutable_to_waypoints(points), regions, required_clearance)
+    violations = [risk for risk in final_risks if risk.margin_m < 0.0 or risk.centerline_intersects]
+    summary["violations"] = len(violations)
+    summary["min_margin"] = round(final_risks[0].margin_m, 6) if final_risks else None
+    summary["top_risks"] = [
+        {
+            "start_id": risk.start_id,
+            "end_id": risk.end_id,
+            "region": risk.region,
+            "margin_m": round(risk.margin_m, 6),
+            "clearance_m": round(risk.clearance_m, 6),
+            "centerline_intersects": risk.centerline_intersects,
+        }
+        for risk in final_risks[: int(args.top)]
+    ]
+
+
+def apply_points_to_payload(payload: dict[str, Any], points: list[MutablePoint]) -> None:
+    for point in points:
+        for row in point.row_refs:
+            if "world_x" in row:
+                row["world_x"] = point.x
+            row["x"] = point.x
+            if "world_y" in row:
+                row["world_y"] = point.y
+            row["y"] = point.y
+            row["yawDeg"] = point.yaw_deg
+            if point.speed is not None:
+                row["speed"] = point.speed
+            if point.tolerance is not None:
+                row["tolerance"] = point.tolerance
+            if point.policy:
+                row["policy"] = point.policy
+
+
+def densify_segments(payload: dict[str, Any], max_len: float) -> None:
+    if max_len <= 0.0:
+        return
+    if not isinstance(payload.get("segments"), list):
+        return
+    for segment in payload["segments"]:
+        rows = segment.get("waypoints") if isinstance(segment, dict) else None
+        if not isinstance(rows, list) or len(rows) < 2:
+            continue
+        new_rows: list[dict[str, Any]] = []
+        for a, b in zip(rows, rows[1:]):
+            new_rows.append(a)
+            ax, ay = float(a.get("x", 0.0)), float(a.get("y", 0.0))
+            bx, by = float(b.get("x", 0.0)), float(b.get("y", 0.0))
+            dist = math.hypot(bx - ax, by - ay)
+            inserts = max(0, int(math.ceil(dist / max_len)) - 1)
+            for j in range(inserts):
+                t = (j + 1) / (inserts + 1)
+                row = copy.deepcopy(a)
+                row["id"] = f"{a.get('id')}_{j + 1}"
+                row["x"] = ax + (bx - ax) * t
+                row["y"] = ay + (by - ay) * t
+                row["yawDeg"] = math.degrees(math.atan2(by - ay, bx - ax))
+                if a.get("speed") is not None and b.get("speed") is not None:
+                    row["speed"] = min(float(a["speed"]), float(b["speed"]))
+                new_rows.append(row)
+        new_rows.append(rows[-1])
+        segment["waypoints"] = new_rows
+    if isinstance(payload.get("waypoints"), list):
+        flat: list[dict[str, Any]] = []
+        for segment in payload["segments"]:
+            rows = segment.get("waypoints") if isinstance(segment, dict) else None
+            if isinstance(rows, list):
+                flat.extend(copy.deepcopy(row) for row in rows if isinstance(row, dict))
+        if flat:
+            payload["waypoints"] = flat
+
+
+def main() -> int:
+    args = parse_args()
+    payload = load_payload(args.points)
+    regions = load_regions(payload)
+    points = load_mutable_points(payload)
+    footprint = float(args.footprint_radius) if args.footprint_radius is not None else default_lateral_footprint_radius()
+    required = footprint + float(args.avoid_margin)
+
+    candidate_payload = copy.deepcopy(payload)
+    candidate_points = load_mutable_points(candidate_payload)
+    optimized, summary = optimize_points(candidate_points, regions, required, args)
+    shift_overrides = apply_shift_ranges(optimized, list(args.shift_range))
+    policy_overrides = apply_policy_ranges(optimized, list(args.policy_range))
+    speed_overrides = apply_speed_ranges(optimized, list(args.speed_range))
+    if shift_overrides:
+        summary["shift_overrides"] = shift_overrides
+    if policy_overrides:
+        summary["policy_overrides"] = policy_overrides
+    if speed_overrides:
+        summary["speed_overrides"] = speed_overrides
+    if shift_overrides or policy_overrides or speed_overrides:
+        refresh_safety_summary(summary, optimized, regions, required, args)
+    apply_points_to_payload(candidate_payload, optimized)
+    densify_segments(candidate_payload, float(args.max_segment_length))
+
+    stamp = time.strftime("%Y%m%d_%H%M%S")
+    args.out_dir.mkdir(parents=True, exist_ok=True)
+    out_path = args.out_dir / f"{args.points.stem}_auto_{stamp}.json"
+    candidate_payload["name"] = f"{payload.get('name', args.points.stem)}_auto_{stamp}"
+    candidate_payload["autoOptimize"] = {
+        "source": str(args.points),
+        "createdAt": time.strftime("%Y-%m-%dT%H:%M:%S"),
+        "footprintRadius": footprint,
+        "avoidMargin": args.avoid_margin,
+        "extraMargin": args.extra_margin,
+        "summary": summary,
+    }
+    out_path.write_text(json.dumps(candidate_payload, ensure_ascii=False, indent=2), encoding="utf-8")
+
+    print("Route candidate optimizer")
+    print(f"  source: {args.points}")
+    print(f"  output: {out_path}")
+    print(f"  required_clearance: {required:.3f} m, target: {required + args.extra_margin:.3f} m")
+    print(f"  final_violations: {summary['violations']}")
+    print(f"  min_margin: {summary['min_margin']} m")
+    print(f"  moved_points: {len(summary['moves'])}")
+    for item in summary["top_risks"][: int(args.top)]:
+        print(
+            "  risk "
+            f"{item['start_id']}->{item['end_id']} {item['region']} "
+            f"margin={item['margin_m']:.3f} clearance={item['clearance_m']:.3f}"
+        )
+    return 0 if summary["violations"] == 0 else 2
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

05_software/train/rc_mjlab/tools/nav_tools/route_safety_check.py

@@ -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_20260715_120154.json"),
+        help="Route JSON exported by nav_map_viewer.",
+    )
+    parser.add_argument(
+        "--xml",
+        type=Path,
+        default=Path("tools/nav_tools/xml/1hao.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)

05_software/train/rc_mjlab/tools/nav_tools/run_route_experiments.py

@@ -0,0 +1,203 @@
+#!/usr/bin/env python3
+"""Run repeatable safety and sim2sim experiments for a route candidate."""
+
+from __future__ import annotations
+
+import argparse
+import json
+import subprocess
+import sys
+import time
+from pathlib import Path
+from typing import Any
+
+
+PROJECT_ROOT = Path(__file__).resolve().parents[2]
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description="Run route safety + sim2sim experiment suite.")
+    parser.add_argument("--points", type=Path, required=True)
+    parser.add_argument("--terrain-xml", type=Path, default=PROJECT_ROOT / "tools/nav_tools/xml/1hao.xml")
+    parser.add_argument("--onnx", type=Path, default=PROJECT_ROOT / "model_6800.onnx")
+    parser.add_argument("--out-dir", type=Path, default=PROJECT_ROOT / "sim2sim/route_experiments")
+    parser.add_argument("--start-yaw-offset-deg", type=float, default=-180.0)
+    parser.add_argument("--heading-offset-deg", type=float, default=180.0)
+    parser.add_argument("--start-z", type=float, default=0.75)
+    parser.add_argument("--settle-steps", type=int, default=500)
+    parser.add_argument("--follower", choices=("waypoint", "pure-pursuit"), default="pure-pursuit")
+    parser.add_argument("--lookahead", type=float, default=0.45)
+    parser.add_argument("--max-vx", type=float, default=0.22)
+    parser.add_argument("--min-cmd-vx", type=float, default=0.04)
+    parser.add_argument("--creep-cmd-vx", type=float, default=0.04)
+    parser.add_argument("--yaw-stop-threshold-deg", type=float, default=45.0)
+    parser.add_argument("--turn-in-place-enter-deg", type=float, default=70.0)
+    parser.add_argument("--cmd-vx-scale", type=float, default=1.0)
+    parser.add_argument("--no-local-safety", action="store_true")
+    parser.add_argument("--include-reverse-vx", action="store_true")
+    parser.add_argument("--quick", action="store_true", help="Use shorter timeouts for fast iteration.")
+    return parser.parse_args()
+
+
+def run_command(args: list[str], cwd: Path) -> tuple[int, str]:
+    proc = subprocess.run(
+        args,
+        cwd=str(cwd),
+        text=True,
+        stdout=subprocess.PIPE,
+        stderr=subprocess.STDOUT,
+    )
+    return proc.returncode, proc.stdout
+
+
+def newest_report(out_dir: Path) -> Path | None:
+    reports = sorted(out_dir.glob("route_check_*/report.json"), key=lambda p: p.stat().st_mtime, reverse=True)
+    return reports[0] if reports else None
+
+
+def load_report(path: Path | None) -> dict[str, Any]:
+    if path is None:
+        return {}
+    return json.loads(path.read_text(encoding="utf-8"))
+
+
+def run_sim_case(
+    name: str,
+    args: argparse.Namespace,
+    extra: list[str],
+    max_time: int,
+    waypoint_timeout: int,
+) -> dict[str, Any]:
+    case_out = args.out_dir / name
+    case_out.mkdir(parents=True, exist_ok=True)
+    cmd = [
+        sys.executable,
+        "sim2sim/nav_route_sim2sim_check.py",
+        "--terrain-xml",
+        str(args.terrain_xml),
+        "--points",
+        str(args.points),
+        "--onnx",
+        str(args.onnx),
+        "--policy-backend",
+        "policy-runner",
+        "--mission",
+        "json",
+        "--start-yaw-offset-deg",
+        str(args.start_yaw_offset_deg),
+        "--heading-offset-deg",
+        str(args.heading_offset_deg),
+        "--start-z",
+        str(args.start_z),
+        "--settle-steps",
+        str(args.settle_steps),
+        "--follower",
+        str(args.follower),
+        "--lookahead",
+        str(args.lookahead),
+        "--max-vx",
+        str(args.max_vx),
+        "--min-cmd-vx",
+        str(args.min_cmd_vx),
+        "--creep-cmd-vx",
+        str(args.creep_cmd_vx),
+        "--yaw-stop-threshold-deg",
+        str(args.yaw_stop_threshold_deg),
+        "--turn-in-place-enter-deg",
+        str(args.turn_in_place_enter_deg),
+        "--cmd-vx-scale",
+        str(args.cmd_vx_scale),
+        "--max-time",
+        str(max_time),
+        "--waypoint-timeout",
+        str(waypoint_timeout),
+        "--sample-every",
+        "10",
+        "--out-dir",
+        str(case_out),
+        *extra,
+    ]
+    if args.no_local_safety:
+        cmd.append("--no-local-safety")
+    code, output = run_command(cmd, PROJECT_ROOT)
+    report_path = newest_report(case_out)
+    report = load_report(report_path)
+    return {
+        "name": name,
+        "exit_code": code,
+        "success": bool(report.get("success", False)),
+        "reason": report.get("reason", "no report"),
+        "reached": f"{report.get('reached_count', '?')}/{report.get('waypoint_count', '?')}",
+        "min_margin": report.get("min_margin"),
+        "report": str(report_path) if report_path else None,
+        "output_tail": "\n".join(output.splitlines()[-12:]),
+    }
+
+
+def main() -> int:
+    args = parse_args()
+    args.out_dir.mkdir(parents=True, exist_ok=True)
+    suite_dir = args.out_dir / f"suite_{time.strftime('%Y%m%d_%H%M%S')}"
+    suite_dir.mkdir(parents=True, exist_ok=True)
+    args.out_dir = suite_dir
+
+    safety_cmd = [
+        sys.executable,
+        "tools/nav_tools/route_safety_check.py",
+        "--points",
+        str(args.points),
+        "--xml",
+        str(args.terrain_xml),
+        "--onnx",
+        str(args.onnx),
+        "--top",
+        "12",
+    ]
+    safety_code, safety_output = run_command(safety_cmd, PROJECT_ROOT)
+    cases = []
+    full_time = 60 if args.quick else 180
+    slice_time = 45 if args.quick else 90
+    cases.append(run_sim_case("full", args, [], full_time, 20))
+    cases.append(run_sim_case("start_1_8", args, ["--start-id", "1", "--end-id", "8"], slice_time, 20))
+    cases.append(run_sim_case("mid_13_22", args, ["--start-id", "13", "--end-id", "22"], slice_time, 20))
+    cases.append(run_sim_case("slalom_30_45", args, ["--start-id", "30", "--end-id", "45"], slice_time, 20))
+    if args.include_reverse_vx:
+        old_scale = args.cmd_vx_scale
+        args.cmd_vx_scale = -abs(old_scale)
+        cases.append(run_sim_case("reverse_start_1_8", args, ["--start-id", "1", "--end-id", "8"], slice_time, 20))
+        cases.append(run_sim_case("reverse_slalom_30_45", args, ["--start-id", "30", "--end-id", "45"], slice_time, 20))
+        args.cmd_vx_scale = old_scale
+
+    summary = {
+        "points": str(args.points),
+        "terrain_xml": str(args.terrain_xml),
+        "onnx": str(args.onnx),
+        "follower": args.follower,
+        "lookahead": args.lookahead,
+        "max_vx": args.max_vx,
+        "cmd_vx_scale": args.cmd_vx_scale,
+        "no_local_safety": args.no_local_safety,
+        "suite_dir": str(suite_dir),
+        "safety_exit_code": safety_code,
+        "safety_tail": "\n".join(safety_output.splitlines()[-18:]),
+        "cases": cases,
+    }
+    summary_path = suite_dir / "summary.json"
+    summary_path.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
+
+    print("Route experiment suite")
+    print(f"  points: {args.points}")
+    print(f"  suite: {suite_dir}")
+    print(f"  safety: {'PASS' if safety_code == 0 else 'FAIL'}")
+    for case in cases:
+        print(
+            f"  {case['name']}: success={case['success']} "
+            f"reached={case['reached']} reason={case['reason']} "
+            f"margin={case['min_margin']} report={case['report']}"
+        )
+    print(f"  summary: {summary_path}")
+    return 0 if safety_code == 0 and all(case["success"] for case in cases) else 2
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

05_software/train/rc_mjlab/tools/nav_tools/transform_pcd_xy.py

@@ -0,0 +1,117 @@
+#!/usr/bin/env python3
+"""Translate and yaw-rotate an ASCII PCD in XY.
+
+Example:
+  python transform_pcd_xy.py pcd/1hao.pcd --in-place --origin-x 9.34 --origin-y 0.88 --yaw-deg -90
+
+The transform is:
+  1. subtract origin from x/y
+  2. rotate around (0, 0) by yaw-deg
+
+Only ASCII PCD files are supported. Non-x/y fields are preserved.
+"""
+
+from __future__ import annotations
+
+import argparse
+import math
+from pathlib import Path
+
+
+TOOL_DIR = Path(__file__).resolve().parent
+
+
+def tool_relative(path: Path) -> Path:
+    if path.is_absolute():
+        raise ValueError("Use a path relative to this nav_tools folder.")
+    return TOOL_DIR / path
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description="Translate and yaw-rotate an ASCII PCD in XY.")
+    parser.add_argument("input", type=Path, help="Input .pcd file.")
+    parser.add_argument("--output", type=Path, help="Output .pcd file. Required unless --in-place is used.")
+    parser.add_argument("--in-place", action="store_true", help="Replace the input file after a successful transform.")
+    parser.add_argument("--origin-x", type=float, default=0.0, help="X value to subtract before rotation.")
+    parser.add_argument("--origin-y", type=float, default=0.0, help="Y value to subtract before rotation.")
+    parser.add_argument("--yaw-deg", type=float, default=0.0, help="Yaw rotation in degrees after origin subtraction.")
+    parser.add_argument("--precision", type=int, default=8, help="Decimal precision for transformed x/y.")
+    return parser.parse_args()
+
+
+def fmt(value: float, precision: int) -> str:
+    if abs(value) < 0.5 * 10 ** (-precision):
+        value = 0.0
+    return f"{value:.{precision}f}".rstrip("0").rstrip(".")
+
+
+def transform_file(
+    source: Path,
+    target: Path,
+    origin_x: float,
+    origin_y: float,
+    yaw_deg: float,
+    precision: int,
+) -> int:
+    yaw = math.radians(yaw_deg)
+    cos_yaw = math.cos(yaw)
+    sin_yaw = math.sin(yaw)
+    data_started = False
+    fields: list[str] = []
+    x_idx = 0
+    y_idx = 1
+    count = 0
+
+    with source.open("r", encoding="utf-8", errors="ignore") as fin, target.open("w", encoding="utf-8", newline="\n") as fout:
+        for line in fin:
+            stripped = line.strip()
+            upper = stripped.upper()
+            if data_started and stripped:
+                parts = stripped.split()
+                if len(parts) > max(x_idx, y_idx):
+                    x0 = float(parts[x_idx]) - origin_x
+                    y0 = float(parts[y_idx]) - origin_y
+                    x1 = x0 * cos_yaw - y0 * sin_yaw
+                    y1 = x0 * sin_yaw + y0 * cos_yaw
+                    parts[x_idx] = fmt(x1, precision)
+                    parts[y_idx] = fmt(y1, precision)
+                    fout.write(" ".join(parts) + "\n")
+                    count += 1
+                else:
+                    fout.write(line)
+            else:
+                fout.write(line)
+                if upper.startswith("FIELDS "):
+                    fields = stripped.split()[1:]
+                    if "x" in fields and "y" in fields:
+                        x_idx = fields.index("x")
+                        y_idx = fields.index("y")
+                elif upper.startswith("DATA "):
+                    if "ASCII" not in upper:
+                        raise RuntimeError(f"Only ASCII PCD is supported: {source}")
+                    data_started = True
+    return count
+
+
+def main() -> int:
+    args = parse_args()
+    source = tool_relative(args.input)
+    if args.in_place:
+        target = source.with_suffix(source.suffix + ".tmp")
+    elif args.output:
+        target = tool_relative(args.output)
+    else:
+        raise SystemExit("--output is required unless --in-place is used")
+
+    count = transform_file(source, target, args.origin_x, args.origin_y, args.yaw_deg, args.precision)
+    if count <= 0:
+        target.unlink(missing_ok=True)
+        raise SystemExit("No PCD points transformed")
+    if args.in_place:
+        target.replace(source)
+    print(f"transformed {count} points: origin=({args.origin_x}, {args.origin_y}), yaw={args.yaw_deg} deg")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

05_software/train/rc_mjlab/tools/nav_tools/xml/1hao.xml

@@ -0,0 +1,72 @@
+<mujoco model="nav_tools terrain A_C">
+  <statistic center="-5.5 1.9 0.4" extent="5.0" />
+  <visual>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
+    <rgba haze="0.15 0.25 0.35 1" />
+    <global azimuth="90" elevation="-20" />
+  </visual>
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
+  </asset>
+  <worldbody>
+    <!-- 比赛地图基准地面。 -->
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
+    <!-- 14m x 6m outer boundary transformed from A_bian: x_C=y_bian, y_C=-x_bian. -->
+    <geom name="border_x_min" pos="-9.5 1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
+    <geom name="border_x_max" pos="4.5 1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
+    <geom name="border_y_min" pos="-2.5 -1.1 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
+    <geom name="border_y_max" pos="-2.5 4.9 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
+    <!-- 高墙：机器人需从上方跃过或攀爬通过，自动/遥控均可计分。 -->
+    <geom pos="-3.620583 -0.010049 0.15" type="box" size="0.025 0.5 0.15" quat="1 0 0 0" rgba="1.0 0.9 0.4 1.0" />
+    <!-- 砂砾碎木坑：L 形障碍，需从 1m 短边进入或离开，完整通过才计分。 -->
+    <geom pos="-8.470246 2.97224 0.075" type="box" size="0.5 0.5 0.075" quat="0.707106781 0 0 0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-7.970246 3.97224 0.075" type="box" size="0.5 1.0 0.075" quat="0.707106781 0 0 0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <!-- 限高杆：机器人需从横杆下方通过，碰落横杆则越障失败。 -->
+    <geom pos="-5.219014 4.406386 0.155" type="cylinder" size="0.025 0.155" quat="0.707106781 0 0 -0.707106781" rgba="0.8 0.1 0.1 1.0" />
+    <geom pos="-5.219014 3.406386 0.155" type="cylinder" size="0.025 0.155" quat="0.707106781 0 0 -0.707106781" rgba="0.8 0.1 0.1 1.0" />
+    <geom pos="-5.219014 3.906386 0.325" type="cylinder" size="0.015 0.5" quat="0.5 -0.5 -0.5 -0.5" rgba="1.0 0.9 0.4 1.0" />
+    <!-- 启动区：规则允许地面和 T 字形台阶上各一个启动区，机器人需完全纳入启动区后开始。 -->
+    <geom pos="-5.5 1.9 0" type="box" size="0.5 0.5 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <!-- T 字形台阶：可选启动区之一在最高平台；通过时每一级台阶顶面需至少接触一次。 -->
+    <geom pos="-1.325629 -0.037815 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-1.025629 -0.037815 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.725629 -0.037815 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.075629 -0.037815 0.35" type="box" size="0.5 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0.574371 -0.037815 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0.874371 -0.037815 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.174371 -0.037815 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.075629 0.612185 0.25" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.075629 0.912185 0.15" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.075629 1.212185 0.05" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <!-- 大斜坡、木桥 A / 木桥 B：大斜坡需满足长边行走距离要求；木桥需从一侧平台经木桥到达另一侧平台。 -->
+    <geom pos="2.540216 0.040352 0.1" type="box" size="0.40 0.5 0.005" quat="0.992546088 0.121869866 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.420216 0.040352 0.1" type="box" size="0.5 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.420216 0.890352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.420216 1.440352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.420216 1.990352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.420216 2.540352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.420216 3.090352 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="2.920216 3.940352 0.1" type="box" size="0.5 1.0 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="2.420216 3.040552 0.0951" type="box" size="0.4133 0.5 0.005" quat="0.701836142 0.086174413 0.086174413 -0.701836142" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.170216 3.590352 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.170216 3.890352 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.170216 4.190352 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.079784 3.940352 0.1" type="box" size="0.5 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-1.579784 2.874152 0.0951" type="box" size="0.5759 2.0 0.005" quat="0.704416012 0.061628579 0.061628579 -0.704416012" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-2.079784 4.008452 0.0951" type="box" size="0.5759 1.5 0.005" quat="0.704416012 -0.061628579 -0.061628579 -0.704416012" rgba="0.75 0.72 0.55 1.0" />
+    <!-- 直角绕杆：需按 S 形绕过竖杆，并经过两端及拐角必达区。 -->
+    <geom pos="-6.121128 0.005994 0" type="cylinder" size="0.15 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <geom pos="-8.521128 1.405994 0" type="cylinder" size="0.15 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <geom pos="-8.771128 -0.244006 0" type="cylinder" size="0.15 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <geom name="slalom_pole_1_base" pos="-6.521128 0.005994 0.02" type="cylinder" size="0.115 0.02" rgba="0.75 0.72 0.55 1.0" />
+    <geom name="slalom_pole_1_shaft" pos="-6.521128 0.005994 0.37" type="cylinder" size="0.115 0.33" rgba="0.75 0.72 0.55 1.0" />
+    <geom name="slalom_pole_2_base" pos="-7.521128 0.005994 0.02" type="cylinder" size="0.115 0.02" rgba="0.75 0.72 0.55 1.0" />
+    <geom name="slalom_pole_2_shaft" pos="-7.521128 0.005994 0.37" type="cylinder" size="0.115 0.33" rgba="0.75 0.72 0.55 1.0" />
+    <geom name="slalom_pole_3_base" pos="-8.521128 0.005994 0.02" type="cylinder" size="0.115 0.02" rgba="0.75 0.72 0.55 1.0" />
+    <geom name="slalom_pole_3_shaft" pos="-8.521128 0.005994 0.37" type="cylinder" size="0.115 0.33" rgba="0.75 0.72 0.55 1.0" />
+    <geom name="slalom_pole_4_base" pos="-8.521128 1.005994 0.02" type="cylinder" size="0.115 0.02" rgba="0.75 0.72 0.55 1.0" />
+    <geom name="slalom_pole_4_shaft" pos="-8.521128 1.005994 0.37" type="cylinder" size="0.115 0.33" rgba="0.75 0.72 0.55 1.0" />
+  </worldbody>
+</mujoco>

05_software/train/rc_mjlab/tools/nav_tools/xml/2hao.xml

@@ -0,0 +1,72 @@
+<mujoco model="nav_tools terrain B_C">
+  <statistic center="-5.5 -1.9 0.4" extent="5.0" />
+  <visual>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
+    <rgba haze="0.15 0.25 0.35 1" />
+    <global azimuth="90" elevation="-20" />
+  </visual>
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
+  </asset>
+  <worldbody>
+    <!-- 比赛地图基准地面。 -->
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
+    <!-- 14m x 6m outer boundary, mirrored from A_C across the x axis. -->
+    <geom name="border_x_min" pos="-9.5 -1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
+    <geom name="border_x_max" pos="4.401477 -1.937395 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
+    <geom name="border_y_min" pos="-2.5 -4.9 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
+    <geom name="border_y_max" pos="-2.5 1.1 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
+    <!-- 高墙：机器人需从上方跃过或攀爬通过，自动/遥控均可计分。 -->
+    <geom pos="-3.589248 0.022509 0.15" type="box" size="0.025 0.5 0.15" quat="1 0 0 0" rgba="1.0 0.9 0.4 1.0" />
+    <!-- 砂砾碎木坑：L 形障碍，需从 1m 短边进入或离开，完整通过才计分。 -->
+    <geom pos="-8.430146 -2.855226 0.075" type="box" size="0.5 0.5 0.075" quat="0.707107 0 0 0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-7.930146 -3.855226 0.075" type="box" size="0.5 1.0 0.075" quat="0.707107 0 0 0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <!-- 限高杆：机器人需从横杆下方通过，碰落横杆则越障失败。 -->
+    <geom pos="-5.231818 -4.372727 0.155" type="cylinder" size="0.025 0.155" quat="0.707107 0 0 -0.707107" rgba="0.8 0.1 0.1 1.0" />
+    <geom pos="-5.231818 -3.372727 0.155" type="cylinder" size="0.025 0.155" quat="0.707107 0 0 -0.707107" rgba="0.8 0.1 0.1 1.0" />
+    <geom pos="-5.231818 -3.872727 0.325" type="cylinder" size="0.015 0.5" quat="0.5 0.5 -0.5 -0.5" rgba="1.0 0.9 0.4 1.0" />
+    <!-- 启动区：规则允许地面和 T 字形台阶上各一个启动区，机器人需完全纳入启动区后开始。 -->
+    <geom pos="-5.5 -1.9 0" type="box" size="0.5 0.5 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <!-- T 字形台阶：可选启动区之一在最高平台；通过时每一级台阶顶面需至少接触一次。 -->
+    <geom pos="-1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 0 0.35" type="box" size="0.5 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 -0.65 0.25" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 -0.95 0.15" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 -1.25 0.05" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <!-- 大斜坡、木桥 A / 木桥 B：大斜坡需满足长边行走距离要求；木桥需从一侧平台经木桥到达另一侧平台。 -->
+    <geom pos="2.521477 -0.037395 0.1" type="box" size="0.40 0.5 0.005" quat="0.992546 -0.12187 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.401477 -0.037395 0.1" type="box" size="0.5 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.401477 -0.887395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.401477 -1.437395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.401477 -1.987395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.401477 -2.537395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.401477 -3.087395 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="2.901477 -3.937395 0.1" type="box" size="0.5 1.0 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="2.401477 -3.037595 0.0951" type="box" size="0.4133 0.5 0.005" quat="0.701836 -0.086174 0.086174 -0.701836" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.151477 -3.587395 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.151477 -3.887395 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.151477 -4.187395 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.098523 -3.937395 0.1" type="box" size="0.5 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-1.598523 -2.871195 0.0951" type="box" size="0.5759 2.0 0.005" quat="0.704416 -0.061629 0.061629 -0.704416" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-2.098523 -4.005495 0.0951" type="box" size="0.5759 1.5 0.005" quat="0.704416 0.061629 -0.061629 -0.704416" rgba="0.75 0.72 0.55 1.0" />
+    <!-- 直角绕杆：需按 S 形绕过竖杆，并经过两端及拐角必达区。 -->
+    <geom pos="-6.075243 0.034483 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <geom pos="-8.475243 -1.365517 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <geom pos="-8.725243 0.284483 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <geom pos="-6.475243 0.034483 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-6.475243 0.034483 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-7.475243 0.034483 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-7.475243 0.034483 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-8.475243 0.034483 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-8.475243 0.034483 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-8.475243 -0.965517 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-8.475243 -0.965517 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+  </worldbody>
+</mujoco>

05_software/train/rc_mjlab/tools/nav_tools/xml/A_C.xml

@@ -0,0 +1,67 @@
+<mujoco model="nav_tools terrain A_C">
+  <statistic center="-5.5 1.9 0.4" extent="5.0" />
+  <visual>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
+    <rgba haze="0.15 0.25 0.35 1" />
+    <global azimuth="90" elevation="-20" />
+  </visual>
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
+  </asset>
+  <worldbody>
+    <!-- 比赛地图基准地面。 -->
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
+    <!-- 高墙：机器人需从上方跃过或攀爬通过，自动/遥控均可计分。 -->
+    <geom pos="-3.5 0 0.15" type="box" size="0.025 0.5 0.15" quat="1 0 0 0" rgba="1.0 0.9 0.4 1.0" />
+    <!-- 砂砾碎木坑：L 形障碍，需从 1m 短边进入或离开，完整通过才计分。 -->
+    <geom pos="-8.4 3.0361 0.075" type="box" size="0.5 0.5 0.075" quat="0.707106781 0 0 0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-7.9 4.0361 0.075" type="box" size="0.5 1.0 0.075" quat="0.707106781 0 0 0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <!-- 限高杆：机器人需从横杆下方通过，碰落横杆则越障失败。 -->
+    <geom pos="-5.5 4.4 0.155" type="cylinder" size="0.025 0.155" quat="0.707106781 0 0 -0.707106781" rgba="0.8 0.1 0.1 1.0" />
+    <geom pos="-5.5 3.4 0.155" type="cylinder" size="0.025 0.155" quat="0.707106781 0 0 -0.707106781" rgba="0.8 0.1 0.1 1.0" />
+    <geom pos="-5.5 3.9 0.325" type="cylinder" size="0.015 0.5" quat="0.5 -0.5 -0.5 -0.5" rgba="1.0 0.9 0.4 1.0" />
+    <!-- 启动区：规则允许地面和 T 字形台阶上各一个启动区，机器人需完全纳入启动区后开始。 -->
+    <geom pos="-5.5 1.9 0" type="box" size="0.5 0.5 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <!-- T 字形台阶：可选启动区之一在最高平台；通过时每一级台阶顶面需至少接触一次。 -->
+    <geom pos="-1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 0 0.35" type="box" size="0.5 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 0.65 0.25" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 0.95 0.15" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 1.25 0.05" type="box" size="0.15 0.5 0.05" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <!-- 大斜坡、木桥 A / 木桥 B：大斜坡需满足长边行走距离要求；木桥需从一侧平台经木桥到达另一侧平台。 -->
+    <geom pos="2.62 0 0.1" type="box" size="0.40 0.5 0.005" quat="0.992546088 0.121869866 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.5 0 0.1" type="box" size="0.5 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.5 0.85 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.5 1.4 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.5 1.95 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.5 2.5 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.5 3.05 0.1" type="box" size="0.2 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3 3.9 0.1" type="box" size="0.5 1.0 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="2.5 3.0002 0.0951" type="box" size="0.4133 0.5 0.005" quat="0.701836142 0.086174413 0.086174413 -0.701836142" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.25 3.55 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.25 3.85 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.25 4.15 0.1" type="box" size="0.1 0.75 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 3.9 0.1" type="box" size="0.5 0.5 0.10" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-1.5 2.8338 0.0951" type="box" size="0.5759 2.0 0.005" quat="0.704416012 0.061628579 0.061628579 -0.704416012" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-2 3.9681 0.0951" type="box" size="0.5759 1.5 0.005" quat="0.704416012 -0.061628579 -0.061628579 -0.704416012" rgba="0.75 0.72 0.55 1.0" />
+    <!-- 直角绕杆：需按 S 形绕过竖杆，并经过两端及拐角必达区。 -->
+    <geom pos="-6.025 0.0861 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <geom pos="-8.425 1.4861 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <geom pos="-8.675 -0.1639 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <geom pos="-6.425 0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-6.425 0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-7.425 0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-7.425 0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-8.425 0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-8.425 0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-8.425 1.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-8.425 1.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707106781 0 0 -0.707106781" rgba="0.75 0.72 0.55 1.0" />
+  </worldbody>
+</mujoco>

05_software/train/rc_mjlab/tools/nav_tools/xml/B_C.xml

@@ -0,0 +1,72 @@
+<mujoco model="nav_tools terrain B_C">
+  <statistic center="-5.5 -1.9 0.4" extent="5.0" />
+  <visual>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
+    <rgba haze="0.15 0.25 0.35 1" />
+    <global azimuth="90" elevation="-20" />
+  </visual>
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
+  </asset>
+  <worldbody>
+    <!-- 比赛地图基准地面。 -->
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
+    <!-- 14m x 6m outer boundary, mirrored from A_C across the x axis. -->
+    <geom name="border_x_min" pos="-9.5 -1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
+    <geom name="border_x_max" pos="4.5 -1.9 0.15" type="box" size="0.025 3.0 0.15" rgba="0.05 0.05 0.05 1" />
+    <geom name="border_y_min" pos="-2.5 -4.9 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
+    <geom name="border_y_max" pos="-2.5 1.1 0.15" type="box" size="7.0 0.025 0.15" rgba="0.05 0.05 0.05 1" />
+    <!-- 高墙：机器人需从上方跃过或攀爬通过，自动/遥控均可计分。 -->
+    <geom pos="-3.5 0 0.15" type="box" size="0.025 0.5 0.15" quat="1 0 0 0" rgba="1.0 0.9 0.4 1.0" />
+    <!-- 砂砾碎木坑：L 形障碍，需从 1m 短边进入或离开，完整通过才计分。 -->
+    <geom pos="-8.4 -3.0361 0.075" type="box" size="0.5 0.5 0.075" quat="0.707107 0 0 0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-7.9 -4.0361 0.075" type="box" size="0.5 1.0 0.075" quat="0.707107 0 0 0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <!-- 限高杆：机器人需从横杆下方通过，碰落横杆则越障失败。 -->
+    <geom pos="-5.5 -4.4 0.155" type="cylinder" size="0.025 0.155" quat="0.707107 0 0 -0.707107" rgba="0.8 0.1 0.1 1.0" />
+    <geom pos="-5.5 -3.4 0.155" type="cylinder" size="0.025 0.155" quat="0.707107 0 0 -0.707107" rgba="0.8 0.1 0.1 1.0" />
+    <geom pos="-5.5 -3.9 0.325" type="cylinder" size="0.015 0.5" quat="0.5 0.5 -0.5 -0.5" rgba="1.0 0.9 0.4 1.0" />
+    <!-- 启动区：规则允许地面和 T 字形台阶上各一个启动区，机器人需完全纳入启动区后开始。 -->
+    <geom pos="-5.5 -1.9 0" type="box" size="0.5 0.5 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <!-- T 字形台阶：可选启动区之一在最高平台；通过时每一级台阶顶面需至少接触一次。 -->
+    <geom pos="-1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 0 0.35" type="box" size="0.5 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0.65 0 0.25" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0.95 0 0.15" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.25 0 0.05" type="box" size="0.15 0.5 0.05" quat="1 0 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 -0.65 0.25" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 -0.95 0.15" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 -1.25 0.05" type="box" size="0.15 0.5 0.05" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <!-- 大斜坡、木桥 A / 木桥 B：大斜坡需满足长边行走距离要求；木桥需从一侧平台经木桥到达另一侧平台。 -->
+    <geom pos="2.62 0 0.1" type="box" size="0.40 0.5 0.005" quat="0.992546 -0.12187 0 0" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.5 0 0.1" type="box" size="0.5 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.5 -0.85 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.5 -1.4 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.5 -1.95 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.5 -2.5 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3.5 -3.05 0.1" type="box" size="0.2 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="3 -3.9 0.1" type="box" size="0.5 1.0 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="2.5 -3.0002 0.0951" type="box" size="0.4133 0.5 0.005" quat="0.701836 -0.086174 0.086174 -0.701836" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.25 -3.55 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.25 -3.85 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="1.25 -4.15 0.1" type="box" size="0.1 0.75 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="0 -3.9 0.1" type="box" size="0.5 0.5 0.10" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-1.5 -2.8338 0.0951" type="box" size="0.5759 2.0 0.005" quat="0.704416 -0.061629 0.061629 -0.704416" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-2 -3.9681 0.0951" type="box" size="0.5759 1.5 0.005" quat="0.704416 0.061629 -0.061629 -0.704416" rgba="0.75 0.72 0.55 1.0" />
+    <!-- 直角绕杆：需按 S 形绕过竖杆，并经过两端及拐角必达区。 -->
+    <geom pos="-6.025 -0.0861 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <geom pos="-8.425 -1.4861 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <geom pos="-8.675 0.1639 0" type="cylinder" size="0.1 0.001" rgba="1.0 0.0 0.0 0.35" contype="0" conaffinity="0" />
+    <geom pos="-6.425 -0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-6.425 -0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-7.425 -0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-7.425 -0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-8.425 -0.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-8.425 -0.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-8.425 -1.0861 0.02" type="cylinder" size="0.05 0.02" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+    <geom pos="-8.425 -1.0861 0.37" type="cylinder" size="0.015 0.33" quat="0.707107 0 0 -0.707107" rgba="0.75 0.72 0.55 1.0" />
+  </worldbody>
+</mujoco>

README.md

@@ -2,7 +2,7 @@
 
 RC_WheelLeg 是山东华宇工学院 16DOF 串联轮足机器人项目。
 
-当前 `16dof` 分支用于整理 16DOF 机械、强化学习训练、Sim2Sim、Sim2Real、ROS 2 部署和比赛版本。机械资料、比赛训练架构和后期 MuJoCo 工具集已经完成整理。
+当前 `16dof` 分支用于整理 16DOF 机械、强化学习训练、Sim2Sim、Sim2Real、ROS 2 部署和比赛版本。机械资料、比赛训练架构、后期 MuJoCo/Sim2Sim 和导航打点工具已经完成整理。
 
 ## 平台概览
 
@@ -37,6 +37,8 @@ RC_WheelLeg/
 - [x] 整理第二版 Sim2Real 随机化训练配置
 - [x] 整理比赛最终训练代码架构
 - [x] 整理后期 MuJoCo 姿态、IK、动力学和 MPC 工具
+- [x] 整理后期 Sim2Sim、路线检查与比赛 Rough ONNX 策略
+- [x] 整理导航地图、打点工具、路线迭代和抽样 PCD
 - [ ] 核对比赛机械与仿真模型参数
 - [ ] 整理 URDF/MJCF 机器人描述
 - [ ] 整理后续统一训练、ROS 2 和比赛版本