[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/training_evolution.md

@@ -0,0 +1,37 @@
+# 训练代码演进
+
+本项目将“训练代码架构”和“训练产生的模型 checkpoint”分别管理。代码、奖励、课程或观测契约发生变化时形成新的 Git 版本；同一架构下继续训练产生的模型编号作为实验和部署工件记录。
+
+| 版本 | 来源快照 | 主要目的 |
+| --- | --- | --- |
+| `v0.4.0` | `uni_mjlab(1)` | 第一份完整的新 MJCF 与新 mjlab 训练工程 |
+| `v0.5.0` | `uni_mjlab_new` | 扩大观测、延迟和动力学随机化，探索更强 Sim2Real 鲁棒性 |
+| `v0.6.0` | `best` | 面向比赛越障重新设计奖励、课程、站姿和诊断体系 |
+
+## 从随机化增强到比赛训练
+
+`v0.6.0` 不是简单地继续增大 `v0.5.0` 的随机范围。实践中重新降低了部分噪声、延迟和动力学随机化强度，并把训练重点转向可控的比赛任务课程：
+
+- 投影重力噪声由 `±0.08` 调回 `±0.05`。
+- 腿和轮动作最大随机延迟由 4 步调回 2 步。
+- 摩擦、刚度和阻尼随机化调回较窄范围。
+- 移除该阶段的连续机身外力扰动与腿部质量随机化。
+- 将 x、y 和 yaw 跟踪拆成独立奖励和独立指令课程。
+- 障碍由课程逐步释放，高墙训练地形改为五道重复横墙。
+- 增加楼梯侧向漂移和偏航漂移惩罚。
+- 增加大量只用于训练评估的误差、轮速、姿态和接触指标。
+
+这种调整反映的是从“广泛鲁棒性探索”转向“比赛场景定向优化”，不表示 `v0.5.0` 被删除；它仍由对应 Tag 完整保留。
+
+## 模型与比赛部署
+
+比赛训练可能先获得基模，再修改参数继续训练和筛选 checkpoint。最终关系为：
+
+```text
+训练代码架构：v0.6.0 / best
+比赛 Rough 策略：model_6800.onnx
+比赛真机工程：last_not_slalom_1050
+比赛得分：1050
+```
+
+`model_6800.onnx` 是比赛最终部署工件，不用模型编号替代训练代码版本号。它将在最终比赛部署版本中与运行配置一起归档。

01_doc/version_history.md

@@ -10,6 +10,10 @@
 | `v0.3.1` | 实机记录 | 补充第一代 Sim2Real 实机视频 |
 | `v0.4.0` | 新训练基线 | 第一份完整的新 MJCF、新 mjlab 框架和 Rough 策略工程 |
 | `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` 的模型变化
 
@@ -28,3 +32,42 @@
 - 执行器刚度和阻尼缩放由 `0.9–1.1` 扩大到 `0.5–1.5`。
 - 增加膝部和轮部质量的 `0.7–1.3` 随机缩放。
 - 增加作用于机身的连续随机外力和力矩扰动。
+
+## `v0.6.0` 的比赛训练架构
+
+- 保持 `v0.4.0` 引入的新 MJCF 和 mjlab 框架不变。
+- 将线速度奖励拆分为 x/y 两轴，并独立配置偏航角速度奖励。
+- 增加自适应 x/y/yaw 指令范围课程。
+- 增加障碍地形逐步释放与更严格的地形晋级逻辑。
+- 增加楼梯横向速度和偏航漂移约束。
+- 默认站姿调整为髋俯仰 `0.550`、膝关节 `-1.125`，初始机身高度为 `0.42 m`。
+- 增加速度误差、轮速跟踪、动作和姿态等训练诊断指标。
+- 该 Tag 保存比赛训练代码架构，不把每次继续训练产生的 checkpoint 误记为新的软件版本。
+
+训练阶段的详细关系见 [`training_evolution.md`](training_evolution.md)。
+
+## `v0.7.0` 的独立 MuJoCo 工具
+
+- 比赛训练架构、MJCF、模型和依赖锁文件保持 `v0.6.0` 状态不变。
+- 增加解析姿态表、RL 友好姿态筛选和 MuJoCo 静态姿态优化。
+- 增加 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

@@ -5,7 +5,7 @@
 ```text
 05_software/
 ├─ train/
-│  └─ rc_mjlab/       # 训练、MJCF、Sim2Sim 和本地 mjlab 依赖
+│  └─ rc_mjlab/       # 训练、MJCF、MuJoCo、Sim2Sim 和本地 mjlab 依赖
 └─ real/
    ├─ ik_real/        # IK 轨迹与早期真机控制
    └─ sim2real/       # 第一代 Python 策略真机部署
@@ -18,6 +18,8 @@ MJCF + mjlab task
         |
         v
    PPO 训练策略
+        |
+        +----> MuJoCo 姿态 / IK / MPC 调试
         |
         +----> Sim2Sim 策略验证
         |
@@ -26,7 +28,7 @@ MJCF + mjlab task
 IK real --------------------------------> 电机
 ```
 
-`rc_mjlab` 是自包含工程。训练、MJCF、Sim2Sim 和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，增强 Sim2Real 随机化的第二版训练配置见 `v0.5.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,19 +2,30 @@
 
 `rc_mjlab/` 保存 16DOF 轮足机器人的当前训练与 Sim2Sim 工程。历史快照由 Git Tag 保留，不在目录中复制 `old`、`new` 或 `final` 版本。
 
-当前内容对应 `v0.5.0`，在 `v0.4.0` 的完整新版训练工程上增强了面向 Sim2Real 的随机化配置。
+当前内容对应 `v0.8.1`：训练代码保持 `v0.6.0` 的比赛架构，包含后期 MuJoCo、Sim2Sim、比赛最终 Rough ONNX 策略，并补充完整导航打点工具、路线迭代和抽样 PCD。训练过程可能先获得基模，再调整奖励、课程和环境参数继续训练；模型 checkpoint 的变化不等同于软件架构变化。
 
 ## 内容
 
 - `src/robot`：Flat、Rough、Crawl 训练任务和自定义 MDP
 - `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 未发生变化。
 
 与 `v0.4.0` 相比，本版本没有再次修改 MJCF 和训练框架，只调整训练环境配置：投影重力噪声从 `±0.05` 扩大到 `±0.08`，最大动作延迟从 2 步增加到 4 步，扩大摩擦、刚度和阻尼随机化，增加腿部质量随机化与连续外力/力矩扰动。
 
+`v0.6.0` 在 `v0.5.0` 之后转向比赛任务优化：降低部分过强随机化，加入分轴速度跟踪奖励、自适应指令课程、障碍地形释放课程、楼梯横向/偏航约束以及更完整的训练诊断。详细对比见 [`../../01_doc/training_evolution.md`](../../01_doc/training_evolution.md)。
+
+`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.5.0`：在第一份完整的新 MJCF 与新框架训练工程上，扩大观测噪声、动作延迟和动力学随机化范围，并加入持续外力扰动。该快照包含 `model_rough.pt`；未包含独立 `mujoco_sim` 工具和单独的 Crawl 策略权重，相关早期内容仍可通过 `v0.3.0` 查看。
+> 当前目录对应 `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 输入输出与传感器、低通滤波器接口
@@ -53,7 +57,10 @@ rc_mjlab/
 │   ├── wheelleg.xml              # 机器人 MuJoCo 模型（含网格引用）
 │   ├── scene.xml                 # mjlab 场景入口文件
 │   └── meshes/                   # STL/OBJ 碰撞与外观网格
-├── model_rough.pt                # 本阶段用于回放和 Sim2Sim 的 Rough 策略
+├── mujoco_sim/                   # 姿态、IK、动力学和 MPC 独立工具
+├── tools/nav_tools/              # 地图/PCD/航点编辑、路线检查与比赛路线数据
+├── model_rough.pt                # 早期 Rough 参考 checkpoint
+├── model_6800.onnx               # 比赛最终 Rough 策略
 ├── pyproject.toml                # 项目依赖（uv 管理，含清华镜像源加速）
 └── uv.lock                       # 精确依赖锁定文件
 ```
@@ -88,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/mujoco_sim/README.md

@@ -0,0 +1,57 @@
+# MuJoCo 独立工具集
+
+本目录保存不依赖训练循环的 MuJoCo、姿态、IK、动力学和 MPC 分析工具。脚本通过父目录读取 `../mjcf/scene.xml` 与 `../mjcf/wheelleg.xml`，因此应从 `rc_mjlab` 工程根目录运行。
+
+## 工具分类
+
+| 入口 | 用途 |
+| --- | --- |
+| `posture_tool.py` | 基于解析运动学快速生成站立和低姿态参数表 |
+| `rl_friendly_opt.py` | 按轮心位置、关节力矩和雅可比条件数筛选适合 RL 的姿态 |
+| `posture_optimizer.py` | 解析计算与 MuJoCo 扫描结合的姿态优化 |
+| `static_posture_optimizer.py` | 在重力和地面接触下评估静态站姿、支撑域和离地间隙 |
+| `ik_diff_sweep.py` | 扫描 IK 姿态和差速轮跟踪参数，可导出 JSON |
+| `run.py` | 启动完整 MuJoCo 控制、GUI 和 MPC 调试链路 |
+| `robot.py`、`controller.py` | 仿真机器人接口和控制器 |
+| `dynamics.py`、`mpc.py`、`mpc_controller.py` | Pinocchio 动力学与 OSQP MPC |
+
+## 依赖
+
+执行工程根目录的 `uv sync` 后，训练环境已经提供 NumPy、SciPy 和 MuJoCo。不同工具还需要：
+
+- 纯解析工具：Python、NumPy。
+- MuJoCo 扫描：`mujoco`、NumPy。
+- 完整 MPC：`pinocchio`、`osqp`、SciPy。
+- GUI：系统可用的 Tk/Tkinter。
+
+Pinocchio 和 OSQP 没有加入训练环境锁文件，因为它们只服务于可选 MPC 工具，且 Pinocchio 的安装方式与操作系统、Conda/Python 环境有关。
+
+## 常用命令
+
+在 `05_software/train/rc_mjlab` 下执行：
+
+```bash
+# 不启动 MuJoCo 的快速姿态表
+uv run python mujoco_sim/posture_tool.py
+uv run python mujoco_sim/rl_friendly_opt.py
+
+# 姿态扫描
+uv run python mujoco_sim/posture_optimizer.py --analyze
+uv run python mujoco_sim/static_posture_optimizer.py --quick
+
+# IK 与差速轮参数快速扫描
+uv run python mujoco_sim/ik_diff_sweep.py --quick
+
+# 完整 GUI/MPC 仿真，需要可选依赖
+uv run python mujoco_sim/run.py
+```
+
+## 参数边界
+
+这是一份历史工具快照，保留当时用于分析和调参的常量：
+
+- `config.py`、`posture_optimizer.py` 和 `static_posture_optimizer.py` 中的解析质量常量为 `12.3 kg`。
+- 当前新版 MJCF 的惯性质量合计约为 `18.0377 kg`。
+- `config.py` 的姿态表默认值为髋俯仰 `0.666`、膝关节 `-1.546`；比赛训练架构的 Rough 默认姿态为 `0.550/-1.125`。
+
+MuJoCo 直接加载模型的工具会使用 MJCF 内的质量和惯性；显式读取 `ROBOT_MASS` 的解析计算和 MPC 工具仍使用历史常量。使用输出作为新版本控制参数前，应先根据目标机械状态完成质量、惯性和默认姿态复核。

05_software/train/rc_mjlab/mujoco_sim/config.py

@@ -0,0 +1,81 @@
+"""Robot constants and control parameters."""
+
+from pathlib import Path
+
+# Paths
+REPO_ROOT = Path(__file__).resolve().parents[1]
+SCENE_XML = REPO_ROOT / "mjcf" / "scene.xml"
+MJCF_PATH = REPO_ROOT / "mjcf" / "wheelleg.xml"
+
+# Robot geometry
+WHEEL_RADIUS = 0.10  # m
+WHEEL_TRACK = 0.32   # m, left-right distance
+ROBOT_MASS = 12.3    # kg
+MAX_TORQUE = 17.0    # Nm per joint
+MAX_JOINT_VEL = 13.0 # rad/s
+
+# Leg link lengths from MJCF, measured to wheel center.
+L_THIGH = 0.25
+L_CALF = 0.20
+
+# Leg names and joint ordering
+LEG_NAMES = ("fl", "fr", "rl", "rr")
+LEG_JOINTS = ("hip_abduction_joint", "hip_pitch_joint", "knee_joint")
+WHEEL_JOINT = "wheel_joint"
+
+# Default standing pose aligned with the soft wheel-X height table.
+# height ~= 0.37m, wheel x-offset ~= 0, peak/RMS leg torque balanced.
+DEFAULT_JOINT_ANGLES = {
+    "hip_abduction": 0.0,
+    "hip_pitch": 0.666,
+    "knee": -1.546,
+}
+
+# Actuator modes, configured at runtime:
+#   Leg joints: position PD, ctrl = target angle
+#   Wheel joints: velocity, ctrl = target velocity in rad/s
+
+# Control rates
+SIM_DT = 0.002
+CTRL_DT = 0.02
+CTRL_DECIMATION = int(CTRL_DT / SIM_DT)
+
+# Wheel drive
+WHEEL_VEL_MAX = 10.0
+
+# Body pose control gains for height/roll/pitch compensation.
+KP_HEIGHT = 3.0
+KP_ROLL = 0.5
+KP_PITCH = 0.5
+
+# Calibrated height-to-joint-angle table.
+# Constraint: avoid large wheel-center X offset from the hip/leg. This is a
+# soft support-geometry guardrail, not a strict x=0 requirement.
+# The optimizer also considers peak motor torque and RMS torque, so one hot
+# motor is not hidden by a low average across all motors.
+# Format: (height_m, hip_pitch_rad, knee_rad)
+HEIGHT_TABLE = [
+    (0.17, 0.914, -2.628),
+    (0.19, 0.926, -2.528),
+    (0.21, 0.924, -2.428),
+    (0.23, 0.912, -2.328),
+    (0.25, 0.892, -2.226),
+    (0.27, 0.864, -2.122),
+    (0.29, 0.834, -2.014),
+    (0.31, 0.798, -1.906),
+    (0.33, 0.758, -1.792),
+    (0.35, 0.714, -1.672),
+    (0.37, 0.666, -1.546),
+    (0.39, 0.612, -1.412),
+    (0.41, 0.552, -1.266),
+    (0.43, 0.484, -1.104),
+    (0.45, 0.404, -0.918),
+]
+
+# Gait parameters
+GAIT_FREQ = 2.5
+GAIT_DUTY = 0.6
+SWING_HEIGHT = 0.06
+
+# Trot phase offsets: FL/RR in phase, FR/RL in phase
+PHASE_OFFSETS = {"fl": 0.0, "fr": 0.5, "rl": 0.5, "rr": 0.0}

05_software/train/rc_mjlab/mujoco_sim/controller.py

@@ -0,0 +1,367 @@
+"""Main controller: wheel mode + trot mode for wheeled-legged robot.
+
+Wheel mode: differential drive + leg posture hold (height/roll/pitch compensation)
+Trot mode: quadruped gait with wheel-assisted propulsion
+
+Actuator interface:
+  - Leg joints: ctrl = target angle (PD: kp=60, kd=3)
+  - Wheel joints: ctrl = target velocity in rad/s (gain=2.0)
+"""
+
+import numpy as np
+from robot import Robot, RobotState
+from dynamics import Dynamics
+from mpc_controller import MPCController
+from config import (
+    LEG_NAMES, DEFAULT_JOINT_ANGLES, WHEEL_RADIUS, WHEEL_TRACK,
+    WHEEL_VEL_MAX, KP_ROLL, KP_PITCH, HEIGHT_TABLE,
+    GAIT_FREQ, GAIT_DUTY, SWING_HEIGHT, PHASE_OFFSETS,
+)
+
+RL_ROUGH_Q = np.array([0.0, 0.550, -1.125], dtype=float)
+LEG_STATE_IDX = np.array([0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14], dtype=int)
+
+
+class Controller:
+    """Wheeled-legged robot controller."""
+
+    def __init__(self, robot: Robot):
+        self.robot = robot
+        self.dynamics = Dynamics()
+
+        # User commands
+        self.vel_x = 0.0       # m/s forward
+        self.vel_y = 0.0       # m/s lateral
+        self.yaw_rate = 0.0    # rad/s
+        self.height = 0.37     # m desired body height (wheel center under hip)
+        self.wheel_posture = "table"  # "table" follows height slider; "rl" matches src/robot default
+
+        # Mode: "wheel", "trot", or "mpc"
+        self.mode = "wheel"
+
+        # Prone (lie down) state
+        self.prone = False
+
+        # MPC controller
+        self._mpc_ctrl = MPCController(robot)
+        self._mpc_active = False  # track torque mode state
+
+        # Gait state
+        self._gait_phase = 0.0
+
+        # Smoothed commands for trot mode (avoid sudden jumps)
+        self._smooth_vx = 0.0
+        self._smooth_vy = 0.0
+        self._smooth_yaw = 0.0
+        self._last_leg_targets = np.tile(
+            np.array(
+                [
+                    DEFAULT_JOINT_ANGLES["hip_abduction"],
+                    DEFAULT_JOINT_ANGLES["hip_pitch"],
+                    DEFAULT_JOINT_ANGLES["knee"],
+                ],
+                dtype=float,
+            ),
+            4,
+        )
+
+        # Wheel-mode sensor feedback.
+        self.yaw_rate_kp = 0.45
+        self.roll_comp_gain = KP_ROLL
+        self.pitch_comp_gain = KP_PITCH
+        self.encoder_posture_kp = 0.12
+        self.encoder_posture_max = 0.025
+        self.encoder_guard_start = 0.28
+        self.encoder_guard_stop = 0.65
+        self.imu_guard_start = np.deg2rad(12.0)
+        self.imu_guard_stop = np.deg2rad(28.0)
+        self.yaw_wheel_gain = 1.0
+        self.max_yaw_wheel_speed = 4.0
+
+        # Default leg angles
+        self._default_q = np.array([
+            DEFAULT_JOINT_ANGLES["hip_abduction"],
+            DEFAULT_JOINT_ANGLES["hip_pitch"],
+            DEFAULT_JOINT_ANGLES["knee"],
+        ])
+
+        # Swing leg memory
+        self._swing_start_foot = {leg: np.zeros(3) for leg in LEG_NAMES}
+        self._last_contact = {leg: True for leg in LEG_NAMES}
+
+    def compute(self, state: RobotState, dt: float) -> tuple[np.ndarray, np.ndarray]:
+        # Smooth all velocity commands (both modes)
+        alpha = min(dt * 3.0, 1.0)  # ~0.33s time constant
+        self._smooth_vx += alpha * (self.vel_x - self._smooth_vx)
+        self._smooth_vy += alpha * (self.vel_y - self._smooth_vy)
+        self._smooth_yaw += alpha * (self.yaw_rate - self._smooth_yaw)
+
+        if self.prone:
+            self._ensure_position_mode()
+            return self._prone_mode()
+        if self.mode == "mpc":
+            return self._mpc_mode(state, dt)
+        if self.mode == "wheel":
+            self._ensure_position_mode()
+            return self._wheel_mode(state, dt)
+        else:
+            self._ensure_position_mode()
+            return self._trot_mode(state, dt)
+
+    def _mpc_mode(self, state: RobotState, dt: float):
+        """MPC locomotion: MIT motor protocol (PD + MPC feedforward torque)."""
+        # Switch to torque mode if not already
+        if not self._mpc_active:
+            self.robot.enable_torque_mode()
+            self._mpc_active = True
+
+        # Sync commands to MPC controller
+        self._mpc_ctrl.vel_x = self.vel_x
+        self._mpc_ctrl.vel_y = self.vel_y
+        self._mpc_ctrl.yaw_rate = self.yaw_rate
+        self._mpc_ctrl.height = self.height
+
+        # Compute and apply (sets ctrl directly via set_ctrl_mit)
+        self._mpc_ctrl.compute(state, dt)
+        # Return dummy - ctrl already set
+        return np.zeros(12), np.zeros(4)
+
+    def _ensure_position_mode(self):
+        """Switch back to position PD mode if coming from MPC."""
+        if self._mpc_active:
+            self.robot.enable_position_mode()
+            self._mpc_active = False
+
+    def _prone_mode(self):
+        """Lie down: actual prone pose from real robot."""
+        leg_targets = np.zeros(12)
+        for i, leg in enumerate(LEG_NAMES):
+            side = 1.0 if leg[1] == "l" else -1.0
+            leg_targets[i*3] = side * 0.3     # fl/rl: +0.3, fr/rr: -0.3
+            leg_targets[i*3+1] = 1.5          # hip pitch
+            leg_targets[i*3+2] = -2.65        # knee hard limit from MJCF
+        return leg_targets, np.zeros(4)
+
+    # ─────────────────────────────────────────────────────────────────────
+    # WHEEL MODE
+    # ─────────────────────────────────────────────────────────────────────
+
+    def _wheel_mode(self, state: RobotState, dt: float):
+        """Wheel drive + leg posture hold.
+
+        vel_y: limited effect in wheel mode (differential drive cannot produce
+        pure lateral motion). Uses hip_abduction lean for small lateral force.
+        For significant lateral motion, use trot mode.
+        """
+        leg_targets = self._posture_control(state)
+        safe_vx, safe_yaw = self._wheel_velocity_envelope(self._smooth_vx, self._smooth_yaw)
+        yaw_feedback = safe_yaw + self.yaw_rate_kp * (safe_yaw - float(state.ang_vel[2]))
+        wheel_targets = self._differential_drive(safe_vx, yaw_feedback)
+        wheel_targets *= self._sensor_command_scale(state, leg_targets)
+        self._last_leg_targets = leg_targets.copy()
+        return leg_targets, wheel_targets
+
+    def _posture_control(self, state: RobotState) -> np.ndarray:
+        """Leg joint targets from the soft wheel-X height table."""
+        leg_targets = np.zeros(12)
+
+        # Calibrated height→angle lookup (minimizes motor torque at each height)
+        _H  = [r[0] for r in HEIGHT_TABLE]
+        _HIP = [r[1] for r in HEIGHT_TABLE]
+        _KNEE = [r[2] for r in HEIGHT_TABLE]
+
+        h_clamp = np.clip(self.height, _H[0], _H[-1])
+        q_hip_base = float(np.interp(h_clamp, _H, _HIP))
+        q_knee_base = float(np.interp(h_clamp, _H, _KNEE))
+        q_ab_base = 0.0
+        if self.wheel_posture == "rl":
+            q_ab_base, q_hip_base, q_knee_base = RL_ROUGH_Q
+
+        roll_corr = -self.roll_comp_gain * state.rpy[0]
+        pitch_corr = -self.pitch_comp_gain * state.rpy[1]
+        lateral_lean = 0.3 * self.vel_y
+
+        for i, leg in enumerate(LEG_NAMES):
+            side = 1.0 if leg[1] == "l" else -1.0
+            leg_targets[i*3] = np.clip(q_ab_base + side * roll_corr + lateral_lean, -0.5, 0.5)
+            leg_targets[i*3+1] = np.clip(q_hip_base + pitch_corr, -1.0, 2.5)
+            leg_targets[i*3+2] = np.clip(q_knee_base, -2.65, -0.3)
+
+        encoder_err = self._last_leg_targets - state.joint_pos[LEG_STATE_IDX]
+        leg_targets += np.clip(
+            self.encoder_posture_kp * encoder_err,
+            -self.encoder_posture_max,
+            self.encoder_posture_max,
+        )
+        leg_targets[0::3] = np.clip(leg_targets[0::3], -0.5, 0.5)
+        leg_targets[1::3] = np.clip(leg_targets[1::3], -1.0, 2.5)
+        leg_targets[2::3] = np.clip(leg_targets[2::3], -2.65, -0.3)
+        return leg_targets
+
+    def _wheel_velocity_envelope(self, vel_x: float, yaw_rate: float) -> tuple[float, float]:
+        """Limit x/yaw combinations that are unsafe for the RL posture."""
+        ax = abs(vel_x)
+        if ax >= 0.8:
+            yaw_lim = 0.35
+        elif ax >= 0.5:
+            yaw_lim = 0.55
+        elif ax >= 0.25:
+            yaw_lim = 0.75
+        else:
+            yaw_lim = 1.0
+        return float(vel_x), float(np.clip(yaw_rate, -yaw_lim, yaw_lim))
+
+    def _sensor_command_scale(self, state: RobotState, leg_targets: np.ndarray) -> float:
+        """Back off wheels when IMU or encoder feedback says posture is degrading."""
+        leg_error = float(np.max(np.abs(state.joint_pos[LEG_STATE_IDX] - leg_targets)))
+        tilt = float(np.hypot(state.rpy[0], state.rpy[1]))
+        scale = 1.0
+
+        if leg_error >= self.encoder_guard_stop:
+            scale = 0.0
+        elif leg_error > self.encoder_guard_start:
+            span = max(1e-6, self.encoder_guard_stop - self.encoder_guard_start)
+            scale *= 1.0 - (leg_error - self.encoder_guard_start) / span
+
+        if tilt >= self.imu_guard_stop:
+            scale = 0.0
+        elif tilt > self.imu_guard_start:
+            span = max(1e-6, self.imu_guard_stop - self.imu_guard_start)
+            scale *= 1.0 - (tilt - self.imu_guard_start) / span
+
+        return float(np.clip(scale, 0.0, 1.0))
+
+    # ─────────────────────────────────────────────────────────────────────
+    # TROT MODE
+    # ─────────────────────────────────────────────────────────────────────
+
+    def _trot_mode(self, state: RobotState, dt: float):
+        """Trot gait with wheel assist."""
+        # Advance gait phase
+        self._gait_phase = (self._gait_phase + dt * GAIT_FREQ) % 1.0
+
+        # Contact state
+        contacts = {}
+        for leg in LEG_NAMES:
+            phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
+            contacts[leg] = phase < GAIT_DUTY
+
+        # Pinocchio update
+        q_pin, dq_pin = self.robot.get_qpos_qvel_for_pinocchio()
+        self.dynamics.update(q_pin, dq_pin)
+
+        leg_targets = np.zeros(12)
+        wheel_targets = np.zeros(4)
+
+        for i, leg in enumerate(LEG_NAMES):
+            if contacts[leg]:
+                # Stance: posture hold
+                leg_targets[i*3:(i+1)*3] = self._stance_leg_target(state, leg)
+                self._swing_start_foot[leg] = self.dynamics.get_foot_pos(leg)
+                self._last_contact[leg] = True
+                # Wheel: drive with smoothed velocity
+                wheel_targets[i] = self._differential_drive_single(
+                    self._smooth_vx, self._smooth_yaw, leg)
+            else:
+                # Swing: IK trajectory
+                swing_phase = self._get_swing_phase(leg)
+                target_foot = self._compute_swing_target(leg, state, swing_phase)
+                q_ik = self.dynamics.inverse_kinematics(leg, target_foot, q_pin)
+                leg_targets[i*3:(i+1)*3] = q_ik
+                self._last_contact[leg] = False
+                # Wheel: zero (free during swing)
+                wheel_targets[i] = 0.0
+
+        return leg_targets, wheel_targets
+
+    def _stance_leg_target(self, state: RobotState, leg: str) -> np.ndarray:
+        """Stance leg: table-interpolated height + attitude compensation."""
+        _H  = [r[0] for r in HEIGHT_TABLE]
+        _HIP = [r[1] for r in HEIGHT_TABLE]
+        _KNEE = [r[2] for r in HEIGHT_TABLE]
+
+        h_clamp = np.clip(self.height, _H[0], _H[-1])
+        q_hip = float(np.interp(h_clamp, _H, _HIP))
+        q_knee = float(np.interp(h_clamp, _H, _KNEE))
+
+        roll_corr = -KP_ROLL * state.rpy[0]
+        pitch_corr = -KP_PITCH * state.rpy[1]
+        side = 1.0 if leg[1] == "l" else -1.0
+        lateral_lean = 0.3 * self.vel_y
+
+        return np.array([
+            np.clip(side * roll_corr + lateral_lean, -0.5, 0.5),
+            np.clip(q_hip + pitch_corr, -1.0, 2.5),
+            np.clip(q_knee, -2.65, -0.3),
+        ])
+
+    # ─────────────────────────────────────────────────────────────────────
+    # DIFFERENTIAL DRIVE
+    # ─────────────────────────────────────────────────────────────────────
+
+    def _differential_drive(self, vel_x: float, yaw_rate: float) -> np.ndarray:
+        """4 wheel velocities from body commands."""
+        linear_wheel = vel_x / WHEEL_RADIUS
+        yaw_wheel = self.yaw_wheel_gain * 0.5 * WHEEL_TRACK * yaw_rate / WHEEL_RADIUS
+        yaw_wheel = float(np.clip(yaw_wheel, -self.max_yaw_wheel_speed, self.max_yaw_wheel_speed))
+        vel_left = linear_wheel - yaw_wheel
+        vel_right = linear_wheel + yaw_wheel
+        targets = np.zeros(4)
+        for i, leg in enumerate(LEG_NAMES):
+            targets[i] = vel_left if leg[1] == "l" else vel_right
+        return np.clip(targets, -WHEEL_VEL_MAX, WHEEL_VEL_MAX)
+
+    def _differential_drive_single(self, vel_x: float, yaw_rate: float, leg: str) -> float:
+        if leg[1] == "l":
+            v = (vel_x - 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
+        else:
+            v = (vel_x + 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
+        return np.clip(v, -WHEEL_VEL_MAX, WHEEL_VEL_MAX)
+
+    # ─────────────────────────────────────────────────────────────────────
+    # SWING TRAJECTORY
+    # ─────────────────────────────────────────────────────────────────────
+
+    def _get_swing_phase(self, leg: str) -> float:
+        phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
+        if phase < GAIT_DUTY:
+            return 0.0
+        return (phase - GAIT_DUTY) / (1.0 - GAIT_DUTY)
+
+    def _compute_swing_target(self, leg: str, state: RobotState,
+                              swing_phase: float) -> np.ndarray:
+        """Swing foot target with Raibert heuristic using COMMANDED velocity."""
+        p_start = self._swing_start_foot[leg]
+        p_end = self._compute_touchdown(leg, state)
+
+        s = swing_phase
+        s_mj = 10*s**3 - 15*s**4 + 6*s**5
+
+        pos = p_start + (p_end - p_start) * s_mj
+
+        # Z lift
+        z_lift = 64.0 * s**3 * (1.0 - s)**3
+        pos[2] = p_start[2] + SWING_HEIGHT * z_lift
+
+        return pos
+
+    def _compute_touchdown(self, leg: str, state: RobotState) -> np.ndarray:
+        """Raibert heuristic using COMMANDED velocity.
+
+        When commands are zero, foot lands at its takeoff position (no net motion).
+        When commands are nonzero, foot placement is offset by commanded velocity.
+        """
+        # Base: land where the foot took off (zero net displacement)
+        td = self._swing_start_foot[leg].copy()
+
+        # Add commanded velocity offset (Raibert-style)
+        t_stance = (1.0 / GAIT_FREQ) * GAIT_DUTY
+        yaw = state.rpy[2]
+        c, s = np.cos(yaw), np.sin(yaw)
+        R_z = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
+        cmd_vel_world = R_z @ np.array([self._smooth_vx, self._smooth_vy, 0.0])
+
+        td[0] += cmd_vel_world[0] * t_stance * 0.5
+        td[1] += cmd_vel_world[1] * t_stance * 0.5
+        td[2] = WHEEL_RADIUS  # ground level
+        return td

05_software/train/rc_mjlab/mujoco_sim/dynamics.py

@@ -0,0 +1,97 @@
+"""Pinocchio dynamics: FK, Jacobian, IK for the wheeled-legged robot."""
+
+import numpy as np
+import pinocchio as pin
+from config import MJCF_PATH, LEG_NAMES
+
+# Foot frame names in Pinocchio model (wheel link centers)
+FOOT_FRAMES = {leg: f"{leg}_wheel_Link" for leg in LEG_NAMES}
+
+# Leg joint names for each leg
+_LEG_JOINT_NAMES = {
+    leg: [f"{leg}_{jt}" for jt in ("hip_abduction_joint", "hip_pitch_joint", "knee_joint")]
+    for leg in LEG_NAMES
+}
+
+
+class Dynamics:
+    """Pinocchio-based kinematics/dynamics. Deployable on real hardware."""
+
+    def __init__(self):
+        self.model = pin.buildModelFromMJCF(str(MJCF_PATH))
+        self.data = self.model.createData()
+
+        # Cache frame IDs
+        self._foot_fids = {}
+        for leg, fname in FOOT_FRAMES.items():
+            self._foot_fids[leg] = self.model.getFrameId(fname)
+
+        # Cache joint velocity indices for each leg (3 joints)
+        self._leg_v_indices = {}
+        for leg, jnames in _LEG_JOINT_NAMES.items():
+            indices = []
+            for jn in jnames:
+                jid = self.model.getJointId(jn)
+                indices.append(self.model.joints[jid].idx_v)
+            self._leg_v_indices[leg] = indices
+
+        # Cache joint config indices for each leg
+        self._leg_q_indices = {}
+        for leg, jnames in _LEG_JOINT_NAMES.items():
+            indices = []
+            for jn in jnames:
+                jid = self.model.getJointId(jn)
+                indices.append(self.model.joints[jid].idx_q)
+            self._leg_q_indices[leg] = indices
+
+    def update(self, q: np.ndarray, dq: np.ndarray):
+        """Forward kinematics + Jacobians.
+
+        Args:
+            q: Pinocchio config (nq=23: pos3, quat_xyzw4, joints16)
+            dq: Pinocchio velocity (nv=22: v_body3, w_body3, joints16)
+        """
+        pin.forwardKinematics(self.model, self.data, q, dq)
+        pin.updateFramePlacements(self.model, self.data)
+        pin.computeJointJacobians(self.model, self.data, q)
+
+    def get_foot_pos(self, leg: str) -> np.ndarray:
+        """Foot (wheel center) position in world frame (3,)."""
+        return self.data.oMf[self._foot_fids[leg]].translation.copy()
+
+    def get_foot_jacobian_leg(self, leg: str) -> np.ndarray:
+        """3x3 linear Jacobian of foot w.r.t. 3 leg joints (world frame)."""
+        fid = self._foot_fids[leg]
+        J_full = pin.getFrameJacobian(
+            self.model, self.data, fid, pin.LOCAL_WORLD_ALIGNED)[:3, :]
+        cols = self._leg_v_indices[leg]
+        return J_full[:, cols]
+
+    def inverse_kinematics(self, leg: str, target_pos: np.ndarray,
+                           q_current: np.ndarray, max_iter=30, eps=1e-4) -> np.ndarray:
+        """Numerical IK for one leg. Returns (3,) joint angles.
+
+        Args:
+            leg: Leg name
+            target_pos: Desired foot position in world frame (3,)
+            q_current: Current full Pinocchio config (nq=23)
+        """
+        q = q_current.copy()
+        fid = self._foot_fids[leg]
+        q_indices = self._leg_q_indices[leg]
+
+        for _ in range(max_iter):
+            pin.forwardKinematics(self.model, self.data, q)
+            pin.updateFramePlacements(self.model, self.data)
+            err = target_pos - self.data.oMf[fid].translation
+            if np.linalg.norm(err) < eps:
+                break
+            pin.computeJointJacobians(self.model, self.data, q)
+            J = pin.getFrameJacobian(
+                self.model, self.data, fid, pin.LOCAL_WORLD_ALIGNED)[:3, :]
+            J_leg = J[:, self._leg_v_indices[leg]]
+            dq = np.linalg.solve(J_leg.T @ J_leg + 1e-6 * np.eye(3), J_leg.T @ err)
+            for i, idx in enumerate(q_indices):
+                q[idx] += dq[i]
+
+        return np.array([q[idx] for idx in q_indices])

05_software/train/rc_mjlab/mujoco_sim/gui.py

@@ -0,0 +1,127 @@
+"""GUI control panel for the wheeled-legged robot."""
+
+import tkinter as tk
+from tkinter import ttk
+
+
+class GUI:
+    """Tkinter control panel: sliders + gait buttons + status display."""
+
+    def __init__(self, controller):
+        self.ctrl = controller
+        self.root = tk.Tk()
+        self.root.title("WheelLeg Control")
+        self.root.geometry("400x500")
+        self.root.protocol("WM_DELETE_WINDOW", self._on_close)
+        self._closed = False
+
+        self._build()
+
+    def _build(self):
+        # Mode buttons
+        mf = ttk.LabelFrame(self.root, text="Mode")
+        mf.pack(fill="x", padx=8, pady=4)
+        for mode in ("wheel", "trot", "mpc"):
+            ttk.Button(mf, text=mode.upper(),
+                       command=lambda m=mode: self._set_mode(m)
+                       ).pack(side="left", padx=4, expand=True)
+        ttk.Button(mf, text="PRONE/STAND",
+                   command=self._toggle_prone).pack(side="left", padx=4, expand=True)
+
+        # Command sliders
+        cf = ttk.LabelFrame(self.root, text="Commands")
+        cf.pack(fill="x", padx=8, pady=4)
+
+        self.vel_x_var = tk.DoubleVar(value=0.0)
+        self.vel_y_var = tk.DoubleVar(value=0.0)
+        self.yaw_var = tk.DoubleVar(value=0.0)
+        self.height_var = tk.DoubleVar(value=self.ctrl.height)
+
+        self._slider(cf, "Vel X", self.vel_x_var, -1.5, 1.5)
+        self._slider(cf, "Vel Y*", self.vel_y_var, -0.5, 0.5)
+        self._slider(cf, "Yaw", self.yaw_var, -2.0, 2.0)
+        self._slider(cf, "Height", self.height_var, 0.17, 0.45)
+
+        ttk.Label(cf, text="* Vel Y: trot mode only (diff-drive can't sidestep)",
+                  font=("", 8)).pack(anchor="w", padx=8)
+
+        ttk.Button(cf, text="Reset", command=self._reset).pack(pady=4)
+
+        # Status display
+        sf = ttk.LabelFrame(self.root, text="Status")
+        sf.pack(fill="both", expand=True, padx=8, pady=4)
+        self.status_text = tk.Text(sf, height=12, width=45, font=("Consolas", 9))
+        self.status_text.pack(fill="both", expand=True, padx=4, pady=4)
+
+    def _slider(self, parent, label, var, lo, hi):
+        f = ttk.Frame(parent)
+        f.pack(fill="x", padx=4, pady=2)
+        ttk.Label(f, text=label, width=7).pack(side="left")
+        ttk.Scale(f, from_=lo, to=hi, variable=var,
+                  command=lambda *_: self._sync()).pack(side="left", fill="x", expand=True)
+        lbl = ttk.Label(f, text="0.00", width=6)
+        lbl.pack(side="left")
+        var.trace_add("write", lambda *_, v=var, l=lbl: l.config(text=f"{v.get():.2f}"))
+
+    def _set_mode(self, mode):
+        self.ctrl.mode = mode
+        self.ctrl.prone = False
+
+    def _toggle_prone(self):
+        self.ctrl.prone = not self.ctrl.prone
+
+    def _sync(self):
+        self.ctrl.vel_x = self.vel_x_var.get()
+        self.ctrl.vel_y = self.vel_y_var.get()
+        self.ctrl.yaw_rate = self.yaw_var.get()
+        self.ctrl.height = self.height_var.get()
+
+    def _reset(self):
+        self.vel_x_var.set(0.0)
+        self.vel_y_var.set(0.0)
+        self.yaw_var.set(0.0)
+        self._sync()
+
+    def _on_close(self):
+        self._closed = True
+        self.root.destroy()
+
+    @property
+    def closed(self):
+        return self._closed
+
+    def update_status(self, state, step):
+        """Update status text with current robot state."""
+        txt = (
+            f"Mode: {self.ctrl.mode}  Step: {step}\n"
+            f"Pos:  x={state.pos[0]:.3f} y={state.pos[1]:.3f} z={state.pos[2]:.3f}\n"
+            f"RPY:  r={np.degrees(state.rpy[0]):.1f}° p={np.degrees(state.rpy[1]):.1f}° "
+            f"y={np.degrees(state.rpy[2]):.1f}°\n"
+            f"Vel:  vx={state.lin_vel[0]:.3f} vy={state.lin_vel[1]:.3f} vz={state.lin_vel[2]:.3f}\n"
+            f"Cmd:  vx={self.ctrl.vel_x:.2f} yaw={self.ctrl.yaw_rate:.2f} h={self.ctrl.height:.3f}\n"
+            f"─────────────────────────────────\n"
+        )
+        # Joint angles (compact)
+        for i, leg in enumerate(("FL", "FR", "RL", "RR")):
+            q = state.joint_pos[i*4:i*4+3]
+            w = state.joint_vel[i*4+3]
+            txt += f"{leg}: [{q[0]:+.2f} {q[1]:+.2f} {q[2]:+.2f}] w={w:+.1f}\n"
+
+        self.status_text.delete("1.0", tk.END)
+        self.status_text.insert(tk.END, txt)
+
+    def tick(self):
+        """Process GUI events. Returns False if window closed."""
+        if self._closed:
+            return False
+        try:
+            self.root.update_idletasks()
+            self.root.update()
+            return True
+        except tk.TclError:
+            self._closed = True
+            return False
+
+
+# Need numpy for degrees conversion in update_status
+import numpy as np

05_software/train/rc_mjlab/mujoco_sim/ik_diff_sweep.py

@@ -0,0 +1,389 @@
+"""Sweep wheel-mode IK postures for differential-drive tracking.
+
+The sweep is intentionally small and reproducible:
+  1. Generate ab=0 leg postures in the requested height range.
+  2. Keep candidates with good static geometry from rl_friendly_opt.
+  3. Simulate forward, yaw, and arc commands in MuJoCo.
+  4. Rank by attitude, x-speed tracking, yaw-rate tracking, and wheel contact.
+
+Usage:
+    uv run python mujoco_sim/ik_diff_sweep.py --quick
+    uv run python mujoco_sim/ik_diff_sweep.py --height-min 0.15 --height-max 0.42
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import math
+import sys
+from dataclasses import dataclass
+from pathlib import Path
+
+import mujoco
+import numpy as np
+
+THIS_DIR = Path(__file__).resolve().parent
+if str(THIS_DIR) not in sys.path:
+    sys.path.insert(0, str(THIS_DIR))
+
+from config import LEG_JOINTS, LEG_NAMES, SCENE_XML, WHEEL_JOINT, WHEEL_RADIUS  # noqa: E402
+from rl_friendly_opt import get_all, rl_cost  # noqa: E402
+from robot import Robot  # noqa: E402
+
+
+@dataclass(frozen=True)
+class Candidate:
+    height: float
+    ab: float
+    hip: float
+    knee: float
+    static_cost: float
+    r_hip_x: float
+    cond: float
+    max_tau: float
+
+
+@dataclass(frozen=True)
+class Trial:
+    name: str
+    vx: float
+    yaw_rate: float
+    duration: float
+
+
+def wrap_pi(x: float) -> float:
+    return (x + math.pi) % (2.0 * math.pi) - math.pi
+
+
+def body_track(model: mujoco.MjModel, data: mujoco.MjData) -> float:
+    wheel_bids = []
+    for leg in LEG_NAMES:
+        bid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, f"{leg}_wheel_Link")
+        wheel_bids.append(bid)
+    y = data.xipos[wheel_bids, 1]
+    return float(np.mean(y[[0, 2]]) - np.mean(y[[1, 3]]))
+
+
+def build_maps(robot: Robot) -> tuple[dict[str, int], dict[str, int], dict[str, int]]:
+    act: dict[str, int] = {}
+    qadr: dict[str, int] = {}
+    vadr: dict[str, int] = {}
+    for leg in LEG_NAMES:
+        for jt in (*LEG_JOINTS, WHEEL_JOINT):
+            name = f"{leg}_{jt}"
+            act[name] = mujoco.mj_name2id(robot.model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
+            jid = mujoco.mj_name2id(robot.model, mujoco.mjtObj.mjOBJ_JOINT, name)
+            qadr[name] = robot.model.jnt_qposadr[jid]
+            vadr[name] = robot.model.jnt_dofadr[jid]
+    return act, qadr, vadr
+
+
+def set_posture(robot: Robot, cand: Candidate, act: dict[str, int], qadr: dict[str, int]) -> None:
+    mujoco.mj_resetData(robot.model, robot.data)
+    for leg in LEG_NAMES:
+        side_ab = cand.ab if leg[1] == "l" else -cand.ab
+        for jt, val in zip(LEG_JOINTS, (side_ab, cand.hip, cand.knee)):
+            name = f"{leg}_{jt}"
+            robot.data.qpos[qadr[name]] = val
+            robot.data.ctrl[act[name]] = val
+        robot.data.ctrl[act[f"{leg}_{WHEEL_JOINT}"]] = 0.0
+    robot.data.qpos[:3] = [0.0, 0.0, max(0.25, cand.height + 0.08)]
+    robot.data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
+    robot.data.qvel[:] = 0.0
+    mujoco.mj_forward(robot.model, robot.data)
+
+
+def wheel_targets(
+    vx: float,
+    yaw_rate: float,
+    track: float,
+    max_wheel: float,
+    yaw_gain: float,
+    wheel_model: str,
+    linear_gain: float,
+    direct_yaw_gain: float,
+    wheel_signs: np.ndarray,
+) -> np.ndarray:
+    if wheel_model == "direct":
+        left = linear_gain * vx - direct_yaw_gain * yaw_rate
+        right = linear_gain * vx + direct_yaw_gain * yaw_rate
+    else:
+        left = (vx - yaw_gain * 0.5 * track * yaw_rate) / WHEEL_RADIUS
+        right = (vx + yaw_gain * 0.5 * track * yaw_rate) / WHEEL_RADIUS
+    raw = np.array([left, right, left, right], dtype=float)
+    return np.clip(raw * wheel_signs, -max_wheel, max_wheel)
+
+
+def run_trial(robot: Robot, cand: Candidate, trial: Trial, args: argparse.Namespace) -> dict:
+    act, qadr, vadr = build_maps(robot)
+    set_posture(robot, cand, act, qadr)
+
+    ctrl_dt = args.control_dt
+    sim_dt = robot.model.opt.timestep
+    steps_per_ctrl = max(1, int(round(ctrl_dt / sim_dt)))
+    track = body_track(robot.model, robot.data) if args.track_source == "model" else args.track_width
+    wheel_signs = np.array(args.wheel_signs, dtype=float)
+    wheel_cmd = wheel_targets(
+        trial.vx,
+        trial.yaw_rate,
+        track,
+        args.max_wheel_speed,
+        args.yaw_gain,
+        args.wheel_model,
+        args.linear_gain,
+        args.direct_yaw_gain,
+        wheel_signs,
+    )
+
+    for _ in range(int(round(args.settle / sim_dt))):
+        for leg in LEG_NAMES:
+            vals = (cand.ab if leg[1] == "l" else -cand.ab, cand.hip, cand.knee)
+            for jt, val in zip(LEG_JOINTS, vals):
+                robot.data.ctrl[act[f"{leg}_{jt}"]] = val
+            robot.data.ctrl[act[f"{leg}_{WHEEL_JOINT}"]] = 0.0
+        robot.step()
+
+    state0 = robot.get_state()
+    yaw0 = float(state0.rpy[2])
+    x0 = float(state0.pos[0])
+
+    max_roll = 0.0
+    max_pitch = 0.0
+    max_tilt = 0.0
+    max_wheel_air = -1e9
+    wheel_err_sum = 0.0
+    samples = 0
+    max_leg_err = 0.0
+    body_vx_sum = 0.0
+    yaw_unwrapped = 0.0
+    last_yaw = yaw0
+    leg_target = np.array([cand.ab, cand.hip, cand.knee] * 4, dtype=float)
+
+    total_steps = int(round(trial.duration / sim_dt))
+    cmd = np.zeros(4, dtype=float)
+    max_delta = args.wheel_accel_limit * ctrl_dt
+    wheel_body_ids = [
+        mujoco.mj_name2id(robot.model, mujoco.mjtObj.mjOBJ_BODY, f"{leg}_wheel_Link")
+        for leg in LEG_NAMES
+    ]
+    wheel_vadr = np.array([vadr[f"{leg}_{WHEEL_JOINT}"] for leg in LEG_NAMES], dtype=int)
+    leg_qadr = np.array([qadr[f"{leg}_{jt}"] for leg in LEG_NAMES for jt in LEG_JOINTS], dtype=int)
+
+    for step in range(total_steps):
+        if step % steps_per_ctrl == 0:
+            cmd = cmd + np.clip(wheel_cmd - cmd, -max_delta, max_delta)
+            for leg in LEG_NAMES:
+                vals = (cand.ab if leg[1] == "l" else -cand.ab, cand.hip, cand.knee)
+                for jt, val in zip(LEG_JOINTS, vals):
+                    robot.data.ctrl[act[f"{leg}_{jt}"]] = val
+            for i, leg in enumerate(LEG_NAMES):
+                robot.data.ctrl[act[f"{leg}_{WHEEL_JOINT}"]] = cmd[i]
+
+        robot.step()
+
+        if step % steps_per_ctrl == 0:
+            st = robot.get_state()
+            roll, pitch = float(st.rpy[0]), float(st.rpy[1])
+            yaw_now = float(st.rpy[2])
+            yaw_unwrapped += wrap_pi(yaw_now - last_yaw)
+            last_yaw = yaw_now
+            body_vx_sum += float(st.rot[:, 0].dot(st.lin_vel))
+            max_roll = max(max_roll, abs(roll))
+            max_pitch = max(max_pitch, abs(pitch))
+            max_tilt = max(max_tilt, math.hypot(roll, pitch))
+            wheel_air = robot.data.xipos[wheel_body_ids, 2] - WHEEL_RADIUS
+            max_wheel_air = max(max_wheel_air, float(np.max(wheel_air)))
+            wheel_err_sum += float(np.mean(np.abs(robot.data.qvel[wheel_vadr] - cmd)))
+            max_leg_err = max(max_leg_err, float(np.max(np.abs(robot.data.qpos[leg_qadr] - leg_target))))
+            samples += 1
+
+    st = robot.get_state()
+    elapsed = max(1e-6, float(st.time - state0.time))
+    world_x_rate = (float(st.pos[0]) - x0) / elapsed
+    x_rate = body_vx_sum / max(1, samples)
+    yaw_rate = yaw_unwrapped / max(1e-6, samples * steps_per_ctrl * sim_dt)
+    x_err = abs(x_rate - trial.vx)
+    yaw_err = abs(yaw_rate - trial.yaw_rate)
+    return {
+        "trial": trial.name,
+        "x_rate": x_rate,
+        "world_x_rate": world_x_rate,
+        "yaw_rate": yaw_rate,
+        "x_err": x_err,
+        "yaw_err": yaw_err,
+        "max_roll_deg": math.degrees(max_roll),
+        "max_pitch_deg": math.degrees(max_pitch),
+        "max_tilt_deg": math.degrees(max_tilt),
+        "max_wheel_air_m": max_wheel_air,
+        "mean_wheel_err": wheel_err_sum / max(1, samples),
+        "max_leg_err": max_leg_err,
+        "track": track,
+        "wheel_cmd": [float(x) for x in wheel_cmd],
+    }
+
+
+def generate_candidates(args: argparse.Namespace) -> list[Candidate]:
+    if args.fixed_hip is not None or args.fixed_knee is not None:
+        if args.fixed_hip is None or args.fixed_knee is None:
+            raise SystemExit("--fixed-hip and --fixed-knee must be provided together")
+        ab = float(args.fixed_ab)
+        hip = float(args.fixed_hip)
+        knee = float(args.fixed_knee)
+        r = get_all(ab, hip, knee)
+        return [
+            Candidate(
+                height=float(r["z"]),
+                ab=ab,
+                hip=hip,
+                knee=knee,
+                static_cost=float(rl_cost(r)),
+                r_hip_x=float(r["r_hip_x_mag"]),
+                cond=float(r["cond"]),
+                max_tau=float(r["max_tau"]),
+            )
+        ]
+
+    cands: list[Candidate] = []
+    h_targets = np.arange(args.height_min, args.height_max + 0.5 * args.height_step, args.height_step)
+    ab_values = [0.0] if args.ab_max <= 1e-9 else np.arange(0.0, args.ab_max + 1e-9, args.ab_step)
+    hip_values = np.arange(args.hip_min, args.hip_max + 0.5 * args.hip_step, args.hip_step)
+    knee_values = np.arange(args.knee_min, args.knee_max + 0.5 * args.knee_step, args.knee_step)
+    for ht in h_targets:
+        bucket: list[Candidate] = []
+        for ab in ab_values:
+            for hip in hip_values:
+                for knee in knee_values:
+                    r = get_all(float(ab), float(hip), float(knee))
+                    if float(r["z"]) < args.height_min or float(r["z"]) > args.height_max:
+                        continue
+                    if abs(float(r["z"]) - float(ht)) > args.height_tol:
+                        continue
+                    if r["wz"] >= r["kz"]:
+                        continue
+                    if abs(r["r_hip_x_mag"]) > args.max_wheel_x:
+                        continue
+                    cost = float(rl_cost(r))
+                    bucket.append(
+                        Candidate(
+                            height=float(r["z"]),
+                            ab=float(ab),
+                            hip=float(hip),
+                            knee=float(knee),
+                            static_cost=cost,
+                            r_hip_x=float(r["r_hip_x_mag"]),
+                            cond=float(r["cond"]),
+                            max_tau=float(r["max_tau"]),
+                        )
+                    )
+        bucket.sort(key=lambda c: c.static_cost)
+        cands.extend(bucket[: args.top_per_height])
+    return cands
+
+
+def score_result(cand: Candidate, trials: list[dict]) -> float:
+    score = 0.08 * cand.static_cost
+    for t in trials:
+        score += 8.0 * t["x_err"]
+        score += 10.0 * t["yaw_err"]
+        score += 0.08 * t["max_tilt_deg"]
+        score += 0.03 * max(0.0, t["max_pitch_deg"] - 8.0) ** 2
+        score += 20.0 * max(0.0, t["max_wheel_air_m"] - 0.015)
+        score += 1.5 * t["mean_wheel_err"]
+        score += 2.0 * max(0.0, t["max_leg_err"] - 0.35)
+    return float(score)
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--height-min", type=float, default=0.15)
+    parser.add_argument("--height-max", type=float, default=0.42)
+    parser.add_argument("--height-step", type=float, default=0.01)
+    parser.add_argument("--height-tol", type=float, default=0.004)
+    parser.add_argument("--top-per-height", type=int, default=1)
+    parser.add_argument("--ab-max", type=float, default=0.0)
+    parser.add_argument("--ab-step", type=float, default=0.04)
+    parser.add_argument("--hip-min", type=float, default=0.25)
+    parser.add_argument("--hip-max", type=float, default=1.05)
+    parser.add_argument("--hip-step", type=float, default=0.025)
+    parser.add_argument("--knee-min", type=float, default=-2.65)
+    parser.add_argument("--knee-max", type=float, default=-0.85)
+    parser.add_argument("--knee-step", type=float, default=0.025)
+    parser.add_argument("--max-wheel-x", type=float, default=0.09)
+    parser.add_argument("--fixed-ab", type=float, default=0.0)
+    parser.add_argument("--fixed-hip", type=float, default=None)
+    parser.add_argument("--fixed-knee", type=float, default=None)
+    parser.add_argument("--duration", type=float, default=4.0)
+    parser.add_argument("--settle", type=float, default=1.5)
+    parser.add_argument("--control-dt", type=float, default=0.02)
+    parser.add_argument("--vx", type=float, default=0.6)
+    parser.add_argument("--yaw", type=float, default=0.3)
+    parser.add_argument("--arc-yaw", type=float, default=0.15)
+    parser.add_argument("--yaw-gain", type=float, default=1.0)
+    parser.add_argument("--wheel-model", choices=("diff", "direct"), default="diff")
+    parser.add_argument("--linear-gain", type=float, default=12.5)
+    parser.add_argument("--direct-yaw-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("--track-source", choices=("model", "fixed"), default="model")
+    parser.add_argument("--track-width", type=float, default=0.394)
+    parser.add_argument(
+        "--wheel-signs",
+        type=float,
+        nargs=4,
+        default=[1.0, 1.0, 1.0, 1.0],
+        metavar=("FL", "FR", "RL", "RR"),
+        help="Per-wheel velocity sign multipliers in joint order.",
+    )
+    parser.add_argument("--quick", action="store_true")
+    parser.add_argument("--json", type=Path, default=None)
+    return parser.parse_args()
+
+
+def main() -> int:
+    args = parse_args()
+    if args.quick:
+        args.height_step = 0.02
+        args.hip_step = 0.05
+        args.knee_step = 0.05
+
+    candidates = generate_candidates(args)
+    if not candidates:
+        raise SystemExit("No candidates found")
+
+    trials = [
+        Trial("forward", args.vx, 0.0, args.duration),
+        Trial("yaw", 0.0, args.yaw, args.duration),
+        Trial("arc", args.vx, args.arc_yaw, args.duration),
+    ]
+    robot = Robot(SCENE_XML)
+    rows = []
+    for i, cand in enumerate(candidates, 1):
+        trial_rows = [run_trial(robot, cand, t, args) for t in trials]
+        rows.append({"candidate": cand.__dict__, "trials": trial_rows, "score": score_result(cand, trial_rows)})
+        if i % 10 == 0:
+            print(f"tested {i}/{len(candidates)}")
+
+    rows.sort(key=lambda r: r["score"])
+    if args.json:
+        args.json.write_text(json.dumps(rows, indent=2), encoding="utf-8")
+
+    print("Top IK postures for differential drive tracking")
+    print("rank score height ab hip knee static xhip cond tau | fwd_x yaw_wz arc_x arc_wz max_tilt max_pitch")
+    for rank, row in enumerate(rows[:10], 1):
+        c = row["candidate"]
+        by = {t["trial"]: t for t in row["trials"]}
+        max_tilt = max(t["max_tilt_deg"] for t in row["trials"])
+        max_pitch = max(t["max_pitch_deg"] for t in row["trials"])
+        print(
+            f"{rank:>2} {row['score']:>7.2f} {c['height']:.3f} {c['ab']:.2f} {c['hip']:.3f} {c['knee']:.3f} "
+            f"{c['static_cost']:.1f} {c['r_hip_x']:.3f} {c['cond']:.2f} {c['max_tau']:.2f} | "
+            f"{by['forward']['x_rate']:.3f} {by['yaw']['yaw_rate']:.3f} {by['arc']['x_rate']:.3f} {by['arc']['yaw_rate']:.3f} "
+            f"{max_tilt:.1f} {max_pitch:.1f}"
+        )
+
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

05_software/train/rc_mjlab/mujoco_sim/mpc.py

@@ -0,0 +1,242 @@
+"""Convex MPC solver for wheeled-legged robot.
+
+Centroidal dynamics: single rigid body model with 4 contact forces.
+State: x = [pos(3), rpy(3), vel(3), omega(3)] = 12
+Input: u = [f1(3), f2(3), f3(3), f4(3)] = 12
+Friction pyramid constraints on each foot.
+
+Reference: MIT Cheetah 3 Convex MPC (Di Carlo et al.)
+"""
+
+import numpy as np
+from scipy import sparse
+from scipy.linalg import block_diag
+import osqp
+
+from config import ROBOT_MASS, LEG_NAMES
+
+# MPC parameters
+MPC_HORIZON = 10       # prediction steps
+MPC_DT = 0.02         # 50 Hz MPC update
+MU = 0.6              # friction coefficient
+FZ_MAX = 200.0        # max vertical force per leg
+FZ_MIN = 10.0         # min vertical force (stance)
+NX = 12               # state dim
+NU = 12               # input dim (4 legs × 3D force)
+
+# Cost weights: [pos_x, pos_y, pos_z, roll, pitch, yaw, vx, vy, vz, wx, wy, wz]
+Q_WEIGHTS = np.array([2.0, 2.0, 50.0, 50.0, 50.0, 10.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0])
+R_WEIGHTS = np.array([1e-6] * 12)
+
+
+def _skew(v):
+    return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
+
+
+class ConvexMPC:
+    """Convex MPC: solves QP for optimal ground reaction forces."""
+
+    def __init__(self, mass=ROBOT_MASS, inertia=None):
+        self.mass = mass
+        # Approximate body inertia (diagonal, world-aligned)
+        if inertia is None:
+            self.I_body = np.diag([0.07, 0.26, 0.24])
+        else:
+            self.I_body = np.array(inertia).reshape(3, 3)
+
+        self.N = MPC_HORIZON
+        self.dt = MPC_DT
+        self.Q = np.diag(Q_WEIGHTS)
+        self.R = np.diag(R_WEIGHTS)
+        self.gravity = np.array([0, 0, -9.81])
+
+        self._last_forces = np.zeros(NU)
+
+    def solve(self, x0, x_ref, foot_positions, contact_schedule):
+        """Solve MPC QP.
+
+        Args:
+            x0: (12,) current state [pos, rpy, vel, omega]
+            x_ref: (12, N) reference trajectory over horizon
+            foot_positions: (4, 3) foot positions in world frame (relative to CoM)
+            contact_schedule: (4, N) binary contact table (1=stance)
+
+        Returns:
+            forces: (12,) optimal forces for current timestep [f1x,f1y,f1z,...,f4x,f4y,f4z]
+        """
+        N = self.N
+
+        # Build dynamics matrices
+        Ad, Bd_list, gd = self._discretize_dynamics(x0, foot_positions)
+
+        # Build QP: min 0.5 z'Hz + f'z  s.t. lb <= Az <= ub, lbx <= z <= ubx
+        # Decision variables: z = [x1,...,xN, u0,...,uN-1]
+        nvars = N * NX + N * NU
+
+        # --- Hessian ---
+        H_diag = np.concatenate([np.tile(2 * Q_WEIGHTS, N), np.tile(2 * R_WEIGHTS, N)])
+        H = sparse.diags(H_diag, format='csc')
+
+        # --- Gradient ---
+        g = np.zeros(nvars)
+        for k in range(N):
+            g[k*NX:(k+1)*NX] = -2 * self.Q @ x_ref[:, k]
+
+        # --- Dynamics equality constraints ---
+        # x_{k+1} = Ad @ x_k + Bd_k @ u_k + gd
+        # Rewrite: x_{k+1} - Ad @ x_k - Bd_k @ u_k = gd (for k>0)
+        #           x_1 - Bd_0 @ u_0 = Ad @ x0 + gd (for k=0)
+        n_eq = N * NX
+        A_eq = np.zeros((n_eq, nvars))
+        b_eq = np.zeros(n_eq)
+
+        # k=0: x_1 = Ad @ x0 + Bd_0 @ u_0 + gd
+        A_eq[0:NX, 0:NX] = np.eye(NX)  # x_1
+        A_eq[0:NX, N*NX:N*NX+NU] = -Bd_list[0]  # -Bd_0 @ u_0
+        b_eq[0:NX] = Ad @ x0 + gd
+
+        for k in range(1, N):
+            row = k * NX
+            # x_{k+1}
+            A_eq[row:row+NX, k*NX:(k+1)*NX] = np.eye(NX)
+            # -Ad @ x_k
+            A_eq[row:row+NX, (k-1)*NX:k*NX] = -Ad
+            # -Bd_k @ u_k
+            A_eq[row:row+NX, N*NX+k*NU:N*NX+(k+1)*NU] = -Bd_list[k]
+            b_eq[row:row+NX] = gd
+
+        # --- Friction pyramid inequality constraints ---
+        # For each stance leg at each timestep: 4 faces
+        # fx - mu*fz <= 0, -fx - mu*fz <= 0, fy - mu*fz <= 0, -fy - mu*fz <= 0
+        n_ineq_max = 4 * 4 * N
+        A_ineq = np.zeros((n_ineq_max, nvars))
+        u_ineq = np.zeros(n_ineq_max)
+
+        row = 0
+        for k in range(N):
+            u_base = N * NX + k * NU
+            for leg in range(4):
+                if contact_schedule[leg, k] == 1:
+                    fx_idx = u_base + leg * 3
+                    fy_idx = u_base + leg * 3 + 1
+                    fz_idx = u_base + leg * 3 + 2
+
+                    # Friction pyramid: stance leg
+                    A_ineq[row, fx_idx] = 1.0
+                    A_ineq[row, fz_idx] = -MU
+                    row += 1
+                    A_ineq[row, fx_idx] = -1.0
+                    A_ineq[row, fz_idx] = -MU
+                    row += 1
+                    A_ineq[row, fy_idx] = 1.0
+                    A_ineq[row, fz_idx] = -MU
+                    row += 1
+                    A_ineq[row, fy_idx] = -1.0
+                    A_ineq[row, fz_idx] = -MU
+                    row += 1
+
+        A_ineq = A_ineq[:row]
+        u_ineq = u_ineq[:row]
+
+        # Stack constraints
+        A_full = np.vstack([A_eq, A_ineq])
+        l_full = np.concatenate([b_eq, -np.inf * np.ones(row)])
+        u_full = np.concatenate([b_eq, u_ineq])
+
+        # --- Box constraints on forces (as identity rows in A) ---
+        A_box = np.zeros((N * NU, nvars))
+        l_box = -np.inf * np.ones(N * NU)
+        u_box = np.inf * np.ones(N * NU)
+
+        for k in range(N):
+            u_base = N * NX + k * NU
+            for leg in range(4):
+                idx = u_base + leg * 3
+                box_row = k * NU + leg * 3
+                # Identity rows for fx, fy, fz
+                for j in range(3):
+                    A_box[box_row + j, idx + j] = 1.0
+
+                if contact_schedule[leg, k] == 1:
+                    # Stance: fz bounded
+                    l_box[box_row + 2] = FZ_MIN
+                    u_box[box_row + 2] = FZ_MAX
+                else:
+                    # Swing: all forces = 0
+                    l_box[box_row:box_row+3] = 0.0
+                    u_box[box_row:box_row+3] = 0.0
+
+        # Final constraint matrix
+        A_full = np.vstack([A_full, A_box])
+        l_full = np.concatenate([l_full, l_box])
+        u_full = np.concatenate([u_full, u_box])
+
+        # --- Solve with OSQP ---
+        A_sparse = sparse.csc_matrix(A_full)
+        H_sparse = sparse.triu(H, format='csc')
+
+        solver = osqp.OSQP()
+        solver.setup(H_sparse, g, A_sparse, l_full, u_full,
+                     eps_abs=1e-4, eps_rel=1e-4,
+                     max_iter=500, polish=True, verbose=False,
+                     warm_start=True)
+
+        # Warm start with previous solution
+        if self._last_forces is not None:
+            x_warm = np.zeros(nvars)
+            x_warm[N*NX:N*NX+NU] = self._last_forces
+            solver.warm_start(x=x_warm)
+
+        result = solver.solve()
+
+        if result.info.status == 'solved' or result.info.status == 'solved_inaccurate':
+            # Extract first timestep forces
+            forces = result.x[N*NX:N*NX+NU]
+            self._last_forces = forces.copy()
+        else:
+            forces = self._last_forces
+
+        return forces
+
+    def _discretize_dynamics(self, x0, foot_positions):
+        """Build discrete-time centroidal dynamics.
+
+        State: [pos, rpy, vel, omega] (12)
+        Continuous: dx/dt = Ac @ x + Bc @ u + gc
+        Discrete: x_{k+1} = Ad @ x + Bd @ u + gd
+        """
+        m = self.mass
+        I_inv = np.linalg.inv(self.I_body)
+        dt = self.dt
+        yaw = x0[5]
+        cy, sy = np.cos(yaw), np.sin(yaw)
+
+        # Rotation for rpy rate ≈ R_z^T @ omega
+        R_zT = np.array([[cy, sy, 0], [-sy, cy, 0], [0, 0, 1]])
+
+        # Ac (12×12)
+        Ac = np.zeros((NX, NX))
+        Ac[0:3, 6:9] = np.eye(3)       # pos_dot = vel
+        Ac[3:6, 9:12] = R_zT           # rpy_dot ≈ R_z^T @ omega
+
+        # Ad = I + Ac*dt (first-order)
+        Ad = np.eye(NX) + Ac * dt
+
+        # Bc varies per timestep (foot positions change contact point)
+        Bd_list = []
+        for k in range(self.N):
+            Bc = np.zeros((NX, NU))
+            for leg in range(4):
+                r = foot_positions[leg]
+                # vel_dot += f/m
+                Bc[6:9, leg*3:(leg+1)*3] = np.eye(3) / m
+                # omega_dot += I^{-1} @ (r × f)
+                Bc[9:12, leg*3:(leg+1)*3] = I_inv @ _skew(r)
+            Bd = Bc * dt
+            Bd_list.append(Bd)
+
+        # Gravity contribution
+        gd = np.zeros(NX)
+        gd[6:9] = self.gravity * dt  # vel += g*dt
+
+        return Ad, Bd_list, gd

05_software/train/rc_mjlab/mujoco_sim/mpc_controller.py

@@ -0,0 +1,261 @@
+"""MPC controller integration for wheeled-legged robot.
+
+Integrates: gait scheduler + reference trajectory + ConvexMPC solver +
+            swing leg control + stance force mapping + wheel drive.
+
+Architecture (following go2-convex-mpc):
+  - MPC runs at ~50 Hz (every MPC_DECIMATION control steps)
+  - Swing/stance leg controller runs at control rate (50 Hz)
+  - Wheel drive: stance legs use differential drive, swing legs coast
+"""
+
+import numpy as np
+from robot import Robot, RobotState
+from dynamics import Dynamics
+from mpc import ConvexMPC, MPC_DT
+from config import (
+    LEG_NAMES, WHEEL_RADIUS, WHEEL_TRACK, WHEEL_VEL_MAX,
+    CTRL_DT, GAIT_FREQ, GAIT_DUTY, SWING_HEIGHT, PHASE_OFFSETS,
+    DEFAULT_JOINT_ANGLES, ROBOT_MASS,
+)
+
+# MPC update decimation (relative to control loop)
+MPC_DECIMATION = max(1, int(MPC_DT / CTRL_DT))  # 1 step at 50Hz
+
+
+class MPCController:
+    """Convex MPC locomotion controller for wheeled-legged robot."""
+
+    def __init__(self, robot: Robot):
+        self.robot = robot
+        self.dynamics = Dynamics()
+        self.mpc = ConvexMPC(mass=ROBOT_MASS)
+
+        # User commands
+        self.vel_x = 0.0
+        self.vel_y = 0.0
+        self.yaw_rate = 0.0
+        self.height = 0.35  # actual standing height with default joint angles
+
+        # Gait state - start at phase 0 with all legs in stance (duty=0.6)
+        self._gait_phase = 0.0
+        self._step_count = 0
+        self._initialized = False
+
+        # MPC solution cache - initialize with gravity compensation
+        self._mpc_forces = np.zeros(12)
+        self._init_gravity_comp()
+
+        # Swing trajectory state
+        self._swing_start_foot = {leg: np.zeros(3) for leg in LEG_NAMES}
+        self._swing_start_time = {leg: 0.0 for leg in LEG_NAMES}
+        self._last_contact = {leg: True for leg in LEG_NAMES}
+
+        # Smoothed commands
+        self._smooth_vx = 0.0
+        self._smooth_vy = 0.0
+        self._smooth_yaw = 0.0
+
+    def _init_gravity_comp(self):
+        """Pre-fill MPC forces with static gravity compensation."""
+        fz_per_leg = ROBOT_MASS * 9.81 / 4.0
+        for i in range(4):
+            self._mpc_forces[i*3 + 2] = fz_per_leg
+
+    def compute(self, state: RobotState, dt: float):
+        """Main MPC control loop.
+
+        Uses MIT motor protocol: tau = kp*(q_des-q) + kd*(dq_des-dq) + tau_ff
+        where tau_ff comes from MPC force mapping via Jacobian transpose.
+
+        Returns:
+            tau_legs: (12,) feedforward torques for MIT mode
+            wheel_targets: (4,) wheel velocity targets
+        """
+        # Smooth commands
+        alpha = min(dt * 3.0, 1.0)
+        self._smooth_vx += alpha * (self.vel_x - self._smooth_vx)
+        self._smooth_vy += alpha * (self.vel_y - self._smooth_vy)
+        self._smooth_yaw += alpha * (self.yaw_rate - self._smooth_yaw)
+
+        # Update Pinocchio
+        q_pin, dq_pin = self.robot.get_qpos_qvel_for_pinocchio()
+        self.dynamics.update(q_pin, dq_pin)
+
+        # Initialize foot positions on first call
+        if not self._initialized:
+            for leg in LEG_NAMES:
+                self._swing_start_foot[leg] = self.dynamics.get_foot_pos(leg)
+            self._initialized = True
+
+        # Decide if we should trot or just stand
+        moving = (abs(self._smooth_vx) > 0.02 or
+                  abs(self._smooth_vy) > 0.02 or
+                  abs(self._smooth_yaw) > 0.05)
+
+        if moving:
+            self._gait_phase = (self._gait_phase + dt * GAIT_FREQ) % 1.0
+        else:
+            self._gait_phase = 0.0  # all legs in stance
+
+        # Contact schedule
+        contacts = {}
+        for leg in LEG_NAMES:
+            phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
+            contacts[leg] = phase < GAIT_DUTY
+
+        # Get foot positions relative to CoM
+        foot_positions = np.zeros((4, 3))
+        for i, leg in enumerate(LEG_NAMES):
+            foot_positions[i] = self.dynamics.get_foot_pos(leg) - state.pos
+
+        # --- Run MPC at lower rate ---
+        if self._step_count % MPC_DECIMATION == 0:
+            x0 = self._build_state_vector(state)
+            x_ref = self._build_reference(state)
+            contact_table = self._build_contact_table()
+            self._mpc_forces = self.mpc.solve(x0, x_ref, foot_positions, contact_table)
+
+        self._step_count += 1
+
+        # --- Compute feedforward torques and desired joint positions ---
+        tau_ff = np.zeros(12)
+        q_des = np.zeros(12)
+        dq_des = np.zeros(12)
+        kp = np.zeros(12)
+        kd = np.zeros(12)
+        wheel_targets = np.zeros(4)
+
+        for i, leg in enumerate(LEG_NAMES):
+            if contacts[leg]:
+                # Stance: MPC force → feedforward torque, PD holds posture
+                f_leg = self._mpc_forces[i*3:(i+1)*3]
+                J = self.dynamics.get_foot_jacobian_leg(leg)
+                tau_ff[i*3:(i+1)*3] = J.T @ (-f_leg)
+
+                # PD target: default standing angles (posture hold)
+                q_des[i*3] = DEFAULT_JOINT_ANGLES["hip_abduction"]
+                q_des[i*3+1] = DEFAULT_JOINT_ANGLES["hip_pitch"]
+                q_des[i*3+2] = DEFAULT_JOINT_ANGLES["knee"]
+                kp[i*3:(i+1)*3] = [40.0, 40.0, 40.0]
+                kd[i*3:(i+1)*3] = [3.0, 3.0, 3.0]
+
+                # Record foot position
+                self._swing_start_foot[leg] = self.dynamics.get_foot_pos(leg)
+                self._last_contact[leg] = True
+
+                # Wheel drive
+                wheel_targets[i] = self._wheel_cmd(leg)
+            else:
+                # Swing: IK target position, strong PD, no feedforward
+                if self._last_contact[leg]:
+                    self._swing_start_foot[leg] = self.dynamics.get_foot_pos(leg)
+                    self._swing_start_time[leg] = state.time
+                    self._last_contact[leg] = False
+
+                q_ik = self._swing_leg_ik(leg, state, q_pin)
+                q_des[i*3:(i+1)*3] = q_ik
+                kp[i*3:(i+1)*3] = [60.0, 60.0, 60.0]  # strong PD for swing
+                kd[i*3:(i+1)*3] = [3.0, 3.0, 3.0]
+                # tau_ff stays 0 for swing
+
+                wheel_targets[i] = 0.0
+
+        # Use MIT protocol via robot interface
+        self.robot.set_ctrl_mit(q_des, dq_des, kp, kd, tau_ff, wheel_targets)
+        # Return dummy (actual ctrl is set directly above)
+        return None, None
+
+    def _build_state_vector(self, state: RobotState):
+        """Build MPC state: [pos, rpy, vel, omega]."""
+        return np.concatenate([state.pos, state.rpy, state.lin_vel, state.ang_vel])
+
+    def _build_reference(self, state: RobotState):
+        """Build reference trajectory over MPC horizon."""
+        N = self.mpc.N
+        x_ref = np.zeros((12, N))
+
+        yaw = state.rpy[2]
+        cy, sy = np.cos(yaw), np.sin(yaw)
+        R_z = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
+        vel_world = R_z @ np.array([self._smooth_vx, self._smooth_vy, 0.0])
+
+        for k in range(N):
+            t = (k + 1) * self.mpc.dt
+            # Position: integrate from current
+            x_ref[0, k] = state.pos[0] + vel_world[0] * t
+            x_ref[1, k] = state.pos[1] + vel_world[1] * t
+            x_ref[2, k] = self.height
+            # RPY: keep roll/pitch zero, integrate yaw
+            x_ref[3, k] = 0.0
+            x_ref[4, k] = 0.0
+            x_ref[5, k] = yaw + self._smooth_yaw * t
+            # Velocity
+            x_ref[6, k] = vel_world[0]
+            x_ref[7, k] = vel_world[1]
+            x_ref[8, k] = 0.0
+            # Angular velocity
+            x_ref[9, k] = 0.0
+            x_ref[10, k] = 0.0
+            x_ref[11, k] = self._smooth_yaw
+
+        return x_ref
+
+    def _build_contact_table(self):
+        """Build contact schedule over MPC horizon."""
+        N = self.mpc.N
+        table = np.zeros((4, N), dtype=int)
+        for k in range(N):
+            future_phase = (self._gait_phase + (k + 1) * self.mpc.dt * GAIT_FREQ) % 1.0
+            for i, leg in enumerate(LEG_NAMES):
+                leg_phase = (future_phase + PHASE_OFFSETS[leg]) % 1.0
+                table[i, k] = 1 if leg_phase < GAIT_DUTY else 0
+        return table
+
+    def _swing_leg_ik(self, leg: str, state: RobotState, q_pin: np.ndarray):
+        """Swing leg: compute IK target joint angles for trajectory."""
+        swing_phase = self._get_swing_phase(leg)
+
+        p_start = self._swing_start_foot[leg]
+        p_end = self._compute_touchdown(leg, state)
+
+        s = swing_phase
+        s_mj = 10*s**3 - 15*s**4 + 6*s**5
+
+        pos_des = p_start + (p_end - p_start) * s_mj
+        # Z lift
+        z_lift = 64.0 * s**3 * (1.0 - s)**3
+        pos_des[2] = p_start[2] + SWING_HEIGHT * z_lift
+
+        # IK to get joint angles
+        q_ik = self.dynamics.inverse_kinematics(leg, pos_des, q_pin)
+        return q_ik
+
+    def _get_swing_phase(self, leg: str) -> float:
+        phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
+        if phase < GAIT_DUTY:
+            return 0.0
+        return (phase - GAIT_DUTY) / (1.0 - GAIT_DUTY)
+
+    def _compute_touchdown(self, leg: str, state: RobotState) -> np.ndarray:
+        """Raibert heuristic for touchdown position."""
+        td = self._swing_start_foot[leg].copy()
+        t_stance = GAIT_DUTY / GAIT_FREQ
+
+        yaw = state.rpy[2]
+        cy, sy = np.cos(yaw), np.sin(yaw)
+        R_z = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
+        cmd_vel_world = R_z @ np.array([self._smooth_vx, self._smooth_vy, 0.0])
+
+        td[0] += cmd_vel_world[0] * t_stance * 0.5
+        td[1] += cmd_vel_world[1] * t_stance * 0.5
+        td[2] = WHEEL_RADIUS
+        return td
+
+    def _wheel_cmd(self, leg: str) -> float:
+        """Differential drive for a single wheel."""
+        if leg[1] == "l":
+            v = (self._smooth_vx - 0.5 * WHEEL_TRACK * self._smooth_yaw) / WHEEL_RADIUS
+        else:
+            v = (self._smooth_vx + 0.5 * WHEEL_TRACK * self._smooth_yaw) / WHEEL_RADIUS
+        return np.clip(v, -WHEEL_VEL_MAX, WHEEL_VEL_MAX)

05_software/train/rc_mjlab/mujoco_sim/posture_optimizer.py

@@ -0,0 +1,367 @@
+"""Posture optimizer for wheeled-leg standing/crawl height table.
+
+The table is not a pure "minimum average torque" table. For crawl and low-bar
+traversal, the wheel center should not be far from the hip/leg in sagittal X,
+otherwise the robot is no longer really using the wheel as the support/drive
+point. This is a soft guardrail, not a strict x=0 constraint. The score combines:
+
+1. wheel center X offset from hip
+2. peak single-motor holding torque
+3. RMS torque, used as a proxy for I^2R heating
+
+Usage:
+    python posture_optimizer.py              # MuJoCo sweep
+    python posture_optimizer.py --quick      # coarse MuJoCo sweep
+    python posture_optimizer.py --analyze    # analytical-only sweep
+"""
+
+import argparse
+import math
+import time
+from pathlib import Path
+
+import numpy as np
+
+REPO_ROOT = Path(__file__).resolve().parents[1]
+
+try:
+    import mujoco
+except ImportError:
+    mujoco = None
+
+SCENE_XML = REPO_ROOT / "mjcf" / "scene.xml"
+WHEEL_RADIUS = 0.10
+HIP_Z_OFFSET = 0.054
+L1, L2 = 0.25, 0.20
+ROBOT_MASS = 12.3
+G = 9.81
+F_PER_LEG = ROBOT_MASS * G / 4.0
+MAX_TORQUE = 17.0
+
+LEG_NAMES = ("fl", "fr", "rl", "rr")
+LEG_JOINTS = ("hip_abduction_joint", "hip_pitch_joint", "knee_joint")
+
+# 0.15m is not a good default table target for ab=0.0. 0.17m is the practical
+# default crawl height, while lower crawl can be evaluated with abduction.
+KNEE_MIN = -2.65
+HEIGHT_MIN = 0.17
+HEIGHT_MAX = 0.46
+SOFT_WHEEL_X_OFFSET = 0.05
+HARD_WHEEL_X_OFFSET = 0.08
+
+_ACTUATOR_NAMES = [f"{leg}_{jt}" for leg in LEG_NAMES for jt in LEG_JOINTS]
+
+
+def compute_fk(hip, knee):
+    """Return wheel-center x offset and base height from (hip_pitch, knee)."""
+    x = L1 * math.sin(hip) + L2 * math.sin(hip + knee)
+    z = L1 * math.cos(hip) + L2 * math.cos(hip + knee)
+    base_height = WHEEL_RADIUS + z - HIP_Z_OFFSET
+    return x, base_height
+
+
+def posture_cost(x_foot, torques):
+    """Score one posture by support geometry, peak torque, and RMS torque."""
+    tau = np.asarray(torques, dtype=float)
+    peak_torque = float(np.max(np.abs(tau)))
+    rms_torque = float(np.sqrt(np.mean(np.square(tau))))
+    mean_i2r = float(np.mean(np.square(tau)))
+    x_penalty = max(0.0, abs(x_foot) - SOFT_WHEEL_X_OFFSET)
+    cost = (
+        0.5 * (abs(x_foot) / SOFT_WHEEL_X_OFFSET) ** 2
+        + 8.0 * (x_penalty / max(1e-6, HARD_WHEEL_X_OFFSET - SOFT_WHEEL_X_OFFSET)) ** 2
+        + 3.0 * (peak_torque / MAX_TORQUE) ** 2
+        + (rms_torque / MAX_TORQUE) ** 2
+    )
+    return cost, peak_torque, rms_torque, mean_i2r
+
+
+def analyze_analytical():
+    """Analytical sweep using static GRF moments."""
+    hip_range = np.arange(0.0, 1.6, 0.002)
+    knee_range = np.arange(KNEE_MIN, -0.4, 0.002)
+
+    results = []
+    for hip in hip_range:
+        for knee in knee_range:
+            x_foot, height = compute_fk(hip, knee)
+            if height < HEIGHT_MIN or height > HEIGHT_MAX:
+                continue
+
+            tau_hip = F_PER_LEG * x_foot
+            x_knee_to_foot = L2 * math.sin(hip + knee)
+            tau_knee = F_PER_LEG * x_knee_to_foot
+            tau_abduction = 0.0
+            cost, peak, rms, mean_i2r = posture_cost(
+                x_foot, (tau_abduction, tau_hip, tau_knee))
+
+            results.append({
+                "hip": float(hip),
+                "knee": float(knee),
+                "height": float(height),
+                "x_foot": float(x_foot),
+                "tau_abd": tau_abduction,
+                "tau_hip": float(tau_hip),
+                "tau_knee": float(tau_knee),
+                "tau_peak": peak,
+                "tau_rms": rms,
+                "mean_i2r": mean_i2r,
+                "cost": cost,
+            })
+
+    return results
+
+
+def run_mujoco_sweep(quick=False):
+    """MuJoCo sweep measuring actual actuator forces at steady state."""
+    if mujoco is None:
+        raise RuntimeError("mujoco is not installed; use --analyze for analytical mode")
+
+    model = mujoco.MjModel.from_xml_path(str(SCENE_XML))
+    data = mujoco.MjData(model)
+
+    act_ids = {}
+    for name in _ACTUATOR_NAMES:
+        act_ids[name] = mujoco.mj_name2id(
+            model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
+
+    for i in range(model.nu):
+        name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
+        model.actuator_biastype[i] = 1
+        model.actuator_gaintype[i] = 0
+        model.actuator_forcelimited[i] = 0
+        if "wheel" not in name:
+            model.actuator_gainprm[i, 0] = 60.0
+            model.actuator_biasprm[i, 0] = 0.0
+            model.actuator_biasprm[i, 1] = -60.0
+            model.actuator_biasprm[i, 2] = -3.0
+            model.actuator_ctrlrange[i] = [-3.14, 3.14]
+        else:
+            model.actuator_gainprm[i, 0] = 2.0
+            model.actuator_biasprm[i, 0] = 0.0
+            model.actuator_biasprm[i, 1] = 0.0
+            model.actuator_biasprm[i, 2] = -2.0
+            model.actuator_ctrlrange[i] = [-20.0, 20.0]
+
+    jnt_ids = {}
+    for name in _ACTUATOR_NAMES:
+        jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)
+        jnt_ids[name] = model.jnt_qposadr[jid]
+
+    if quick:
+        hip_range = np.arange(0.3, 1.5, 0.10)
+        knee_range = np.arange(KNEE_MIN, -0.6, 0.10)
+    else:
+        hip_range = np.arange(0.0, 1.6, 0.04)
+        knee_range = np.arange(KNEE_MIN, -0.4, 0.04)
+
+    results = []
+    total = 0
+    valid = 0
+
+    for hip in hip_range:
+        for knee in knee_range:
+            total += 1
+            x_foot, height = compute_fk(hip, knee)
+            if height < HEIGHT_MIN or height > HEIGHT_MAX:
+                continue
+
+            mujoco.mj_resetData(model, data)
+
+            for leg in LEG_NAMES:
+                for jt, val in zip(LEG_JOINTS, (0.0, hip, knee)):
+                    name = f"{leg}_{jt}"
+                    data.qpos[jnt_ids[name]] = val
+
+            data.qpos[2] = height
+            data.qpos[3] = 1.0
+            data.qpos[4:7] = 0.0
+
+            for leg in LEG_NAMES:
+                for jt, val in zip(LEG_JOINTS, (0.0, hip, knee)):
+                    name = f"{leg}_{jt}"
+                    data.ctrl[act_ids[name]] = val
+                name_w = f"{leg}_wheel_joint"
+                if name_w in act_ids:
+                    data.ctrl[act_ids[name_w]] = 0.0
+
+            mujoco.mj_forward(model, data)
+
+            for _ in range(500):
+                mujoco.mj_step(model, data)
+
+            roll, pitch, _ = _get_rpy(data, model)
+            if abs(roll) > 0.8 or abs(pitch) > 0.8:
+                continue
+
+            torque_buf = []
+            for _ in range(100):
+                mujoco.mj_step(model, data)
+                torque_buf.append([data.actuator_force[act_ids[name]]
+                                   for name in _ACTUATOR_NAMES])
+            tau_avg = np.array(torque_buf).mean(axis=0)
+
+            tau_hip_val = tau_avg[1]
+            tau_knee_val = tau_avg[2]
+            tau_abd_val = tau_avg[0]
+            cost, peak, rms, mean_i2r = posture_cost(x_foot, tau_avg)
+
+            q_hip_actual = float(data.qpos[jnt_ids["fl_hip_pitch_joint"]])
+            q_knee_actual = float(data.qpos[jnt_ids["fl_knee_joint"]])
+
+            valid += 1
+            results.append({
+                "hip": float(hip),
+                "knee": float(knee),
+                "height": float(f"{height:.4f}"),
+                "x_foot": float(f"{x_foot:.4f}"),
+                "tau_abd": float(f"{tau_abd_val:.4f}"),
+                "tau_hip": float(f"{tau_hip_val:.4f}"),
+                "tau_knee": float(f"{tau_knee_val:.4f}"),
+                "tau_peak": float(f"{peak:.4f}"),
+                "tau_rms": float(f"{rms:.4f}"),
+                "mean_i2r": float(f"{mean_i2r:.4f}"),
+                "cost": float(f"{cost:.4f}"),
+                "q_hip_actual": float(f"{q_hip_actual:.4f}"),
+                "q_knee_actual": float(f"{q_knee_actual:.4f}"),
+            })
+
+            if valid % 20 == 0:
+                print(f"  [{valid}/{total}] hip={hip:.2f} knee={knee:.2f} "
+                      f"h={height:.3f} x={x_foot:+.4f} "
+                      f"peak={peak:.2f} rms={rms:.2f} cost={cost:.4f}")
+
+    return results
+
+
+def _get_rpy(data, model):
+    """Extract roll and pitch from MuJoCo data."""
+    base_bid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "base_link")
+    rot = data.xmat[base_bid].reshape(3, 3)
+    roll = math.atan2(rot[2, 1], rot[2, 2])
+    pitch = math.atan2(-rot[2, 0], math.sqrt(rot[2, 1] ** 2 + rot[2, 2] ** 2))
+    return roll, pitch, 0.0
+
+
+def print_top_results(results, n=10):
+    """Print the top-N results with lowest cost."""
+    sorted_r = sorted(results, key=lambda r: r["cost"])
+
+    print(f"\n{'=' * 104}")
+    print(f"TOP {n} CONFIGURATIONS (soft wheel-X + peak/RMS torque score)")
+    print(f"{'=' * 104}")
+    print(f"{'Rank':>4} {'hip':>6} {'knee':>7} {'height':>7} {'x_foot':>8} "
+          f"{'tau_abd':>8} {'tau_hip':>8} {'tau_knee':>8} "
+          f"{'peak':>8} {'rms':>8} {'cost':>9}")
+    print(f"{'-' * 104}")
+
+    for i, r in enumerate(sorted_r[:n]):
+        print(f"{i + 1:>4} {r['hip']:>6.3f} {r['knee']:>7.3f} "
+              f"{r['height']:>7.3f} {r.get('x_foot', 0):>8.4f} "
+              f"{r.get('tau_abd', 0):>8.3f} {r['tau_hip']:>8.3f} "
+              f"{r['tau_knee']:>8.3f} {r.get('tau_peak', 0):>8.3f} "
+              f"{r.get('tau_rms', 0):>8.3f} {r['cost']:>9.4f}")
+
+    best = sorted_r[0]
+    print(f"\nBEST: hip={best['hip']:.3f} knee={best['knee']:.3f} "
+          f"z={best['height']:.3f}m x={best.get('x_foot', 0):+.4f}m "
+          f"peak={best.get('tau_peak', 0):.3f}Nm "
+          f"rms={best.get('tau_rms', 0):.3f}Nm cost={best['cost']:.4f}\n")
+
+    return sorted_r
+
+
+def compute_height_table(results):
+    """Build height-to-angle lookup with soft wheel-X support guardrail."""
+    sorted_r = sorted(
+        (r for r in results if abs(r.get("x_foot", 999.0)) <= HARD_WHEEL_X_OFFSET),
+        key=lambda r: r["height"],
+    )
+    if not sorted_r:
+        raise RuntimeError("No candidates satisfy HARD_WHEEL_X_OFFSET")
+
+    h_range = np.arange(0.17, 0.46, 0.02)
+    table_h, table_hip, table_knee = [], [], []
+
+    for h_target in h_range:
+        candidates = [(r, abs(r["height"] - h_target)) for r in sorted_r]
+        candidates.sort(key=lambda x: (x[1], x[0]["cost"]))
+        best = candidates[0][0]
+        table_h.append(best["height"])
+        table_hip.append(best["hip"])
+        table_knee.append(best["knee"])
+
+    return {
+        "height": [round(h, 3) for h in table_h],
+        "hip": [round(h, 3) for h in table_hip],
+        "knee": [round(k, 3) for k in table_knee],
+    }
+
+
+def export_calibrated_table(table):
+    """Print the new height table in copy-paste format."""
+    print(f"\n{'=' * 80}")
+    print("CALIBRATED HEIGHT TABLE (soft wheel-X support guardrail)")
+    print(f"{'=' * 80}")
+    print(f"_H = {table['height']}")
+    print(f"_HIP = {table['hip']}")
+    print(f"_KNEE = {table['knee']}")
+    print(f"{'=' * 80}\n")
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Find wheeled-leg posture table")
+    parser.add_argument("--quick", action="store_true", help="Coarse MuJoCo sweep")
+    parser.add_argument("--analyze", action="store_true", help="Analytical only")
+    parser.add_argument("--mujoco", action="store_true", default=True,
+                        help="Run MuJoCo simulation when available")
+    args = parser.parse_args()
+
+    print("=" * 72)
+    print("WHEELED-LEG POSTURE OPTIMIZER")
+    print("=" * 72)
+    print(f"Robot mass: {ROBOT_MASS} kg, F_per_leg: {F_PER_LEG:.1f} N")
+    print(f"L1={L1}m, L2={L2}m, wheel_r={WHEEL_RADIUS}m")
+    print(f"Height range: [{HEIGHT_MIN}, {HEIGHT_MAX}] m")
+    print(f"Knee min hard limit: {KNEE_MIN} rad")
+    print(f"Soft wheel X offset: {SOFT_WHEEL_X_OFFSET} m")
+    print(f"Hard wheel X offset: {HARD_WHEEL_X_OFFSET} m\n")
+
+    t0 = time.time()
+    if args.analyze or mujoco is None:
+        print("[Analytical mode]")
+        results = analyze_analytical()
+    else:
+        print("[MuJoCo simulation mode]")
+        results = run_mujoco_sweep(quick=args.quick)
+
+    elapsed = time.time() - t0
+    print(f"Evaluated {len(results)} valid configurations in {elapsed:.1f}s")
+    if not results:
+        print("No valid configurations found")
+        return
+
+    best_results = print_top_results(results, n=15)
+
+    x_def, h_def = compute_fk(0.666, -1.546)
+    print(f"Current config default: hip=0.666, knee=-1.546 "
+          f"=> z={h_def:.3f}m, x={x_def:+.4f}m")
+
+    table = compute_height_table(best_results)
+    export_calibrated_table(table)
+
+    best = best_results[0]
+    print("=" * 72)
+    print("RECOMMENDED DEFAULT")
+    print("=" * 72)
+    print(f"hip_abduction: 0.0")
+    print(f"hip_pitch:     {best['hip']:.3f}")
+    print(f"knee:          {best['knee']:.3f}")
+    print(f"height:        {best['height']:.3f} m")
+    print(f"x_foot:        {best.get('x_foot', 0):+.4f} m")
+    print(f"tau_peak:      {best.get('tau_peak', 0):.3f} Nm")
+    print(f"tau_rms:       {best.get('tau_rms', 0):.3f} Nm")
+
+
+if __name__ == "__main__":
+    main()

05_software/train/rc_mjlab/mujoco_sim/posture_tool.py

@@ -0,0 +1,189 @@
+"""Fast posture table helper based on wheelleg.xml link offsets.
+
+This tool is useful because it uses the real FL leg offsets from MJCF instead
+of the simplified two-link geometry used by posture_optimizer.py. It is still a
+static single-leg approximation, so use it to choose candidate crawl/standing
+poses, then verify in MuJoCo and on the robot at low speed.
+"""
+
+import math
+
+F_PER_LEG = 12.3 * 9.81 / 4.0
+KNEE_MIN = -2.65
+HIP_MIN = -2.58
+HIP_MAX = 2.58
+AB_MIN = -0.436
+AB_MAX = 0.611
+HARD_WHEEL_X_OFFSET = 0.08
+
+
+def rx(a):
+    c, s = math.cos(a), math.sin(a)
+    return ((1, 0, 0), (0, c, -s), (0, s, c))
+
+
+def ry(a):
+    c, s = math.cos(a), math.sin(a)
+    return ((c, 0, s), (0, 1, 0), (-s, 0, c))
+
+
+def mv(m, v):
+    return [
+        m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2],
+        m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2],
+        m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2],
+    ]
+
+
+def add(a, b):
+    return [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
+
+
+def get_posture(q_ab, q_hip, q_knee):
+    """Return static FK/torque metrics for one FL leg.
+
+    Returns:
+        base_z, tau_ab, tau_hip, tau_knee, i2r_total, knee_z, wheel_z, wheel_x_from_hip
+    """
+    # FL offsets from mjcf/wheelleg.xml.
+    t_ab = [0.32826, 0.066172, 0.053981]
+    t_hip = [0.06389, -0.027344, 0.00010727]
+    t_knee = [0.0, 0.1035, -0.25]
+    t_wheel = [0.0, 0.014699, -0.20011]
+
+    p = mv(ry(q_knee), t_wheel)
+    p = add(p, t_knee)
+    p = mv(ry(q_hip), p)
+    p = add(p, t_hip)
+    p = mv(rx(q_ab), p)
+    p = add(p, t_ab)
+
+    base_z = 0.10 - p[2]
+    wheel_z = p[2]
+
+    knee_pos = mv(ry(q_hip), t_knee)
+    knee_pos = mv(rx(q_ab), knee_pos)
+    knee_z = knee_pos[2] + t_ab[2]
+
+    joint_knee = mv(ry(q_hip), add(t_knee, t_hip))
+    joint_knee = mv(rx(q_ab), joint_knee)
+    joint_knee = add(t_ab, joint_knee)
+
+    joint_hip = mv(rx(q_ab), t_hip)
+    joint_hip = add(t_ab, joint_hip)
+
+    r_knee = [p[0] - joint_knee[0], p[1] - joint_knee[1], p[2] - joint_knee[2]]
+    r_hip = [p[0] - joint_hip[0], p[1] - joint_hip[1], p[2] - joint_hip[2]]
+    r_ab = [p[0] - t_ab[0], p[1] - t_ab[1], p[2] - t_ab[2]]
+
+    tau_ab = r_ab[1] * F_PER_LEG
+    tau_hip = -r_hip[0] * F_PER_LEG
+    tau_knee = -r_knee[0] * F_PER_LEG
+    i2r_total = tau_ab * tau_ab + tau_hip * tau_hip + tau_knee * tau_knee
+    wheel_x_from_hip = p[0] - joint_hip[0]
+    return base_z, tau_ab, tau_hip, tau_knee, i2r_total, knee_z, wheel_z, wheel_x_from_hip
+
+
+def score_candidate(z, z_target, tau_ab, tau_hip, tau_knee, wheel_x, x_target=0.0):
+    peak = max(abs(tau_ab), abs(tau_hip), abs(tau_knee))
+    rms = math.sqrt((tau_ab * tau_ab + tau_hip * tau_hip + tau_knee * tau_knee) / 3.0)
+    x_err = wheel_x - x_target
+    x_over = max(0.0, abs(wheel_x) - 0.05)
+    return (
+        3000.0 * (z - z_target) ** 2
+        + 2.5 * (peak / 17.0) ** 2
+        + (rms / 17.0) ** 2
+        + 0.4 * (x_err / 0.05) ** 2
+        + 6.0 * (x_over / 0.03) ** 2
+    ), peak, rms
+
+
+def find_best(z_target, ab_range=(0.0, 0.0), step=0.002, x_target=0.0, hard_wheel_x_offset=HARD_WHEEL_X_OFFSET):
+    """Find one static posture near target height without violating hard limits."""
+    best = None
+    ab0, ab1 = ab_range
+    n_ab = max(1, int(round((ab1 - ab0) / step)) + 1)
+    n_hip = int(round((1.6 - 0.0) / step)) + 1
+    n_knee = int(round((-0.4 - KNEE_MIN) / step)) + 1
+
+    for ia in range(n_ab):
+        ab = ab0 + ia * step
+        if ab < AB_MIN or ab > AB_MAX:
+            continue
+        for ih in range(n_hip):
+            hip = ih * step
+            if hip < HIP_MIN or hip > HIP_MAX:
+                continue
+            for ik in range(n_knee):
+                knee = KNEE_MIN + ik * step
+                z, ta, th, tk, i2r, kz, wz, wx = get_posture(ab, hip, knee)
+                if abs(z - z_target) > 0.0015:
+                    continue
+                if abs(wx) > hard_wheel_x_offset:
+                    continue
+                if wz >= kz:
+                    continue
+                cost, peak, rms = score_candidate(z, z_target, ta, th, tk, wx, x_target=x_target)
+                cand = (cost, ab, hip, knee, z, ta, th, tk, peak, rms, i2r, wx)
+                if best is None or cand[0] < best[0]:
+                    best = cand
+    return best
+
+
+def print_table(
+    z_targets,
+    name,
+    ab_range=(0.0, 0.0),
+    step=0.002,
+    x_target=0.0,
+    hard_wheel_x_offset=HARD_WHEEL_X_OFFSET,
+):
+    print(f"\n{'=' * 96}")
+    print(name)
+    print(f"{'=' * 96}")
+    print(f"{'z':>6} {'ab':>6} {'hip':>7} {'knee':>7} "
+          f"{'tau_ab':>8} {'tau_hip':>8} {'tau_knee':>9} "
+          f"{'peak':>8} {'rms':>8} {'x_hip':>8}")
+    print("-" * 96)
+    for zt in z_targets:
+        best = find_best(
+            zt,
+            ab_range=ab_range,
+            step=step,
+            x_target=x_target,
+            hard_wheel_x_offset=hard_wheel_x_offset,
+        )
+        if best is None:
+            print(f"{zt:>6.3f} no valid config")
+            continue
+        _, ab, hip, knee, z, ta, th, tk, peak, rms, _, wx = best
+        print(f"{z:>6.3f} {ab:>6.3f} {hip:>7.3f} {knee:>7.3f} "
+              f"{ta:>8.3f} {th:>8.3f} {tk:>9.3f} "
+              f"{peak:>8.3f} {rms:>8.3f} {wx:>8.4f}")
+
+
+def print_crawl_default():
+    best = find_best(0.17, ab_range=(0.0, 0.0), step=0.002)
+    if best is None:
+        return
+    _, ab, hip, knee, z, ta, th, tk, peak, rms, _, wx = best
+    print("\nSuggested runtime crawl_default_dof_pos:")
+    print(
+        f"[{ab:.3f}, {hip:.3f}, {knee:.3f}, "
+        f"{ab:.3f}, {hip:.3f}, {knee:.3f}, "
+        f"{ab:.3f}, {hip:.3f}, {knee:.3f}, "
+        f"{ab:.3f}, {hip:.3f}, {knee:.3f}, "
+        "0.0, 0.0, 0.0, 0.0]"
+    )
+    print(f"# z={z:.3f}, peak={peak:.3f}Nm, rms={rms:.3f}Nm, wheel_x_from_hip={wx:+.4f}m")
+
+
+if __name__ == "__main__":
+    print_table([round(0.36 + 0.01 * i, 2) for i in range(10)],
+                "STANDING candidates from MJCF FL geometry", step=0.004)
+    print_table([round(0.17 + 0.01 * i, 2) for i in range(9)],
+                "CRAWL candidates from MJCF FL geometry", step=0.002)
+    print_table([round(0.10 + 0.01 * i, 2) for i in range(6)],
+                "LOW CRAWL candidates, fixed FL abduction = +0.2",
+                ab_range=(0.2, 0.2), step=0.002, hard_wheel_x_offset=0.45)
+    print_crawl_default()

05_software/train/rc_mjlab/mujoco_sim/rl_friendly_opt.py

@@ -0,0 +1,200 @@
+"""RlFriendlyPostureOpt — 结合电机发热 + RL友好度约束的站/爬姿优化
+
+RL友好度约束（从实机经验总结，静力学可计算）：
+  1. 髋关节力臂 ≥ 0.08m   — 不让髋闲置（动态响应差）
+  2. 三电机不均衡 ≤ 1.3x   — 不让单电机先超载
+  3. 运动学条件数 κ ≤ 3.5 — 不让有效传动比过高（放大控制噪声）
+
+使用方法：
+  uv run python mujoco_sim/rl_friendly_opt.py          # 打印推荐
+  uv run python -c "from mujoco_sim.rl_friendly_opt import get_all; print(get_all(0, 0.8, -1.22))"
+
+基于 mjcf/wheelleg.xml 的 FL 腿运动学。
+"""
+import math
+import numpy as np
+
+# ---------------------------------------------------------------------------
+# 运动学常数（来自 wheelleg.xml FL 腿）
+# ---------------------------------------------------------------------------
+T_AB = [0.32826, 0.066172, 0.053981]
+T_HIP = [0.06389, -0.027344, 0.00010727]
+T_KNEE = [0.0, 0.1035, -0.25]
+T_WHEEL = [0.0, 0.014699, -0.20011]
+F_PER_LEG = 12.3 * 9.81 / 4.0
+KNEE_MIN, KNEE_MAX = -2.65, 2.65
+HIP_MIN, HIP_MAX = -2.58, 2.58
+
+# ---------------------------------------------------------------------------
+# 工具函数
+# ---------------------------------------------------------------------------
+def rx(a):
+    c = math.cos(a); s = math.sin(a)
+    return ((1, 0, 0), (0, c, -s), (0, s, c))
+
+def ry(a):
+    c = math.cos(a); s = math.sin(a)
+    return ((c, 0, s), (0, 1, 0), (-s, 0, c))
+
+def mv(m, v):
+    return [m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2],
+            m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2],
+            m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2]]
+
+def add(a, b): return [a[0]+b[0], a[1]+b[1], a[2]+b[2]]
+
+# ---------------------------------------------------------------------------
+# 主函数
+# ---------------------------------------------------------------------------
+def get_all(q_ab, q_hip, q_knee):
+    """FK + 力矩 + 运动学指标。
+
+    返回 dict:
+      z, ab, hip, knee, tau_ab, tau_hip, tau_knee,
+      max_tau, i2r, imbal, cond, r_hip_x_mag, calf_deg
+    """
+    # --- FK ---
+    p = mv(ry(q_knee), T_WHEEL)
+    p = add(p, T_KNEE)
+    p = mv(ry(q_hip), p)
+    p = add(p, T_HIP)
+    p = mv(rx(q_ab), p)
+    wh = add(p, T_AB)
+    base_z = 0.10 - wh[2]
+    wheel_z = wh[2]
+
+    # 膝位置（用于 wheel-below-knee）
+    pk = mv(ry(q_hip), T_KNEE)
+    pk = mv(rx(q_ab), pk)
+    kz = pk[2] + T_AB[2]
+
+    # 关节位置
+    jk = mv(ry(q_hip), add(T_KNEE, T_HIP))
+    jk = mv(rx(q_ab), jk)
+    jk = add(T_AB, jk)
+    jh = mv(rx(q_ab), T_HIP)
+    jh = add(T_AB, jh)
+
+    # 力矩
+    rk = [wh[0]-jk[0], wh[1]-jk[1], wh[2]-jk[2]]
+    rh = [wh[0]-jh[0], wh[1]-jh[1], wh[2]-jh[2]]
+    ra = [wh[0]-T_AB[0], wh[1]-T_AB[1], wh[2]-T_AB[2]]
+    tau_ab = ra[1] * F_PER_LEG
+    tau_hip = -rh[0] * F_PER_LEG
+    tau_knee = -rk[0] * F_PER_LEG
+
+    # --- Jacobian（有限差分） ---
+    eps = 1e-6
+    def foot_rel_hip(h, k):
+        fp = mv(ry(k), T_WHEEL)
+        fp = add(fp, T_KNEE)
+        fp = mv(ry(h), fp)
+        return [fp[0] + T_AB[0] - jh[0], fp[2] + T_AB[2] - jh[2]]
+    fp0 = foot_rel_hip(q_hip, q_knee)
+    fph = foot_rel_hip(q_hip + eps, q_knee)
+    fpk = foot_rel_hip(q_hip, q_knee + eps)
+    J = np.array([
+        [(fph[0]-fp0[0])/eps, (fpk[0]-fp0[0])/eps],
+        [(fph[1]-fp0[1])/eps, (fpk[1]-fp0[1])/eps],
+    ])
+    s = np.linalg.svd(J, compute_uv=False)
+    cond = s[0] / s[-1] if s[-1] > 1e-10 else 999.0
+    min_sv = s[-1]
+
+    # --- 小腿角度（相对铅垂线） ---
+    calf_x = (-0.20011) * math.sin(q_knee)
+    calf_z = (-0.20011) * math.cos(q_knee)
+    cv_x = calf_x * math.cos(q_hip) + calf_z * math.sin(q_hip)
+    cv_z = -calf_x * math.sin(q_hip) + calf_z * math.cos(q_hip)
+    calf_deg = math.degrees(math.atan2(cv_x, -cv_z))
+
+    abs_taus = [abs(tau_ab), abs(tau_hip), abs(tau_knee)]
+    return {
+        'z': base_z, 'ab': q_ab, 'hip': q_hip, 'knee': q_knee,
+        'tau_ab': tau_ab, 'tau_hip': tau_hip, 'tau_knee': tau_knee,
+        'max_tau': max(abs_taus),
+        'i2r': tau_ab**2 + tau_hip**2 + tau_knee**2,
+        'imbal': max(abs_taus) / max(1e-10, min(abs_taus)),
+        'cond': cond, 'min_sv': min_sv,
+        'r_hip_x_mag': abs(rh[0]),
+        'calf_deg': calf_deg,
+        'kz': kz, 'wz': wheel_z,
+    }
+
+
+def rl_cost(r):
+    """RL友好度综合成本（越小越好）。
+
+    约束来源：
+      c1 — 瓶颈电机发热           τ²/τ_max²      主目标
+      c2 — 电机不均衡 > 1.3x      (imbal-1.3)²    单电机先超载
+      c3 — 髋力臂 < 8cm           (0.08 - r_hip)  髋闲置→动态响应差
+      c4 — 有效传动比 κ > 3.5     (κ - 3.5)       高刚度→冲击传递大
+      c5 — 腿的被动刚度 > 1.3x    (stiff-1.3)     刚度比→冲击吸收（新！）
+    """
+    c1 = (r['max_tau'] / 17.0) ** 2
+    c2 = max(0.0, (r['imbal'] - 1.3) / 1.0) ** 2
+    c3 = max(0.0, (0.08 - r['r_hip_x_mag'])) / 0.08
+    c4 = max(0.0, (r['cond'] - 3.5)) / 5.0
+    # stiffness ratio normalized to z=0.40 (σ_min≈0.115)
+    stiff = (0.115 / r['min_sv']) ** 2
+    c5 = max(0.0, (stiff - 1.3)) / 3.0
+    return 100.0*c1 + 50.0*c2 + 80.0*c3 + 30.0*c4 + 40.0*c5
+
+
+def sweep_z(z_targets, name, ab_max=0.44, tol=0.004):
+    """遍历 z 扫描最优姿态。"""
+    print(f"\n{'='*100}")
+    print(f" {name}")
+    print(f"{'='*100}")
+    print(f"{'z_tgt':>6} {'z':>6} {'ab':>5} {'hip':>6} {'knee':>6}  | "
+          f"{'maxτ':>6} {'imbal':>6} {'r_hip':>6} {'κ':>5}  | "
+          f"{'c1热':>6} {'c2均':>6} {'c3髋':>6} {'c4奇':>6} {'cost':>7}")
+    print("-"*100)
+    results = []
+    for zt in z_targets:
+        best_cost = float('inf')
+        best_r = None
+        for ab in [round(i*0.02, 2) for i in range(int(ab_max/0.02)+1)]:
+            for h in [round(i*0.01, 2) for i in range(260)]:
+                for kn in [round(-2.65+i*0.01, 2) for i in range(256)]:
+                    if h + kn > -0.1: continue
+                    r = get_all(ab, h, kn)
+                    if abs(r['z'] - zt) > tol: continue
+                    if r['wz'] >= r['kz']: continue
+                    if r['tau_ab'] < 0 or r['tau_hip'] < 0: continue
+                    cost = rl_cost(r)
+                    if cost < best_cost:
+                        best_cost = cost; best_r = r
+        if best_r:
+            r = best_r
+            c1s = 100.0*(r['max_tau']/17.0)**2
+            c2s = 50.0*max(0.0,(r['imbal']-1.3))**2
+            c3s = 80.0*max(0.0,(0.08-r['r_hip_x_mag']))/0.08
+            c4s = 30.0*max(0.0,(r['cond']-3.5))/5.0
+            print(f"{zt:>6.2f} {r['z']:>6.3f} {r['ab']:>5.2f} {r['hip']:>6.2f} {r['knee']:>6.2f}  | "
+                  f"{r['max_tau']:>6.3f} {r['imbal']:>6.1f}x {r['r_hip_x_mag']:>6.3f} {r['cond']:>5.1f}  | "
+                  f"{c1s:>6.1f} {c2s:>6.1f} {c3s:>6.1f} {c4s:>6.1f} {best_cost:>7.1f}")
+            results.append((zt, r))
+        else:
+            print(f"{zt:>6.2f}  —  no valid")
+    return results
+
+
+if __name__ == "__main__":
+    stand = sweep_z([round(0.36+0.01*i, 2) for i in range(10)],
+                    "STANDING — heat + RL constraints (ab free)", 0.44)
+    crawl = sweep_z([round(0.10+0.01*i, 2) for i in range(9)],
+                    "CRAWL — heat + RL constraints (ab_max=0.25)", 0.25)
+
+    print(f"\n{'='*100}")
+    print(" RECOMMENDATION")
+    print(f"{'='*100}")
+    if stand:
+        zt, r = min(stand, key=lambda x: rl_cost(x[1]))
+        print(f"  Standing: z={r['z']:.3f}  ab={r['ab']:.2f}  hip={r['hip']:.2f}  knee={r['knee']:.2f}")
+        print(f"    maxτ={r['max_tau']:.3f}  imbal={r['imbal']:.1f}x  κ={r['cond']:.1f}  r_hip={r['r_hip_x_mag']:.3f}m")
+    if crawl:
+        zt, r = min(crawl, key=lambda x: rl_cost(x[1]))
+        print(f"  Crawl:    z={r['z']:.3f}  ab={r['ab']:.2f}  hip={r['hip']:.2f}  knee={r['knee']:.2f}")
+        print(f"    maxτ={r['max_tau']:.3f}  imbal={r['imbal']:.1f}x  κ={r['cond']:.1f}  r_hip={r['r_hip_x_mag']:.3f}m")

05_software/train/rc_mjlab/mujoco_sim/robot.py

@@ -0,0 +1,231 @@
+"""MuJoCo interface for the wheeled-legged robot.
+
+Configures actuators as proper PD controllers at runtime:
+  - Leg joints: force = kp*(ctrl - qpos) - kd*qvel, ctrl = target angle
+  - Wheel joints: force = gain*(ctrl - qvel), ctrl = target velocity (rad/s)
+"""
+
+import math
+import numpy as np
+import mujoco
+from dataclasses import dataclass
+from config import (SCENE_XML, LEG_NAMES, LEG_JOINTS, WHEEL_JOINT,
+                    DEFAULT_JOINT_ANGLES, WHEEL_RADIUS, WHEEL_TRACK,
+                    L_THIGH, L_CALF)
+
+
+@dataclass
+class RobotState:
+    """Robot state from MuJoCo."""
+    pos: np.ndarray       # (3,) world position
+    quat: np.ndarray      # (4,) quaternion (w,x,y,z) MuJoCo convention
+    rot: np.ndarray       # (3,3) body→world rotation
+    rpy: np.ndarray       # (3,) roll, pitch, yaw
+    lin_vel: np.ndarray   # (3,) world frame linear velocity
+    ang_vel: np.ndarray   # (3,) body frame angular velocity
+    joint_pos: np.ndarray # (16,) all joint positions [fl3+wheel, fr3+wheel, rl3+wheel, rr3+wheel]
+    joint_vel: np.ndarray # (16,) all joint velocities
+    time: float
+
+
+class Robot:
+    """MuJoCo simulation interface with proper PD actuator configuration."""
+
+    # Leg PD gains (tuned for 12.3kg robot)
+    LEG_KP = 60.0
+    LEG_KD = 3.0
+    # Wheel velocity gain
+    WHEEL_KP = 2.0
+
+    def __init__(self, xml_path=None):
+        self.model = mujoco.MjModel.from_xml_path(str(xml_path or SCENE_XML))
+        self.data = mujoco.MjData(self.model)
+
+        # Cache IDs
+        self._base_bid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "base_link")
+        self._actuator_ids = {}  # name → actuator index
+        self._joint_qpos_adr = {}  # name → qpos address
+        self._joint_qvel_adr = {}  # name → qvel address
+
+        # Build joint/actuator maps
+        self._ctrl_order = []
+        for leg in LEG_NAMES:
+            for jt in (*LEG_JOINTS, WHEEL_JOINT):
+                name = f"{leg}_{jt}"
+                aid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
+                jid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name)
+                self._actuator_ids[name] = aid
+                self._joint_qpos_adr[name] = self.model.jnt_qposadr[jid]
+                self._joint_qvel_adr[name] = self.model.jnt_dofadr[jid]
+                self._ctrl_order.append(name)
+
+        # Configure actuators as proper PD controllers
+        self._configure_actuators()
+
+    def _configure_actuators(self):
+        """Set actuators to proper PD mode.
+
+        Leg joints: force = kp*(ctrl - qpos) - kd*qvel
+        Wheels: force = gain*(ctrl - qvel)  (velocity tracking)
+        """
+        for i in range(self.model.nu):
+            name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
+            self.model.actuator_biastype[i] = 1  # affine bias
+            self.model.actuator_gaintype[i] = 0  # fixed gain
+            self.model.actuator_forcelimited[i] = 0  # no force clamp (17Nm is in actuatorfrcrange)
+
+            if 'wheel' not in name:
+                self.model.actuator_gainprm[i, 0] = self.LEG_KP
+                self.model.actuator_biasprm[i, 0] = 0.0
+                self.model.actuator_biasprm[i, 1] = -self.LEG_KP
+                self.model.actuator_biasprm[i, 2] = -self.LEG_KD
+                self.model.actuator_ctrlrange[i] = [-3.14, 3.14]
+            else:
+                self.model.actuator_gainprm[i, 0] = self.WHEEL_KP
+                self.model.actuator_biasprm[i, 0] = 0.0
+                self.model.actuator_biasprm[i, 1] = 0.0
+                self.model.actuator_biasprm[i, 2] = -self.WHEEL_KP
+                self.model.actuator_ctrlrange[i] = [-20.0, 20.0]
+
+    @property
+    def dt(self):
+        return self.model.opt.timestep
+
+    def reset(self):
+        """Reset to standing pose at correct height for default joint angles."""
+        mujoco.mj_resetData(self.model, self.data)
+
+        # Set default leg angles
+        for leg in LEG_NAMES:
+            for jt, key in zip(LEG_JOINTS, ("hip_abduction", "hip_pitch", "knee")):
+                name = f"{leg}_{jt}"
+                adr = self._joint_qpos_adr[name]
+                self.data.qpos[adr] = DEFAULT_JOINT_ANGLES[key]
+
+        # Compute correct base height from default angles using exact FK
+        # z_base = wheel_radius + L1*cos(hip) + L2*cos(hip+knee) - hip_z_offset
+        hip = DEFAULT_JOINT_ANGLES["hip_pitch"]
+        knee = DEFAULT_JOINT_ANGLES["knee"]
+        base_z = (WHEEL_RADIUS
+                  + L_THIGH * math.cos(hip)
+                  + L_CALF * math.cos(hip + knee)
+                  - 0.054)
+        self.data.qpos[2] = base_z
+        self.data.qpos[3] = 1.0  # quat w
+
+        mujoco.mj_forward(self.model, self.data)
+
+        # Set ctrl to match initial pose (so PD doesn't jerk)
+        for leg in LEG_NAMES:
+            for jt, key in zip(LEG_JOINTS, ("hip_abduction", "hip_pitch", "knee")):
+                name = f"{leg}_{jt}"
+                self.data.ctrl[self._actuator_ids[name]] = DEFAULT_JOINT_ANGLES[key]
+            # Wheels: zero velocity
+            self.data.ctrl[self._actuator_ids[f"{leg}_{WHEEL_JOINT}"]] = 0.0
+
+    def get_state(self) -> RobotState:
+        """Extract robot state."""
+        pos = self.data.xpos[self._base_bid].copy()
+        quat = self.data.xquat[self._base_bid].copy()  # (w,x,y,z)
+        rot = self.data.xmat[self._base_bid].reshape(3, 3).copy()
+
+        rpy = np.array([
+            np.arctan2(rot[2, 1], rot[2, 2]),
+            np.arctan2(-rot[2, 0], np.sqrt(rot[2, 1]**2 + rot[2, 2]**2)),
+            np.arctan2(rot[1, 0], rot[0, 0]),
+        ])
+
+        # Base velocity (world frame)
+        lin_vel = self.data.qvel[0:3].copy()
+        ang_vel = self.data.qvel[3:6].copy()
+
+        # Joint states (16 joints: 4 legs × 4 joints each)
+        joint_pos = np.zeros(16)
+        joint_vel = np.zeros(16)
+        for i, name in enumerate(self._ctrl_order):
+            joint_pos[i] = self.data.qpos[self._joint_qpos_adr[name]]
+            joint_vel[i] = self.data.qvel[self._joint_qvel_adr[name]]
+
+        return RobotState(
+            pos=pos, quat=quat, rot=rot, rpy=rpy,
+            lin_vel=lin_vel, ang_vel=ang_vel,
+            joint_pos=joint_pos, joint_vel=joint_vel,
+            time=self.data.time,
+        )
+
+    def set_ctrl(self, leg_targets: np.ndarray, wheel_targets: np.ndarray):
+        """Set actuator commands (position PD mode).
+
+        Args:
+            leg_targets: (12,) target joint angles for legs [fl3, fr3, rl3, rr3]
+            wheel_targets: (4,) target wheel velocities [fl, fr, rl, rr] in rad/s
+        """
+        for i, leg in enumerate(LEG_NAMES):
+            for j, jt in enumerate(LEG_JOINTS):
+                name = f"{leg}_{jt}"
+                self.data.ctrl[self._actuator_ids[name]] = leg_targets[i * 3 + j]
+            name = f"{leg}_{WHEEL_JOINT}"
+            self.data.ctrl[self._actuator_ids[name]] = wheel_targets[i]
+
+    def set_ctrl_mit(self, q_des: np.ndarray, dq_des: np.ndarray,
+                     kp: np.ndarray, kd: np.ndarray, tau_ff: np.ndarray,
+                     wheel_targets: np.ndarray):
+        """MIT motor protocol: tau = kp*(q_des-q) + kd*(dq_des-dq) + tau_ff.
+
+        Computes torque in software, sends to actuators in torque mode.
+        Call enable_torque_mode() first.
+
+        Args:
+            q_des: (12,) desired joint angles
+            dq_des: (12,) desired joint velocities
+            kp: (12,) position gains (0 for pure torque)
+            kd: (12,) velocity gains
+            tau_ff: (12,) feedforward torques
+            wheel_targets: (4,) wheel velocity targets
+        """
+        for i, leg in enumerate(LEG_NAMES):
+            for j, jt in enumerate(LEG_JOINTS):
+                name = f"{leg}_{jt}"
+                aid = self._actuator_ids[name]
+                idx = i * 3 + j
+                q = self.data.qpos[self._joint_qpos_adr[name]]
+                dq = self.data.qvel[self._joint_qvel_adr[name]]
+                tau = (kp[idx] * (q_des[idx] - q)
+                       + kd[idx] * (dq_des[idx] - dq)
+                       + tau_ff[idx])
+                self.data.ctrl[aid] = np.clip(tau, -17.0, 17.0)
+            name = f"{leg}_{WHEEL_JOINT}"
+            self.data.ctrl[self._actuator_ids[name]] = wheel_targets[i]
+
+    def enable_torque_mode(self):
+        """Switch leg actuators to direct torque mode (for MPC/MIT)."""
+        for i in range(self.model.nu):
+            name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
+            if 'wheel' not in name:
+                self.model.actuator_gainprm[i, 0] = 1.0
+                self.model.actuator_biasprm[i, :3] = [0, 0, 0]
+                self.model.actuator_biastype[i] = 0
+                self.model.actuator_ctrlrange[i] = [-17.0, 17.0]
+
+    def enable_position_mode(self):
+        """Switch leg actuators back to position PD mode."""
+        self._configure_actuators()
+
+    def step(self):
+        """Advance one simulation timestep."""
+        mujoco.mj_step(self.model, self.data)
+
+    def get_qpos_qvel_for_pinocchio(self):
+        """Get full qpos/qvel for Pinocchio (reorder quaternion)."""
+        qpos = self.data.qpos.copy()
+        qvel = self.data.qvel.copy()
+        # MuJoCo quat: (w,x,y,z) → Pinocchio: (x,y,z,w)
+        w, x, y, z = qpos[3], qpos[4], qpos[5], qpos[6]
+        q_pin = np.concatenate([qpos[0:3], [x, y, z, w], qpos[7:]])
+        # MuJoCo vel is already [lin_world(3), ang_body(3), joints(16)]
+        # Pinocchio wants [lin_body(3), ang_body(3), joints(16)]
+        from scipy.spatial.transform import Rotation
+        R = Rotation.from_quat([x, y, z, w]).as_matrix()
+        v_body = R.T @ qvel[0:3]
+        dq_pin = np.concatenate([v_body, qvel[3:]])
+        return q_pin, dq_pin

05_software/train/rc_mjlab/mujoco_sim/run.py

@@ -0,0 +1,111 @@
+"""Main entry point: wheeled-legged robot simulation.
+
+Controls:
+  Mode: wheel (default) - differential drive + posture hold
+        trot - quadruped gait with wheel assist
+        mpc  - convex MPC locomotion (torque control)
+
+  Keyboard (in MuJoCo viewer):
+    W/S: vel_x ±0.1
+    A/D: yaw_rate ±0.2
+    Q/E: height ±0.02
+    1: wheel mode
+    2: trot mode
+    3: MPC mode
+    4: prone toggle
+    Z: reset commands
+"""
+
+import time
+import numpy as np
+import mujoco.viewer as mjv
+
+from robot import Robot
+from controller import Controller
+from gui import GUI
+from config import CTRL_DECIMATION
+
+
+def main():
+    robot = Robot()
+    robot.reset()
+    ctrl = Controller(robot)
+    gui = GUI(ctrl)
+
+    step = 0
+    sim_steps_per_ctrl = CTRL_DECIMATION
+
+    def key_callback(keycode):
+        """Called from MuJoCo render thread - only modify ctrl directly, not tkinter."""
+        try:
+            c = chr(keycode).lower()
+        except (ValueError, OverflowError):
+            return
+        if c == 'w':
+            ctrl.vel_x = min(ctrl.vel_x + 0.1, 1.5)
+        elif c == 's':
+            ctrl.vel_x = max(ctrl.vel_x - 0.1, -1.5)
+        elif c == 'a':
+            ctrl.yaw_rate = min(ctrl.yaw_rate + 0.2, 2.0)
+        elif c == 'd':
+            ctrl.yaw_rate = max(ctrl.yaw_rate - 0.2, -2.0)
+        elif c == 'q':
+            ctrl.height = min(ctrl.height + 0.02, 0.45)
+        elif c == 'e':
+            ctrl.height = max(ctrl.height - 0.02, 0.17)
+        elif c == '1':
+            ctrl.mode = "wheel"; ctrl.prone = False
+        elif c == '2':
+            ctrl.mode = "trot"; ctrl.prone = False
+        elif c == '3':
+            ctrl.mode = "mpc"; ctrl.prone = False
+        elif c == '4':
+            ctrl.prone = not ctrl.prone
+        elif c == 'z':
+            ctrl.vel_x = 0.0; ctrl.vel_y = 0.0; ctrl.yaw_rate = 0.0
+
+    with mjv.launch_passive(robot.model, robot.data, key_callback=key_callback) as viewer:
+        viewer.cam.distance = 2.5
+        viewer.cam.elevation = -20
+        viewer.cam.azimuth = 135
+
+        last_time = robot.data.time
+
+        while viewer.is_running() and not gui.closed:
+            t_start = time.perf_counter()
+
+            # Detect viewer reset (Backspace) - time jumps back to 0
+            if robot.data.time < last_time:
+                robot.reset()
+            last_time = robot.data.time
+
+            # Get state and compute control
+            state = robot.get_state()
+            leg_targets, wheel_targets = ctrl.compute(state, robot.dt * sim_steps_per_ctrl)
+
+            # Apply control and step simulation
+            # MPC mode sets ctrl directly via set_ctrl_mit, skip set_ctrl
+            if ctrl.mode != "mpc":
+                robot.set_ctrl(leg_targets, wheel_targets)
+            for _ in range(sim_steps_per_ctrl):
+                robot.step()
+
+            viewer.sync()
+            step += 1
+
+            # Update GUI every 25 steps (~10 Hz)
+            if step % 25 == 0:
+                state = robot.get_state()
+                gui.update_status(state, step)
+                if not gui.tick():
+                    break
+
+            # Real-time sync
+            elapsed = time.perf_counter() - t_start
+            target_dt = robot.dt * sim_steps_per_ctrl
+            if elapsed < target_dt:
+                time.sleep(target_dt - elapsed)
+
+
+if __name__ == "__main__":
+    main()

05_software/train/rc_mjlab/mujoco_sim/static_posture_optimizer.py

@@ -0,0 +1,528 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+"""MuJoCo static posture optimizer for wheeled-leg standing defaults.
+
+The old posture tools are mostly analytical. This script keeps the fast MJCF
+kinematics as a candidate generator, then evaluates the best candidates in
+MuJoCo with gravity and floor contact enabled.
+
+The score is meant for RL default pose / real deployment:
+  - low peak and RMS standing torque, so one hot motor is not hidden by average
+  - wheel contact point close to the hip in X for wheel speed tracking
+  - COM projection margin inside the four-wheel support rectangle
+  - non-singular leg Jacobian for posture control authority
+  - underbody and knee clearance for obstacle tolerance
+
+Usage:
+    uv run python mujoco_sim/static_posture_optimizer.py
+    uv run python mujoco_sim/static_posture_optimizer.py --quick
+    uv run python mujoco_sim/static_posture_optimizer.py --ab-max 0.08
+"""
+
+from __future__ import annotations
+
+import argparse
+import math
+import sys
+from dataclasses import dataclass
+from pathlib import Path
+
+import mujoco
+import numpy as np
+
+REPO_ROOT = Path(__file__).resolve().parents[1]
+if str(REPO_ROOT) not in sys.path:
+    sys.path.insert(0, str(REPO_ROOT))
+
+from mujoco_sim.rl_friendly_opt import get_all  # noqa: E402
+
+SCENE_XML = REPO_ROOT / "mjcf" / "scene.xml"
+
+LEG_NAMES = ("fl", "fr", "rl", "rr")
+LEG_JOINTS = ("hip_abduction_joint", "hip_pitch_joint", "knee_joint")
+WHEEL_JOINT = "wheel_joint"
+LEG_ACTUATORS = [f"{leg}_{jt}" for leg in LEG_NAMES for jt in LEG_JOINTS]
+
+ROBOT_MASS = 12.3
+G = 9.81
+MAX_TORQUE = 17.0
+WHEEL_RADIUS = 0.10
+
+HIP_MIN, HIP_MAX = -2.58, 2.58
+KNEE_MIN, KNEE_MAX = -2.65, 2.65
+HIP_SCAN = (0.20, 1.15)
+KNEE_SCAN = (-1.90, -0.65)
+
+SOFT_WHEEL_X = 0.045
+HARD_WHEEL_X = 0.085
+MIN_COM_MARGIN = 0.045
+MIN_KNEE_CLEARANCE = 0.105
+MIN_UNDERBODY_CLEARANCE = 0.33
+MAX_COND = 3.7
+
+
+@dataclass(frozen=True)
+class Candidate:
+    ab: float
+    hip: float
+    knee: float
+    z_fk: float
+    analytic_cost: float
+
+
+@dataclass
+class StaticResult:
+    z_target: float
+    z: float
+    ab: float
+    hip: float
+    knee: float
+    cost: float
+    peak_tau: float
+    rms_tau: float
+    mean_i2r: float
+    imbal: float
+    wheel_x: float
+    cond: float
+    min_sv: float
+    com_margin_x: float
+    com_margin_y: float
+    support_margin: float
+    normal_cv: float
+    body_clearance: float
+    knee_clearance: float
+    roll: float
+    pitch: float
+    height_err: float
+
+
+def _configure_actuators(model: mujoco.MjModel) -> None:
+    """Configure leg joints as position PD and wheels as zero-velocity motors."""
+    for i in range(model.nu):
+        name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) or ""
+        model.actuator_biastype[i] = 1
+        model.actuator_gaintype[i] = 0
+        model.actuator_forcelimited[i] = 0
+        if "wheel" in name:
+            model.actuator_gainprm[i, 0] = 2.0
+            model.actuator_biasprm[i, 0] = 0.0
+            model.actuator_biasprm[i, 1] = 0.0
+            model.actuator_biasprm[i, 2] = -2.0
+            model.actuator_ctrlrange[i] = [-20.0, 20.0]
+        else:
+            model.actuator_gainprm[i, 0] = 60.0
+            model.actuator_biasprm[i, 0] = 0.0
+            model.actuator_biasprm[i, 1] = -60.0
+            model.actuator_biasprm[i, 2] = -3.0
+            model.actuator_ctrlrange[i] = [-3.14, 3.14]
+
+
+def _ids(model: mujoco.MjModel):
+    act = {}
+    qadr = {}
+    for leg in LEG_NAMES:
+        for jt in (*LEG_JOINTS, WHEEL_JOINT):
+            name = f"{leg}_{jt}"
+            act[name] = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)
+            jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)
+            qadr[name] = model.jnt_qposadr[jid]
+    bodies = {
+        "base": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "base_link"),
+        **{
+            f"{leg}_wheel": mujoco.mj_name2id(
+                model, mujoco.mjtObj.mjOBJ_BODY, f"{leg}_wheel_Link"
+            )
+            for leg in LEG_NAMES
+        },
+        **{
+            f"{leg}_knee": mujoco.mj_name2id(
+                model, mujoco.mjtObj.mjOBJ_BODY, f"{leg}_knee_Link"
+            )
+            for leg in LEG_NAMES
+        },
+    }
+    return act, qadr, bodies
+
+
+def _mirrored_ab(leg: str, ab: float) -> float:
+    return ab if leg[1] == "l" else -ab
+
+
+def _set_pose(
+    model: mujoco.MjModel,
+    data: mujoco.MjData,
+    act: dict[str, int],
+    qadr: dict[str, int],
+    cand: Candidate,
+) -> None:
+    mujoco.mj_resetData(model, data)
+    for leg in LEG_NAMES:
+        vals = (_mirrored_ab(leg, cand.ab), cand.hip, cand.knee)
+        for jt, val in zip(LEG_JOINTS, vals):
+            name = f"{leg}_{jt}"
+            data.qpos[qadr[name]] = val
+            data.ctrl[act[name]] = val
+        data.ctrl[act[f"{leg}_{WHEEL_JOINT}"]] = 0.0
+    data.qpos[0:3] = [0.0, 0.0, cand.z_fk]
+    data.qpos[3] = 1.0
+    data.qpos[4:7] = 0.0
+    mujoco.mj_forward(model, data)
+
+
+def _rpy(data: mujoco.MjData, bodies: dict[str, int]) -> tuple[float, float]:
+    rot = data.xmat[bodies["base"]].reshape(3, 3)
+    roll = math.atan2(rot[2, 1], rot[2, 2])
+    pitch = math.atan2(-rot[2, 0], math.sqrt(rot[2, 1] ** 2 + rot[2, 2] ** 2))
+    return roll, pitch
+
+
+def _robot_com(model: mujoco.MjModel, data: mujoco.MjData) -> np.ndarray:
+    masses = model.body_mass[1:]
+    return (data.xipos[1:] * masses[:, None]).sum(axis=0) / masses.sum()
+
+
+def _support_metrics(
+    model: mujoco.MjModel,
+    data: mujoco.MjData,
+    bodies: dict[str, int],
+) -> tuple[float, float, float, float]:
+    wheel_xy = np.array([data.xpos[bodies[f"{leg}_wheel"]][:2] for leg in LEG_NAMES])
+    com_xy = _robot_com(model, data)[:2]
+    min_xy = wheel_xy.min(axis=0)
+    max_xy = wheel_xy.max(axis=0)
+    margin_low = com_xy - min_xy
+    margin_high = max_xy - com_xy
+    margin_x = float(min(margin_low[0], margin_high[0]))
+    margin_y = float(min(margin_low[1], margin_high[1]))
+    support_margin = float(min(margin_x, margin_y))
+
+    normal = []
+    for leg in LEG_NAMES:
+        bid = bodies[f"{leg}_wheel"]
+        fz = 0.0
+        for i in range(data.ncon):
+            con = data.contact[i]
+            b1 = model.geom_bodyid[con.geom1]
+            b2 = model.geom_bodyid[con.geom2]
+            if b1 == bid or b2 == bid:
+                wrench = np.zeros(6)
+                mujoco.mj_contactForce(model, data, i, wrench)
+                fz += abs(float(wrench[0]))
+        normal.append(fz)
+    normal = np.asarray(normal, dtype=float)
+    if normal.sum() < 1e-6:
+        normal_cv = 9.99
+    else:
+        normal_cv = float(normal.std() / max(1e-6, normal.mean()))
+    return margin_x, margin_y, support_margin, normal_cv
+
+
+def _clearance_metrics(data: mujoco.MjData, bodies: dict[str, int]) -> tuple[float, float]:
+    base_z = float(data.xpos[bodies["base"]][2])
+    # The collision box in wheelleg.xml is centered at z=0.054 with half-height 0.073.
+    body_clearance = base_z + 0.054 - 0.073
+    knee_z = min(float(data.xpos[bodies[f"{leg}_knee"]][2]) for leg in LEG_NAMES)
+    wheel_z = min(float(data.xpos[bodies[f"{leg}_wheel"]][2]) for leg in LEG_NAMES)
+    return body_clearance, knee_z - wheel_z
+
+
+def _static_cost(r: StaticResult) -> float:
+    wheel_over = max(0.0, abs(r.wheel_x) - SOFT_WHEEL_X)
+    support_short = max(0.0, MIN_COM_MARGIN - r.support_margin)
+    body_short = max(0.0, MIN_UNDERBODY_CLEARANCE - r.body_clearance)
+    knee_short = max(0.0, MIN_KNEE_CLEARANCE - r.knee_clearance)
+    cond_over = max(0.0, r.cond - MAX_COND)
+    tilt = math.hypot(r.roll, r.pitch)
+
+    return (
+        6500.0 * r.height_err**2
+        + 2.6 * (r.peak_tau / MAX_TORQUE) ** 2
+        + 1.1 * (r.rms_tau / MAX_TORQUE) ** 2
+        + 1.8 * max(0.0, r.imbal - 1.6) ** 2
+        + 0.65 * (abs(r.wheel_x) / SOFT_WHEEL_X) ** 2
+        + 9.0 * (wheel_over / max(1e-6, HARD_WHEEL_X - SOFT_WHEEL_X)) ** 2
+        + 10.0 * (support_short / MIN_COM_MARGIN) ** 2
+        + 2.0 * (r.normal_cv / 0.35) ** 2
+        + 30.0 * (cond_over / 1.0) ** 2
+        + 4.0 * (body_short / 0.06) ** 2
+        + 2.0 * (knee_short / 0.04) ** 2
+        + 1.0 * (tilt / 0.05) ** 2
+    )
+
+
+def _analytic_cost(r: dict, z_target: float) -> float:
+    wheel_over = max(0.0, abs(r["r_hip_x_mag"]) - SOFT_WHEEL_X)
+    cond_over = max(0.0, r["cond"] - MAX_COND)
+    return (
+        1400.0 * (r["z"] - z_target) ** 2
+        + 2.2 * (r["max_tau"] / MAX_TORQUE) ** 2
+        + 0.7 * (math.sqrt(r["i2r"] / 3.0) / MAX_TORQUE) ** 2
+        + 0.55 * (abs(r["r_hip_x_mag"]) / SOFT_WHEEL_X) ** 2
+        + 8.0 * (wheel_over / max(1e-6, HARD_WHEEL_X - SOFT_WHEEL_X)) ** 2
+        + 2.0 * (cond_over / 2.0) ** 2
+    )
+
+
+def generate_candidates(
+    z_target: float,
+    step: float,
+    ab_max: float,
+    keep: int,
+    z_tol: float,
+) -> list[Candidate]:
+    candidates: list[Candidate] = []
+    ab_values = np.arange(0.0, ab_max + 0.5 * step, step)
+    hip_values = np.arange(HIP_SCAN[0], HIP_SCAN[1] + 0.5 * step, step)
+    knee_values = np.arange(KNEE_SCAN[0], KNEE_SCAN[1] + 0.5 * step, step)
+    for ab in ab_values:
+        for hip in hip_values:
+            if hip < HIP_MIN or hip > HIP_MAX:
+                continue
+            for knee in knee_values:
+                if knee < KNEE_MIN or knee > KNEE_MAX:
+                    continue
+                r = get_all(float(ab), float(hip), float(knee))
+                if abs(r["z"] - z_target) > z_tol:
+                    continue
+                if r["wz"] >= r["kz"]:
+                    continue
+                if abs(r["r_hip_x_mag"]) > HARD_WHEEL_X:
+                    continue
+                if r["max_tau"] > MAX_TORQUE * 1.15:
+                    continue
+                candidates.append(
+                    Candidate(
+                        ab=float(ab),
+                        hip=float(hip),
+                        knee=float(knee),
+                        z_fk=float(r["z"]),
+                        analytic_cost=_analytic_cost(r, z_target),
+                    )
+                )
+    candidates.sort(key=lambda c: c.analytic_cost)
+    return candidates[:keep]
+
+
+def evaluate_candidate(
+    model: mujoco.MjModel,
+    data: mujoco.MjData,
+    act: dict[str, int],
+    qadr: dict[str, int],
+    bodies: dict[str, int],
+    cand: Candidate,
+    z_target: float,
+    settle_steps: int,
+    avg_steps: int,
+) -> StaticResult | None:
+    _set_pose(model, data, act, qadr, cand)
+    for _ in range(settle_steps):
+        mujoco.mj_step(model, data)
+
+    roll, pitch = _rpy(data, bodies)
+    if abs(roll) > 0.35 or abs(pitch) > 0.35:
+        return None
+
+    tau_buf = []
+    for _ in range(avg_steps):
+        mujoco.mj_step(model, data)
+        tau_buf.append([data.actuator_force[act[name]] for name in LEG_ACTUATORS])
+    tau = np.asarray(tau_buf, dtype=float).mean(axis=0)
+    tau_abs = np.abs(tau)
+    peak = float(tau_abs.max())
+    rms = float(np.sqrt(np.mean(tau * tau)))
+    mean_i2r = float(np.mean(tau * tau))
+
+    sagittal_abs = []
+    for name, value in zip(LEG_ACTUATORS, tau):
+        if "hip_pitch" in name or "knee" in name:
+            sagittal_abs.append(abs(float(value)))
+    sagittal_abs = np.asarray(sagittal_abs, dtype=float)
+    imbal = float(sagittal_abs.max() / max(1e-6, sagittal_abs.mean()))
+
+    fk = get_all(cand.ab, cand.hip, cand.knee)
+    margin_x, margin_y, support_margin, normal_cv = _support_metrics(model, data, bodies)
+    body_clearance, knee_clearance = _clearance_metrics(data, bodies)
+    z = float(data.xpos[bodies["base"]][2])
+
+    result = StaticResult(
+        z_target=z_target,
+        z=z,
+        ab=cand.ab,
+        hip=cand.hip,
+        knee=cand.knee,
+        cost=0.0,
+        peak_tau=peak,
+        rms_tau=rms,
+        mean_i2r=mean_i2r,
+        imbal=imbal,
+        wheel_x=float(fk["r_hip_x_mag"]),
+        cond=float(fk["cond"]),
+        min_sv=float(fk["min_sv"]),
+        com_margin_x=margin_x,
+        com_margin_y=margin_y,
+        support_margin=support_margin,
+        normal_cv=normal_cv,
+        body_clearance=body_clearance,
+        knee_clearance=knee_clearance,
+        roll=roll,
+        pitch=pitch,
+        height_err=z - z_target,
+    )
+    result.cost = _static_cost(result)
+    return result
+
+
+def optimize(
+    z_targets: list[float],
+    step: float,
+    ab_max: float,
+    keep: int,
+    z_tol: float,
+    settle_steps: int,
+    avg_steps: int,
+) -> list[StaticResult]:
+    model = mujoco.MjModel.from_xml_path(str(SCENE_XML))
+    data = mujoco.MjData(model)
+    _configure_actuators(model)
+    act, qadr, bodies = _ids(model)
+
+    results: list[StaticResult] = []
+    for zt in z_targets:
+        candidates = generate_candidates(zt, step=step, ab_max=ab_max, keep=keep, z_tol=z_tol)
+        best: StaticResult | None = None
+        for cand in candidates:
+            result = evaluate_candidate(
+                model,
+                data,
+                act,
+                qadr,
+                bodies,
+                cand,
+                zt,
+                settle_steps=settle_steps,
+                avg_steps=avg_steps,
+            )
+            if result is None:
+                continue
+            if best is None or result.cost < best.cost:
+                best = result
+        if best is None:
+            print(f"{zt:.2f}: no valid MuJoCo-static candidate from {len(candidates)} seeds")
+            continue
+        results.append(best)
+        print_result(best)
+    return results
+
+
+def print_header() -> None:
+    print("\nMuJoCo static posture optimization")
+    print("z_tgt      z    ab    hip   knee | peak   rms  imbal wheelX  cond | comX  comY  clrB  clrK | cost")
+    print("-" * 111)
+
+
+def print_result(r: StaticResult) -> None:
+    print(
+        f"{r.z_target:5.2f} {r.z:6.3f} {r.ab:5.2f} {r.hip:6.3f} {r.knee:6.3f} | "
+        f"{r.peak_tau:5.2f} {r.rms_tau:5.2f} {r.imbal:6.2f} "
+        f"{r.wheel_x:6.3f} {r.cond:5.2f} | "
+        f"{r.com_margin_x:5.3f} {r.com_margin_y:5.3f} "
+        f"{r.body_clearance:5.3f} {r.knee_clearance:5.3f} | "
+        f"{r.cost:6.2f}"
+    )
+
+
+def print_tables(results: list[StaticResult]) -> None:
+    if not results:
+        return
+    print("\nCopy-paste tables:")
+    print("_H_TARGET = " + repr([round(r.z_target, 3) for r in results]))
+    print("_Z_STATIC = " + repr([round(r.z, 3) for r in results]))
+    print("_HIP = " + repr([round(r.hip, 3) for r in results]))
+    print("_KNEE = " + repr([round(r.knee, 3) for r in results]))
+    print("_ABD_LEFT = " + repr([round(r.ab, 3) for r in results]))
+    print("_ABD_RIGHT = " + repr([round(-r.ab, 3) for r in results]))
+
+    best = min(results, key=lambda r: r.cost)
+    print("\nRecommended default:")
+    print(
+        f"z={best.z_target:.2f}, ab={best.ab:.3f}, hip={best.hip:.3f}, "
+        f"knee={best.knee:.3f}, peak={best.peak_tau:.2f}Nm, "
+        f"rms={best.rms_tau:.2f}Nm, support_margin={best.support_margin:.3f}m"
+    )
+    pose = []
+    for leg in LEG_NAMES:
+        pose.extend([_mirrored_ab(leg, best.ab), best.hip, best.knee])
+    pose.extend([0.0, 0.0, 0.0, 0.0])
+    print("default_dof_pos = " + repr([round(v, 3) for v in pose]))
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--z-min", type=float, default=0.36)
+    parser.add_argument("--z-max", type=float, default=0.45)
+    parser.add_argument("--z-step", type=float, default=0.01)
+    parser.add_argument("--grid-step", type=float, default=0.01)
+    parser.add_argument("--ab-max", type=float, default=0.0)
+    parser.add_argument("--keep", type=int, default=45)
+    parser.add_argument("--z-tol", type=float, default=0.035)
+    parser.add_argument("--settle-steps", type=int, default=350)
+    parser.add_argument("--avg-steps", type=int, default=80)
+    parser.add_argument("--quick", action="store_true", help="Coarser and faster scan")
+    args = parser.parse_args()
+
+    if args.quick:
+        args.grid_step = max(args.grid_step, 0.02)
+        args.keep = min(args.keep, 20)
+        args.z_tol = max(args.z_tol, 0.040)
+        args.settle_steps = min(args.settle_steps, 220)
+        args.avg_steps = min(args.avg_steps, 40)
+
+    n = int(round((args.z_max - args.z_min) / args.z_step)) + 1
+    z_targets = [round(args.z_min + i * args.z_step, 3) for i in range(n)]
+
+    print_header()
+    results = optimize(
+        z_targets,
+        step=args.grid_step,
+        ab_max=args.ab_max,
+        keep=args.keep,
+        z_tol=args.z_tol,
+        settle_steps=args.settle_steps,
+        avg_steps=args.avg_steps,
+    )
+    print_tables(results)
+
+
+if __name__ == "__main__":
+    main()

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)
+        if hasattr(spec, "delete"):
-        for act in actuators_to_delete:
+            actuators_to_delete = list(spec.actuators)
-            spec.delete(act)
+            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",
+        "rough": rough_onnx if rough_onnx.exists() else project_root / "model_rough.pt",
-        "crawl": project_root / "model_crawl.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 = None
-        self.listener.start()
+        if keyboard is not None:
-        print("[PolicyRunner] Background Keyboard Switcher active: Press '1' for ROUGH, '2' for CRAWL")
+            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/src/robot/config/env_cfgs.py

@@ -52,7 +52,11 @@ from ..mdp.only_positive_rewards import enable_only_positive_rewards
 from ..mdp.rewards import (
     track_linear_velocity,
     track_linear_velocity_l1,
+    track_linear_velocity_x,
+    track_linear_velocity_y,
     track_angular_velocity,
+    track_angular_velocity_z,
+    stair_lateral_yaw_drift_l2,
     base_height_l2,
     safe_base_lin_vel,
     safe_foot_contact,
@@ -79,8 +83,34 @@ from ..mdp.rewards import (
     ang_vel_xy_l2,
     undesired_contacts,
     contact_forces,
+    tracking_lin_vel_error,
+    tracking_yaw_vel_error,
+    tracking_lin_vel_x_error,
+    tracking_lin_vel_y_error,
+    tracking_lin_vel_along_command_error,
+    actual_lin_vel_orthogonal_command_mean,
+    command_lin_vel_mean,
+    command_yaw_vel_abs_mean,
+    actual_lin_vel_mean,
+    tracking_lin_vel_error_band_mean,
+    tracking_lin_vel_axis_error_band_mean,
+    command_band_active,
+    wheel_raw_action_abs_mean,
+    wheel_target_vel_abs_mean,
+    wheel_actual_vel_abs_mean,
+    wheel_target_actual_vel_error_mean,
+    wheel_actual_to_target_vel_ratio_mean,
+    wheel_target_actual_sign_agreement,
+    upright_metric,
+    base_ground_contact_metric,
+)
+from ..mdp.curriculums import (
+    command_axis_levels_vel,
+    command_levels_adaptive,
+    terrain_levels_obstacle_release,
+    terrain_levels_ramp_strict,
+    terrain_levels_vel_strict,
 )
-from ..mdp.curriculums import terrain_levels_vel_strict
 from ..mdp.commands import UniformThresholdVelocityCommandCfg
 
 # Constant Definitions
@@ -153,7 +183,7 @@ def _make_base_env_cfg() -> ManagerBasedRlEnvCfg:
         ),
         "projected_gravity": ObservationTermCfg(
             func=velocity_mdp.projected_gravity,
-            noise=Unoise(n_min=-0.08, n_max=0.08),
+            noise=Unoise(n_min=-0.05, n_max=0.05),
         ),
         "command": ObservationTermCfg(
             func=velocity_mdp.generated_commands,
@@ -212,13 +242,13 @@ def _make_base_env_cfg() -> ManagerBasedRlEnvCfg:
             actuator_names=(".*_hip_abduction_joint", ".*_hip_pitch_joint", ".*_knee_joint"),
             scale={".*_hip_abduction_joint": 0.125, "^(?!.*_hip_abduction_joint).*": 0.25}, use_default_offset=True,
             control_frequency=50.0, cut_off_frequency=5.0,
-            min_delay=0, max_delay=4,
+            min_delay=0, max_delay=2,
         ),
         "wheel_joint_vel": JointVelocityDelayedLowPassActionCfg(
             entity_name="robot", actuator_names=(".*_wheel_joint",),
             scale=5.0, offset=0.0, use_default_offset=False,
             control_frequency=50.0, cut_off_frequency=15.0,
-            min_delay=0, max_delay=4,
+            min_delay=0, max_delay=2,
         ),
     }
 
@@ -262,20 +292,20 @@ def _make_base_env_cfg() -> ManagerBasedRlEnvCfg:
         ),
         "base_com": EventTermCfg(
             func=envs_dr.body_com_offset, mode="startup",
-            params={"asset_cfg": SceneEntityCfg("robot", body_names=(".*",)),
+            params={"asset_cfg": SceneEntityCfg("robot", body_names=("base_link",)),
                     "operation": "add", "ranges": {0: (-0.05, 0.05), 1: (-0.05, 0.05), 2: (-0.05, 0.05)}},
         ),
         "body_friction": EventTermCfg(
             func=envs_dr.geom_friction, mode="startup",
-            params={"asset_cfg": SceneEntityCfg("robot", geom_names=(".*",)), "operation": "abs", "ranges": (0.15, 1.25)},
+            params={"asset_cfg": SceneEntityCfg("robot", geom_names=(".*",)), "operation": "abs", "ranges": (0.3, 1.0)},
         ),
         "actuator_stiffness": EventTermCfg(
             func=envs_dr.joint_stiffness, mode="startup",
-            params={"asset_cfg": SceneEntityCfg("robot"), "ranges": (0.5, 1.5), "operation": "scale", "distribution": "log_uniform"},
+            params={"asset_cfg": SceneEntityCfg("robot"), "ranges": (0.9, 1.1), "operation": "scale", "distribution": "log_uniform"},
         ),
         "actuator_damping": EventTermCfg(
             func=envs_dr.joint_damping, mode="startup",
-            params={"asset_cfg": SceneEntityCfg("robot"), "ranges": (0.5, 1.5), "operation": "scale", "distribution": "log_uniform"},
+            params={"asset_cfg": SceneEntityCfg("robot"), "ranges": (0.9, 1.1), "operation": "scale", "distribution": "log_uniform"},
         ),
         "body_mass_base": EventTermCfg(
             func=envs_dr.body_mass, mode="startup",
@@ -285,24 +315,6 @@ def _make_base_env_cfg() -> ManagerBasedRlEnvCfg:
                 "ranges": (-1.0, 3.0),
             },
         ),
-        "body_mass_limbs": EventTermCfg(
-            func=envs_dr.body_mass, mode="startup",
-            params={
-                "asset_cfg": SceneEntityCfg("robot", body_names=(".*_knee_.*", ".*_wheel_.*")),
-                "operation": "scale",
-                "ranges": (0.7, 1.3),
-            },
-        ),
-        "apply_continuous_disturbance": EventTermCfg(
-            func=apply_continuous_disturbance, mode="step",
-            params={
-                "asset_cfg": SceneEntityCfg("robot", body_names=("base_link",)),
-                "force_range": (-10.0, 10.0),
-                "torque_range": (-10.0, 10.0),
-                "resample_time_range": (5.0, 10.0),
-                "time_constant": 1.0,
-            },
-        ),
     }
 
     # ------------------
@@ -385,13 +397,18 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
         terrain_generator=TerrainGeneratorCfg(
             size=(8.0, 8.0), border_width=20.0, num_rows=10, num_cols=20, curriculum=True,
             sub_terrains={
-                "flat": BoxFlatTerrainCfg(proportion=0.05, size=(8.0, 8.0)),
+                "flat": BoxFlatTerrainCfg(proportion=0.15, size=(8.0, 8.0)),
-                "pyramid_stairs": BoxPyramidStairsTerrainCfg(proportion=0.05, step_height_range=(0.0, 0.3), step_width=0.30, size=(8.0, 8.0)),
+                "pyramid_stairs": BoxPyramidStairsTerrainCfg(proportion=0.05, step_height_range=(0.0, 0.20), step_width=0.30, size=(8.0, 8.0)),
-                "pyramid_stairs_inv": BoxInvertedPyramidStairsTerrainCfg(proportion=0.45, step_height_range=(0.0, 0.3), step_width=0.30, size=(8.0, 8.0)),
+                "pyramid_stairs_inv": BoxInvertedPyramidStairsTerrainCfg(proportion=0.35, step_height_range=(0.0, 0.20), step_width=0.30, size=(8.0, 8.0)),
-                "random_grid": BoxRandomGridTerrainCfg(proportion=0.27, grid_width=0.45, grid_height_range=(0.0, 0.3), size=(8.0, 8.0)),
+                "random_grid": BoxRandomGridTerrainCfg(proportion=0.27, grid_width=0.45, grid_height_range=(0.0, 0.20), size=(8.0, 8.0)),
                 "random_rough": HfRandomUniformTerrainCfg(proportion=0.01, noise_range=(0.0, 0.06), noise_step=0.01, horizontal_scale=0.20, downsampled_scale=0.20, border_width=0.25, base_thickness_ratio=100.0, size=(8.0, 8.0)),
                 "perlin_noise": HfPerlinNoiseTerrainCfg(proportion=0.01, height_range=(0.0, 0.06), octaves=2, persistence=0.4, lacunarity=2.0, horizontal_scale=0.20, resolution=0.20, border_width=0.50, base_thickness_ratio=100.0, size=(8.0, 8.0)),
-                "rc_wall": RCWallTerrainCfg(proportion=0.15, wall_height_range=(0.0, 0.45), size=(8.0, 8.0)),
+                "rc_wall": RCWallTerrainCfg(
+                    proportion=0.15,
+                    wall_height_range=(0.10, 0.35),
+                    wall_centers_x=(2.1, 3.2, 4.3, 5.4, 6.5),
+                    size=(8.0, 8.0),
+                ),
                 "sloped_terrain": HfPyramidSlopedTerrainCfg(proportion=0.01, slope_range=(0.052, 0.325), platform_width=2.0, border_width=0.25, base_thickness_ratio=100.0, horizontal_scale=0.20, size=(8.0, 8.0)),
             },
         ),
@@ -400,15 +417,65 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
 
     # Keep the custom terrain set, but align command/curriculum behavior with go2w rough.
     cfg.curriculum.pop("command_vel", None)
-    cfg.curriculum["terrain_levels"] = CurriculumTermCfg(func=velocity_mdp.terrain_levels_vel, params={"command_name": "twist"})
+    cfg.curriculum["terrain_levels"] = CurriculumTermCfg(
+        func=terrain_levels_obstacle_release,
+        params={
+            "command_name": "twist",
+            "initial_terrain_names": ("flat", "random_rough", "perlin_noise", "sloped_terrain", "pyramid_stairs"),
+            "release_schedule": (
+                (200 * 24, ("random_grid",)),
+                (500 * 24, ("pyramid_stairs_inv",)),
+                (700 * 24, ("rc_wall",)),
+            ),
+        },
+    )
+    cfg.curriculum["command_x_levels"] = CurriculumTermCfg(
+        func=command_levels_adaptive,
+        params={
+            "command_name": "twist",
+            "reward_term_name": "track_lin_vel_x_exp",
+            "axis": "x",
+            "initial_range": (-0.5, 0.5),
+            "delta_command": 0.05,
+            "target_ratio": 0.8,
+            "ema_alpha": 0.5,
+        },
+    )
+    cfg.curriculum["command_y_levels"] = CurriculumTermCfg(
+        func=command_levels_adaptive,
+        params={
+            "command_name": "twist",
+            "reward_term_name": "track_lin_vel_y_exp",
+            "axis": "y",
+            "initial_range": (-0.5, 0.5),
+            "delta_command": 0.05,
+            "target_ratio": 0.8,
+            "ema_alpha": 0.5,
+        },
+    )
+    cfg.curriculum["command_yaw_levels"] = CurriculumTermCfg(
+        func=command_levels_adaptive,
+        params={
+            "command_name": "twist",
+            "reward_term_name": "track_ang_vel_z_exp",
+            "axis": "yaw",
+            "initial_range": (-0.5, 0.5),
+            "delta_command": 0.05,
+            "target_ratio": 0.8,
+            "ema_alpha": 0.5,
+        },
+    )
 
     cfg.commands["twist"].heading_command = True
     cfg.commands["twist"].rel_heading_envs = 1.0
     cfg.commands["twist"].heading_control_stiffness = 0.5
     cfg.commands["twist"].ranges.heading = (-math.pi, math.pi)
     cfg.commands["twist"].rel_standing_envs = 0.02
+    cfg.commands["twist"].rel_forward_envs = 0.30
+    cfg.commands["twist"].rel_lateral_envs = 0.20
+    cfg.commands["twist"].rel_yaw_envs = 0.20
     cfg.commands["twist"].ranges.lin_vel_x = (-1.0, 1.0)
-    cfg.commands["twist"].ranges.lin_vel_y = (-0.6, 0.6)
+    cfg.commands["twist"].ranges.lin_vel_y = (-1.0, 1.0)
     cfg.commands["twist"].ranges.ang_vel_z = (-1.0, 1.0)
 
     # ------------------
@@ -420,7 +487,7 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
     cfg.events["reset_base"] = EventTermCfg(
         func=envs_mdp.reset_root_state_uniform, mode="reset",
         params={
-            "pose_range": {"z": (0.40, 0.45), "yaw": (-math.pi, math.pi)},
+            "pose_range": {"z": (0.42, 0.42), "yaw": (-math.pi, math.pi)},
             "velocity_range": {"x": (-0.2, 0.2), "y": (-0.1, 0.1), "yaw": (-0.2, 0.2)},
             "asset_cfg": SceneEntityCfg("robot"),
         },
@@ -434,15 +501,32 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
     # ------------------
     # Rewards Integration
     # ------------------
-    cfg.rewards["track_lin_vel"] = RewardTermCfg(
+    cfg.rewards.pop("track_lin_vel", None)
-        func=track_linear_velocity,
+    cfg.rewards.pop("track_ang_vel", None)
-        weight=3.0,
+    cfg.rewards["track_lin_vel_x_exp"] = RewardTermCfg(
-        params={"std": 0.5, "command_name": "twist"}
+        func=track_linear_velocity_x,
+        weight=1.0,
+        params={"std": 0.25, "command_name": "twist"},
     )
-    cfg.rewards["track_ang_vel"] = RewardTermCfg(
+    cfg.rewards["track_lin_vel_y_exp"] = RewardTermCfg(
-        func=track_angular_velocity,
+        func=track_linear_velocity_y,
-        weight=1.5,
+        weight=1.0,
-        params={"std": 0.5, "command_name": "twist"}
+        params={"std": 0.25, "command_name": "twist"},
+    )
+    cfg.rewards["track_ang_vel_z_exp"] = RewardTermCfg(
+        func=track_angular_velocity_z,
+        weight=1.0,
+        params={"std": 0.25, "command_name": "twist"},
+    )
+    cfg.rewards["stair_lateral_yaw_drift"] = RewardTermCfg(
+        func=stair_lateral_yaw_drift_l2,
+        weight=-1.0,
+        params={
+            "terrain_names": ("pyramid_stairs", "pyramid_stairs_inv", "random_grid"),
+            "y_scale": 1.0,
+            "yaw_scale": 1.0,
+            "asset_cfg": SceneEntityCfg("robot"),
+        },
     )
     
     cfg.rewards["lin_vel_z"] = RewardTermCfg(func=lin_vel_z_l2, weight=-2.0)
@@ -473,8 +557,8 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
         weight=-0.05, 
         params={
             "mirror_joints": [
-                ["fl_(hip_abduction|hip_pitch|knee)_joint", "rr_(hip_abduction|hip_pitch|knee)_joint"],
+                ["fl_(hip_pitch|knee)_joint", "rr_(hip_pitch|knee)_joint"],
-                ["fr_(hip_abduction|hip_pitch|knee)_joint", "rl_(hip_abduction|hip_pitch|knee)_joint"]
+                ["fr_(hip_pitch|knee)_joint", "rl_(hip_pitch|knee)_joint"]
             ], 
             "asset_cfg": SceneEntityCfg("robot")
         }
@@ -488,28 +572,56 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
     cfg.rewards.pop("variable_posture", None)
 
     cfg.rewards.pop("joint_deviation_l2", None)
-    cfg.rewards["joint_pos_penalty"] = RewardTermCfg(
+
+    # 针对 ab 关节施加较严厉的惩罚，防止在 yaw 时乱撇腿
+    cfg.rewards["joint_pos_penalty_ab"] = RewardTermCfg(
         func=joint_pos_penalty,
         weight=-1.0,
         params={
             "stand_still_scale": 5.0,
             "velocity_threshold": 0.5,
             "command_threshold": 0.1,
-            "asset_cfg": SceneEntityCfg("robot", joint_names=(".*_hip_abduction_joint", ".*_hip_pitch_joint", ".*_knee_joint")),
+            "asset_cfg": SceneEntityCfg("robot", joint_names=(".*_hip_abduction_joint",)),
             "command_name": "twist"
         }
     )
 
+    # 针对 pitch 和 knee 关节施加较宽松的惩罚，保留跨越障碍的抬腿自由度
+    cfg.rewards["joint_pos_penalty_sagittal"] = RewardTermCfg(
+        func=joint_pos_penalty,
+        weight=-0.3,
+        params={
+            "stand_still_scale": 5.0,
+            "velocity_threshold": 0.5,
+            "command_threshold": 0.1,
+            "asset_cfg": SceneEntityCfg("robot", joint_names=(".*_hip_pitch_joint", ".*_knee_joint")),
+            "command_name": "twist"
+        }
+    )
+
+    # 🌟 强力约束同侧外展关节平行对称，消除转向时的前后剪刀式摆动
+    cfg.rewards["abduction_mirror"] = RewardTermCfg(
+        func=joint_mirror,
+        weight=-0.5,  # 施加合理惩罚，限制前后腿同侧外展关节反向运动
+        params={
+            "mirror_joints": [
+                ["fl_hip_abduction_joint", "rl_hip_abduction_joint"],
+                ["fr_hip_abduction_joint", "rr_hip_abduction_joint"]
+            ],
+            "asset_cfg": SceneEntityCfg("robot")
+        }
+    )
+
     cfg.rewards["feet_contact_without_cmd"] = RewardTermCfg(
         func=feet_contact_without_cmd,
         weight=0.1,
         params={"command_name": "twist", "sensor_name": "feet_ground_contact"}
     )
-    cfg.rewards["feet_air_time"].weight = 0.0
+    cfg.rewards["feet_air_time"].weight = 0.15
-    cfg.rewards["upward"] = RewardTermCfg(func=upward, weight=1.0)
+    cfg.rewards["upward"] = RewardTermCfg(func=upward, weight=0.5)
     
     cfg.rewards["base_height_l2"].weight = 0.0
-    cfg.rewards["base_height_l2"].params["target_height"] = 0.40
+    cfg.rewards["base_height_l2"].params["target_height"] = 0.42
     cfg.rewards["base_height_l2"].params["sensor_cfg"] = SceneEntityCfg("height_scanner")
 
     # 恢复机身碰撞惩罚为-1.0，逼迫机器人高抬腿跨越障碍，防止拖地
@@ -533,6 +645,79 @@ def rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg:
         cfg.scene.terrain.num_envs = 2048
         cfg.scene.terrain.env_spacing = 2.5
 
+    cfg.metrics.update(
+        {
+            "tracking_lin_vel_error": MetricsTermCfg(func=tracking_lin_vel_error, params={"command_name": "twist"}),
+            "tracking_lin_vel_x_error": MetricsTermCfg(func=tracking_lin_vel_x_error, params={"command_name": "twist"}),
+            "tracking_lin_vel_y_error": MetricsTermCfg(func=tracking_lin_vel_y_error, params={"command_name": "twist"}),
+            "tracking_lin_vel_along_cmd_error": MetricsTermCfg(
+                func=tracking_lin_vel_along_command_error, params={"command_name": "twist"}
+            ),
+            "actual_lin_vel_orthogonal_cmd": MetricsTermCfg(
+                func=actual_lin_vel_orthogonal_command_mean, params={"command_name": "twist"}
+            ),
+            "tracking_yaw_vel_error": MetricsTermCfg(func=tracking_yaw_vel_error, params={"command_name": "twist"}),
+            "cmd_lin_vel": MetricsTermCfg(func=command_lin_vel_mean, params={"command_name": "twist"}),
+            "cmd_yaw_vel": MetricsTermCfg(func=command_yaw_vel_abs_mean, params={"command_name": "twist"}),
+            "actual_lin_vel": MetricsTermCfg(func=actual_lin_vel_mean),
+            "tracking_lin_vel_error_cmd_0_03": MetricsTermCfg(
+                func=tracking_lin_vel_error_band_mean,
+                params={"command_name": "twist", "min_speed": 0.0, "max_speed": 0.3},
+            ),
+            "tracking_lin_vel_error_cmd_03_07": MetricsTermCfg(
+                func=tracking_lin_vel_error_band_mean,
+                params={"command_name": "twist", "min_speed": 0.3, "max_speed": 0.7},
+            ),
+            "tracking_lin_vel_error_cmd_07_up": MetricsTermCfg(
+                func=tracking_lin_vel_error_band_mean,
+                params={"command_name": "twist", "min_speed": 0.7, "max_speed": 10.0},
+            ),
+            "tracking_lin_vel_x_error_cmd_0_03": MetricsTermCfg(
+                func=tracking_lin_vel_axis_error_band_mean,
+                params={"axis": 0, "command_name": "twist", "min_speed": 0.0, "max_speed": 0.3},
+            ),
+            "tracking_lin_vel_x_error_cmd_03_07": MetricsTermCfg(
+                func=tracking_lin_vel_axis_error_band_mean,
+                params={"axis": 0, "command_name": "twist", "min_speed": 0.3, "max_speed": 0.7},
+            ),
+            "tracking_lin_vel_x_error_cmd_07_up": MetricsTermCfg(
+                func=tracking_lin_vel_axis_error_band_mean,
+                params={"axis": 0, "command_name": "twist", "min_speed": 0.7, "max_speed": 10.0},
+            ),
+            "tracking_lin_vel_y_error_cmd_0_03": MetricsTermCfg(
+                func=tracking_lin_vel_axis_error_band_mean,
+                params={"axis": 1, "command_name": "twist", "min_speed": 0.0, "max_speed": 0.3},
+            ),
+            "tracking_lin_vel_y_error_cmd_03_07": MetricsTermCfg(
+                func=tracking_lin_vel_axis_error_band_mean,
+                params={"axis": 1, "command_name": "twist", "min_speed": 0.3, "max_speed": 0.7},
+            ),
+            "tracking_lin_vel_y_error_cmd_07_up": MetricsTermCfg(
+                func=tracking_lin_vel_axis_error_band_mean,
+                params={"axis": 1, "command_name": "twist", "min_speed": 0.7, "max_speed": 10.0},
+            ),
+            "cmd_band_0_03": MetricsTermCfg(
+                func=command_band_active, params={"command_name": "twist", "min_speed": 0.0, "max_speed": 0.3}
+            ),
+            "cmd_band_03_07": MetricsTermCfg(
+                func=command_band_active, params={"command_name": "twist", "min_speed": 0.3, "max_speed": 0.7}
+            ),
+            "cmd_band_07_up": MetricsTermCfg(
+                func=command_band_active, params={"command_name": "twist", "min_speed": 0.7, "max_speed": 10.0}
+            ),
+            "wheel_raw_action_abs": MetricsTermCfg(func=wheel_raw_action_abs_mean),
+            "wheel_target_vel_abs": MetricsTermCfg(func=wheel_target_vel_abs_mean),
+            "wheel_actual_vel_abs": MetricsTermCfg(func=wheel_actual_vel_abs_mean),
+            "wheel_target_actual_vel_error": MetricsTermCfg(func=wheel_target_actual_vel_error_mean),
+            "wheel_actual_to_target_vel_ratio": MetricsTermCfg(func=wheel_actual_to_target_vel_ratio_mean),
+            "wheel_target_actual_sign_agreement": MetricsTermCfg(func=wheel_target_actual_sign_agreement),
+            "upright": MetricsTermCfg(func=upright_metric),
+            "base_ground_contact_rate": MetricsTermCfg(
+                func=base_ground_contact_metric, params={"sensor_name": "base_ground_contact"}
+            ),
+        }
+    )
+
     if play:
         cfg.episode_length_s = int(1e9)
         cfg.observations["actor"].enable_corruption = False

05_software/train/rc_mjlab/src/robot/config/rl_cfg.py

@@ -33,9 +33,9 @@ def rough_ppo_runner_cfg() -> RslRlOnPolicyRunnerCfg:
             value_loss_coef=1.0,
             use_clipped_value_loss=True,
             clip_param=0.2,
-            entropy_coef=0.001,#第一轮为0.003
+            entropy_coef=0.003,#第一轮为0.003，第二轮0.001，第三轮0.0008
-            num_learning_epochs=5,
+            num_learning_epochs=5,#2048为5 4096为3
-            num_mini_batches=4,
+            num_mini_batches=4,#2048为4 4096为8
             learning_rate=8.0e-4,
             schedule="adaptive",
             gamma=0.99,

05_software/train/rc_mjlab/src/robot/mdp/commands.py

@@ -21,14 +21,20 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
 
     def __init__(self, cfg: UniformThresholdVelocityCommandCfg, env: ManagerBasedRlEnv):
         super().__init__(cfg, env)
+        self.is_lateral_env = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
+        self.is_yaw_env = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
+        self.was_climbing = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device)
         # 缓存地形类型的索引（台阶、反向台阶、垂直短墙），实现高容错动态查找
         self._climbing_indices = []
+        self._flat_index = -1
         terrain = getattr(self._env.scene, "terrain", None)
         if terrain is not None and getattr(terrain.cfg, "terrain_generator", None) is not None:
             sub_terrain_names = list(terrain.cfg.terrain_generator.sub_terrains.keys())
             for name in ["pyramid_stairs", "pyramid_stairs_inv", "rc_wall"]:
                 if name in sub_terrain_names:
                     self._climbing_indices.append(sub_terrain_names.index(name))
+            if "flat" in sub_terrain_names:
+                self._flat_index = sub_terrain_names.index("flat")
 
     def _resample_command(self, env_ids: torch.Tensor) -> None:
         # 1. 调用基类的标准采样
@@ -42,6 +48,41 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
             self.vel_command_b[small_cmd_ids, :] = 0.0
             self.vel_command_w[small_cmd_ids, :] = 0.0
 
+        # 重置并采样单 Y（只横移）与单 Z（只原地转向）指令分布
+        self.is_lateral_env[env_ids] = False
+        self.is_yaw_env[env_ids] = False
+
+        # 对没有设为前进且没有静止的激活环境进行单轴采样
+        active_non_fwd_mask = (~self.is_forward_env[env_ids]) & (~self.is_standing_env[env_ids])
+        active_non_fwd_ids = env_ids[active_non_fwd_mask]
+
+        if len(active_non_fwd_ids) > 0:
+            r = torch.empty(len(active_non_fwd_ids), device=self.device)
+            # 采样单 Y 占比
+            self.is_lateral_env[active_non_fwd_ids] = r.uniform_(0.0, 1.0) <= self.cfg.rel_lateral_envs
+            lat_ids = active_non_fwd_ids[self.is_lateral_env[active_non_fwd_ids]]
+            if len(lat_ids) > 0:
+                self.vel_command_b[lat_ids, 0] = 0.0
+                y_signs = self.vel_command_b[lat_ids, 1].sign()
+                y_signs[y_signs == 0] = 1.0
+                self.vel_command_b[lat_ids, 1] = y_signs * self.vel_command_b[lat_ids, 1].abs().clamp(min=0.3)
+                self.vel_command_b[lat_ids, 2] = 0.0
+
+            # 对不是 forward 也不是 lateral 的环境采样单 Z 占比
+            non_lat_mask = ~self.is_lateral_env[active_non_fwd_ids]
+            non_lat_ids = active_non_fwd_ids[non_lat_mask]
+
+            if len(non_lat_ids) > 0:
+                r_yaw = torch.empty(len(non_lat_ids), device=self.device)
+                self.is_yaw_env[non_lat_ids] = r_yaw.uniform_(0.0, 1.0) <= self.cfg.rel_yaw_envs
+                yaw_ids = non_lat_ids[self.is_yaw_env[non_lat_ids]]
+                if len(yaw_ids) > 0:
+                    self.vel_command_b[yaw_ids, 0] = 0.0
+                    self.vel_command_b[yaw_ids, 1] = 0.0
+                    z_signs = self.vel_command_b[yaw_ids, 2].sign()
+                    z_signs[z_signs == 0] = 1.0
+                    self.vel_command_b[yaw_ids, 2] = z_signs * self.vel_command_b[yaw_ids, 2].abs().clamp(min=0.3)
+
         # 3. 地形自适应重采样限制：若在爬行地形，强制纯前进方向且速度 >= 0.3 m/s
         terrain = getattr(self._env.scene, "terrain", None)
         terrain_types = getattr(terrain, "terrain_types", None)
@@ -58,6 +99,37 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
                 self.vel_command_b[climbing_env_ids, 2] = 0.0
                 self.is_heading_env[climbing_env_ids] = True
 
+        # 4. 在平地（flat）地形上生成 35% 纯 Y 和 35% 纯 Z 指令，排除静止环境
+        if terrain_types is not None and self._flat_index != -1:
+            is_flat = terrain_types == self._flat_index
+            flat_env_ids = env_ids[is_flat[env_ids]]
+            if len(flat_env_ids) > 0:
+                active_flat_mask = ~self.is_standing_env[flat_env_ids]
+                active_flat_ids = flat_env_ids[active_flat_mask]
+                if len(active_flat_ids) > 0:
+                    r = torch.empty(len(active_flat_ids), device=self.device).uniform_(0.0, 1.0)
+                    y_only_mask = r < 0.35
+                    z_only_mask = (r >= 0.35) & (r < 0.70)
+
+                    y_only_ids = active_flat_ids[y_only_mask]
+                    if len(y_only_ids) > 0:
+                        self.vel_command_b[y_only_ids, 0] = 0.0  # x = 0
+                        self.vel_command_b[y_only_ids, 2] = 0.0  # yaw = 0
+                        if self.cfg.heading_command:
+                            self.heading_target[y_only_ids] = self.robot.data.heading_w[y_only_ids]
+                            self.is_heading_env[y_only_ids] = True
+
+                    z_only_ids = active_flat_ids[z_only_mask]
+                    if len(z_only_ids) > 0:
+                        self.vel_command_b[z_only_ids, 0] = 0.0  # x = 0
+                        self.vel_command_b[z_only_ids, 1] = 0.0  # y = 0
+                        if self.cfg.heading_command:
+                            yaw_delta = torch.empty(len(z_only_ids), device=self.device).uniform_(
+                                -self.cfg.yaw_only_heading_range, self.cfg.yaw_only_heading_range
+                            )
+                            self.heading_target[z_only_ids] = self.robot.data.heading_w[z_only_ids] + yaw_delta
+                            self.is_heading_env[z_only_ids] = True
+
     def _update_command(self) -> None:
         # 调用基类的每步更新
         super()._update_command()
@@ -70,6 +142,13 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
             for idx in self._climbing_indices:
                 is_climbing |= (terrain_types == idx)
 
+            # 【出台阶实时重采样】：检查从爬坡状态刚刚离开环境的机器人，进行即时指令重采样
+            left_climbing_mask = self.was_climbing & ~is_climbing
+            if left_climbing_mask.any():
+                left_climbing_ids = torch.where(left_climbing_mask)[0]
+                self._resample_command(left_climbing_ids)
+
+            # 【入台阶实时截断】：对于正在爬楼梯/翻越短墙的环境，强行截断其横向横移指令，并进行温和偏航对齐
             climbing_env_ids = is_climbing.nonzero(as_tuple=False).flatten()
             if len(climbing_env_ids) > 0:
                 self.vel_command_b[climbing_env_ids, 1] = 0.0
@@ -79,6 +158,7 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
                     min=-0.3,
                     max=0.3
                 )
+            self.was_climbing = is_climbing
 
         # 5. 高速侧向解耦（适用于平地/斜坡等混合路面）：当前进速度 >= 0.8 m/s 时，清空侧向指令，防止高速甩尾甩飞
         high_speed_mask = self.vel_command_b[:, 0].abs() >= 0.8
@@ -90,3 +170,6 @@ class UniformThresholdVelocityCommand(UniformVelocityCommand):
 @dataclass(kw_only=True)
 class UniformThresholdVelocityCommandCfg(UniformVelocityCommandCfg):
     class_type: type = UniformThresholdVelocityCommand
+    yaw_only_heading_range: float = 1.0
+    rel_lateral_envs: float = 0.0
+    rel_yaw_envs: float = 0.0

05_software/train/rc_mjlab/src/robot/mdp/curriculums.py

@@ -99,6 +99,165 @@ class adaptive_command_vel:
         }
 
 
+class command_axis_levels_vel:
+    """Linearly expand one command axis over training steps."""
+
+    def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRlEnv):
+        from mjlab.tasks.velocity.mdp import UniformVelocityCommandCfg
+
+        p = cfg.params
+        self._command_name: str = p.get("command_name", "twist")
+        self._reward_name: str = p["reward_term_name"]
+        self._axis: str = p["axis"]
+        self._range_multiplier: tuple[float, float] = tuple(p.get("range_multiplier", (0.4, 1.0)))
+        self._initial_range_param: tuple[float, float] | None = (
+            tuple(p["initial_range"]) if "initial_range" in p else None
+        )
+        self._ema_alpha: float = p.get("ema_alpha", 0.5)
+        self._warmup_steps: int = p.get("warmup_steps", 0)
+        self._ramp_steps: int = p.get("ramp_steps", 800 * 24)
+
+        command_term = env.command_manager.get_term(self._command_name)
+        self._cfg = cast(UniformVelocityCommandCfg, command_term.cfg)
+        self._reward_idx = list(env.reward_manager._term_names).index(self._reward_name)
+        self._reward_weight = env.reward_manager.get_term_cfg(self._reward_name).weight
+        self._running_mean = 0.0
+
+        self._full_range = self._get_range()
+        self._min_range = self._initial_range_param or self._scaled_range(self._range_multiplier[0])
+        self._set_range(self._min_range)
+
+    def __call__(self, env: ManagerBasedRlEnv, env_ids: torch.Tensor, **kwargs) -> dict[str, torch.Tensor]:
+        if len(env_ids) > 0:
+            episode_sums = env.reward_manager._episode_sums[self._reward_name][env_ids]
+            mean_raw = torch.mean(episode_sums / env.cfg.episode_length_s / self._reward_weight).item()
+            self._running_mean = self._ema_alpha * mean_raw + (1.0 - self._ema_alpha) * self._running_mean
+        self._set_range(self._current_max_range(env.common_step_counter))
+
+        lo, hi = self._get_range()
+        return {
+            f"{self._axis}_range_min": torch.tensor(lo),
+            f"{self._axis}_range_max": torch.tensor(hi),
+            f"{self._axis}_tracking_ema": torch.tensor(self._running_mean),
+            f"{self._axis}_warmup_active": torch.tensor(float(env.common_step_counter < self._warmup_steps)),
+            f"{self._axis}_range_cap": torch.tensor(self._current_multiplier(env.common_step_counter)),
+        }
+
+    def _get_range(self) -> tuple[float, float]:
+        if self._axis == "x":
+            return tuple(self._cfg.ranges.lin_vel_x)
+        if self._axis == "y":
+            return tuple(self._cfg.ranges.lin_vel_y)
+        if self._axis == "yaw":
+            return tuple(self._cfg.ranges.ang_vel_z)
+        raise ValueError(f"Unknown command curriculum axis: {self._axis}")
+
+    def _set_range(self, value: tuple[float, float]) -> None:
+        if self._axis == "x":
+            self._cfg.ranges.lin_vel_x = value
+        elif self._axis == "y":
+            self._cfg.ranges.lin_vel_y = value
+        elif self._axis == "yaw":
+            self._cfg.ranges.ang_vel_z = value
+        else:
+            raise ValueError(f"Unknown command curriculum axis: {self._axis}")
+
+    def _scaled_range(self, multiplier: float) -> tuple[float, float]:
+        lo, hi = self._full_range
+        return (lo * multiplier, hi * multiplier)
+
+    def _current_multiplier(self, step: int) -> float:
+        start, end = self._range_multiplier
+        if self._ramp_steps <= 0:
+            return end
+        progress = max(0.0, min(1.0, (step - self._warmup_steps) / self._ramp_steps))
+        return start + (end - start) * progress
+
+    def _current_max_range(self, step: int) -> tuple[float, float]:
+        if self._initial_range_param is None:
+            return self._scaled_range(self._current_multiplier(step))
+        progress = self._current_progress(step)
+        lo = self._min_range[0] + (self._full_range[0] - self._min_range[0]) * progress
+        hi = self._min_range[1] + (self._full_range[1] - self._min_range[1]) * progress
+        return (lo, hi)
+
+    def _current_progress(self, step: int) -> float:
+        if self._ramp_steps <= 0:
+            return 1.0
+        return max(0.0, min(1.0, (step - self._warmup_steps) / self._ramp_steps))
+
+
+class command_levels_adaptive:
+    """Adaptive command range curriculum based on average tracking performance."""
+
+    def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRlEnv):
+        p = cfg.params
+        self._command_name: str = p.get("command_name", "twist")
+        self._reward_name: str = p["reward_term_name"]
+        self._axis: str = p["axis"]
+        self._delta_command: float = p.get("delta_command", 0.05)
+        self._target_ratio: float = p.get("target_ratio", 0.8)
+        self._ema_alpha: float = p.get("ema_alpha", 0.5)
+
+        command_term = env.command_manager.get_term(self._command_name)
+        self._cfg = command_term.cfg
+        self._reward_weight = env.reward_manager.get_term_cfg(self._reward_name).weight
+        self._running_mean = 0.0
+
+        # Read the full range configured in the environment configuration
+        self._full_range = self._get_range()
+
+        # Set the command range to initial range at the start
+        self._initial_range = list(p["initial_range"])  # e.g. [-0.5, 0.5]
+        self._current_range = list(self._initial_range)
+        self._set_range(self._current_range)
+
+    def __call__(self, env: ManagerBasedRlEnv, env_ids: torch.Tensor, **kwargs) -> dict[str, torch.Tensor]:
+        if len(env_ids) > 0:
+            episode_sums = env.reward_manager._episode_sums[self._reward_name][env_ids]
+            mean_raw = torch.mean(episode_sums / env.cfg.episode_length_s / self._reward_weight).item()
+            self._running_mean = self._ema_alpha * mean_raw + (1.0 - self._ema_alpha) * self._running_mean
+
+        # Check performance at the end of every episode (or every max_episode_length_s)
+        episode_length_steps = int(env.cfg.episode_length_s / env.step_dt)
+        if env.common_step_counter > 0 and env.common_step_counter % episode_length_steps == 0:
+            # If performance exceeds target ratio (e.g., 0.8), widen the range
+            if self._running_mean > self._target_ratio:
+                # Widen the range
+                lo, hi = self._current_range
+                new_lo = max(self._full_range[0], lo - self._delta_command)
+                new_hi = min(self._full_range[1], hi + self._delta_command)
+                self._current_range = [new_lo, new_hi]
+                self._set_range(self._current_range)
+
+        lo, hi = self._current_range
+        return {
+            f"{self._axis}_range_min": torch.tensor(lo),
+            f"{self._axis}_range_max": torch.tensor(hi),
+            f"{self._axis}_tracking_ema": torch.tensor(self._running_mean),
+            f"{self._axis}_target_ratio": torch.tensor(self._target_ratio),
+        }
+
+    def _get_range(self) -> tuple[float, float]:
+        if self._axis == "x":
+            return tuple(self._cfg.ranges.lin_vel_x)
+        if self._axis == "y":
+            return tuple(self._cfg.ranges.lin_vel_y)
+        if self._axis == "yaw":
+            return tuple(self._cfg.ranges.ang_vel_z)
+        raise ValueError(f"Unknown command curriculum axis: {self._axis}")
+
+    def _set_range(self, value: list[float]) -> None:
+        if self._axis == "x":
+            self._cfg.ranges.lin_vel_x = tuple(value)
+        elif self._axis == "y":
+            self._cfg.ranges.lin_vel_y = tuple(value)
+        elif self._axis == "yaw":
+            self._cfg.ranges.ang_vel_z = tuple(value)
+        else:
+            raise ValueError(f"Unknown command curriculum axis: {self._axis}")
+
+
 def terrain_levels_vel_strict(
     env: ManagerBasedRlEnv,
     env_ids: torch.Tensor,
@@ -163,3 +322,221 @@ def terrain_levels_vel_strict(
 
     return result
 
+
+def terrain_levels_ramp_strict(
+    env: ManagerBasedRlEnv,
+    env_ids: torch.Tensor,
+    command_name: str,
+    ramp_steps: int = 50 * 24,
+    move_up_expected_distance_ratio: float = 0.60,
+    move_down_distance_ratio: float = 0.50,
+    min_command_speed: float = 0.15,
+    asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
+) -> dict[str, torch.Tensor]:
+    """Terrain curriculum active from start, with strict promotion and time-ramped max level."""
+    asset: Entity = env.scene[asset_cfg.name]
+    terrain = env.scene.terrain
+    assert terrain is not None
+    terrain_generator = terrain.cfg.terrain_generator
+    assert terrain_generator is not None
+    assert terrain.terrain_origins is not None
+    assert terrain.env_origins is not None
+
+    command = env.command_manager.get_command(command_name)
+    assert command is not None
+
+    distance = torch.norm(
+        asset.data.root_link_pos_w[env_ids, :2] - env.scene.env_origins[env_ids, :2],
+        dim=1,
+    )
+    cmd_speed = torch.norm(command[env_ids, :2], dim=1)
+    active_command = cmd_speed >= min_command_speed
+
+    expected_distance = cmd_speed * env.max_episode_length_s
+    move_up = (distance > expected_distance * move_up_expected_distance_ratio) & active_command
+    move_down = (
+        (distance < expected_distance * move_down_distance_ratio)
+        & active_command
+        & ~move_up
+    )
+
+    terrain.terrain_levels[env_ids] += 1 * move_up - 1 * move_down
+
+    max_level = max(int(terrain.max_terrain_level) - 1, 0)
+    if ramp_steps <= 0:
+        level_cap = max_level
+    else:
+        progress = max(0.0, min(1.0, env.common_step_counter / ramp_steps))
+        level_cap = int(round(progress * max_level))
+    terrain.terrain_levels[env_ids] = torch.clamp(
+        terrain.terrain_levels[env_ids],
+        min=0,
+        max=min(level_cap, max_level),
+    )
+
+    terrain.env_origins[env_ids] = terrain.terrain_origins[
+        terrain.terrain_levels[env_ids], terrain.terrain_types[env_ids]
+    ]
+
+    levels = terrain.terrain_levels.float()
+    result: dict[str, torch.Tensor] = {
+        "mean": torch.mean(levels),
+        "max": torch.max(levels),
+        "level_cap": torch.tensor(float(level_cap), device=env.device),
+    }
+
+    sub_terrain_names = list(terrain_generator.sub_terrains.keys())
+    num_cols = terrain.terrain_origins.shape[1]
+    if num_cols == len(sub_terrain_names):
+        types = terrain.terrain_types
+        for i, name in enumerate(sub_terrain_names):
+            mask = types == i
+            if mask.any():
+                result[name] = torch.mean(levels[mask])
+
+    return result
+
+
+def terrain_levels_flat_warmup(
+    env: ManagerBasedRlEnv,
+    env_ids: torch.Tensor,
+    command_name: str,
+    warmup_steps: int = 4800,
+    flat_terrain_name: str = "flat",
+    asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
+) -> dict[str, torch.Tensor]:
+    """Keep all reset envs on flat level 0 before enabling terrain curriculum."""
+    terrain = env.scene.terrain
+    assert terrain is not None
+    terrain_generator = terrain.cfg.terrain_generator
+    assert terrain_generator is not None
+    assert terrain.terrain_origins is not None
+    assert terrain.env_origins is not None
+
+    sub_terrain_names = list(terrain_generator.sub_terrains.keys())
+    flat_type = sub_terrain_names.index(flat_terrain_name) if flat_terrain_name in sub_terrain_names else 0
+
+    if env.common_step_counter < warmup_steps:
+        terrain.terrain_levels[env_ids] = 0
+        terrain.terrain_types[env_ids] = flat_type
+        terrain.env_origins[env_ids] = terrain.terrain_origins[0, flat_type]
+        if not hasattr(terrain, "_flat_warmup_env_released"):
+            terrain._flat_warmup_env_released = torch.zeros(
+                env.num_envs, dtype=torch.bool, device=env.device
+            )
+        terrain._flat_warmup_env_released[env_ids] = False
+
+        levels = terrain.terrain_levels.float()
+        result: dict[str, torch.Tensor] = {
+            "mean": torch.mean(levels),
+            "max": torch.max(levels),
+            "warmup_active": torch.ones((), device=env.device),
+        }
+        for i, name in enumerate(sub_terrain_names):
+            mask = terrain.terrain_types == i
+            if mask.any():
+                result[name] = torch.mean(levels[mask])
+        return result
+
+    if not hasattr(terrain, "_flat_warmup_released"):
+        terrain._flat_warmup_released = True
+        terrain._flat_warmup_release_counts = 0
+    if not hasattr(terrain, "_flat_warmup_env_released"):
+        terrain._flat_warmup_env_released = torch.zeros(
+            env.num_envs, dtype=torch.bool, device=env.device
+        )
+
+    newly_released = env_ids[~terrain._flat_warmup_env_released[env_ids]]
+    if len(newly_released) > 0:
+        proportions = torch.tensor(
+            [sub.proportion for sub in terrain_generator.sub_terrains.values()],
+            device=env.device,
+            dtype=torch.float,
+        )
+        proportions = proportions / torch.clamp(proportions.sum(), min=1.0e-6)
+        terrain.terrain_types[newly_released] = torch.multinomial(
+            proportions, len(newly_released), replacement=True
+        )
+        terrain.terrain_levels[newly_released] = 0
+        terrain.env_origins[newly_released] = terrain.terrain_origins[
+            terrain.terrain_levels[newly_released], terrain.terrain_types[newly_released]
+        ]
+        terrain._flat_warmup_env_released[newly_released] = True
+        terrain._flat_warmup_release_counts += len(newly_released)
+
+    result = terrain_levels_vel_strict(env, env_ids, command_name, asset_cfg=asset_cfg)
+    result["warmup_active"] = torch.zeros((), device=env.device)
+    result["released_envs"] = torch.tensor(float(getattr(terrain, "_flat_warmup_release_counts", 0)), device=env.device)
+    return result
+
+
+def terrain_levels_obstacle_release(
+    env: ManagerBasedRlEnv,
+    env_ids: torch.Tensor,
+    command_name: str,
+    release_schedule: tuple[tuple[int, tuple[str, ...]], ...],
+    initial_terrain_names: tuple[str, ...] = ("flat", "random_rough", "perlin_noise", "sloped_terrain"),
+    asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"),
+) -> dict[str, torch.Tensor]:
+    """Release obstacle terrain types gradually while keeping standard level progression."""
+    terrain = env.scene.terrain
+    assert terrain is not None
+    terrain_generator = terrain.cfg.terrain_generator
+    assert terrain_generator is not None
+    assert terrain.terrain_origins is not None
+    assert terrain.env_origins is not None
+
+    sub_terrain_names = list(terrain_generator.sub_terrains.keys())
+    allowed_names = list(initial_terrain_names)
+    for step, names in release_schedule:
+        if env.common_step_counter >= step:
+            allowed_names.extend(names)
+    allowed_type_ids = [
+        sub_terrain_names.index(name) for name in allowed_names if name in sub_terrain_names
+    ]
+    if not allowed_type_ids:
+        allowed_type_ids = [0]
+
+    if not hasattr(terrain, "_obstacle_release_env_allowed"):
+        terrain._obstacle_release_env_allowed = torch.zeros(
+            env.num_envs, dtype=torch.bool, device=env.device
+        )
+        terrain._last_allowed_count = len(allowed_type_ids)
+
+    # If new terrains were released, force all envs to eventually resample upon their next reset
+    if len(allowed_type_ids) > terrain._last_allowed_count:
+        terrain._obstacle_release_env_allowed.fill_(False)
+        terrain._last_allowed_count = len(allowed_type_ids)
+
+    allowed_tensor = torch.tensor(allowed_type_ids, dtype=torch.long, device=env.device)
+    current_allowed = torch.isin(terrain.terrain_types[env_ids], allowed_tensor)
+    need_resample = env_ids[~terrain._obstacle_release_env_allowed[env_ids] | ~current_allowed]
+
+    if len(need_resample) > 0:
+        proportions = torch.tensor(
+            [terrain_generator.sub_terrains[sub_terrain_names[i]].proportion for i in allowed_type_ids],
+            device=env.device,
+            dtype=torch.float,
+        )
+        proportions = proportions / torch.clamp(proportions.sum(), min=1.0e-6)
+        sampled = allowed_tensor[torch.multinomial(proportions, len(need_resample), replacement=True)]
+
+        # Check which envs actually changed terrain type
+        changed_mask = terrain.terrain_types[need_resample] != sampled
+        changed_envs = need_resample[changed_mask]
+
+        terrain.terrain_types[need_resample] = sampled
+        # Only reset the level to 0 if the terrain type was actually changed
+        if len(changed_envs) > 0:
+            terrain.terrain_levels[changed_envs] = 0
+
+        terrain.env_origins[need_resample] = terrain.terrain_origins[
+            terrain.terrain_levels[need_resample], terrain.terrain_types[need_resample]
+        ]
+        terrain._obstacle_release_env_allowed[need_resample] = True
+
+    result = terrain_levels_vel_strict(env, env_ids, command_name, asset_cfg=asset_cfg)
+    result["allowed_types"] = torch.tensor(float(len(allowed_type_ids)), device=env.device)
+    for name in ("pyramid_stairs", "pyramid_stairs_inv", "random_grid", "rc_wall"):
+        result[f"{name}_released"] = torch.tensor(float(name in allowed_names), device=env.device)
+    return result

05_software/train/rc_mjlab/src/robot/mdp/rewards.py

@@ -14,6 +14,11 @@ if TYPE_CHECKING:
     from mjlab.envs import ManagerBasedRlEnv
 
 
+def _finite(value: torch.Tensor, nan: float = 0.0, posinf: float = 0.0, neginf: float = 0.0) -> torch.Tensor:
+    """Keep diagnostic metrics finite when a terminating env has invalid physics state."""
+    return torch.nan_to_num(value, nan=nan, posinf=posinf, neginf=neginf)
+
+
 def track_linear_velocity(
     env: ManagerBasedRlEnv,
     std: float,
@@ -32,6 +37,69 @@ def track_linear_velocity(
     return reward
 
 
+def track_linear_velocity_x(
+    env: ManagerBasedRlEnv,
+    std: float,
+    command_name: str,
+    gravity_z_power: float | None = None,
+    asset_cfg: SceneEntityCfg | None = None,
+) -> torch.Tensor:
+    """LocoLeggedWheel-style independent x velocity tracking reward."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset: Entity = env.scene[asset_cfg.name]
+    command = env.command_manager.get_command(command_name)
+    x_error = torch.square(command[:, 0] - asset.data.root_link_lin_vel_b[:, 0])
+    reward = torch.exp(-x_error / std**2)
+    if gravity_z_power is not None:
+        reward *= torch.clamp(-asset.data.projected_gravity_b[:, 2], min=0.0) ** gravity_z_power
+    else:
+        reward *= -asset.data.projected_gravity_b[:, 2]
+    return reward
+
+
+def track_linear_velocity_y(
+    env: ManagerBasedRlEnv,
+    std: float,
+    command_name: str,
+    gravity_z_power: float | None = None,
+    asset_cfg: SceneEntityCfg | None = None,
+) -> torch.Tensor:
+    """LocoLeggedWheel-style independent y velocity tracking reward."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset: Entity = env.scene[asset_cfg.name]
+    command = env.command_manager.get_command(command_name)
+    y_error = torch.square(command[:, 1] - asset.data.root_link_lin_vel_b[:, 1])
+    reward = torch.exp(-y_error / std**2)
+    if gravity_z_power is not None:
+        reward *= torch.clamp(-asset.data.projected_gravity_b[:, 2], min=0.0) ** gravity_z_power
+    else:
+        reward *= -asset.data.projected_gravity_b[:, 2]
+    return reward
+
+
+def track_angular_velocity_z(
+    env: ManagerBasedRlEnv,
+    std: float,
+    command_name: str,
+    gravity_z_power: float | None = None,
+    asset_cfg: SceneEntityCfg | None = None,
+) -> torch.Tensor:
+    """LocoLeggedWheel-style independent yaw velocity tracking reward."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset: Entity = env.scene[asset_cfg.name]
+    command = env.command_manager.get_command(command_name)
+    z_error = torch.square(command[:, 2] - asset.data.root_link_ang_vel_b[:, 2])
+    reward = torch.exp(-z_error / std**2)
+    if gravity_z_power is not None:
+        reward *= torch.clamp(-asset.data.projected_gravity_b[:, 2], min=0.0) ** gravity_z_power
+    else:
+        reward *= -asset.data.projected_gravity_b[:, 2]
+    return reward
+
+
 def track_angular_velocity(
     env: ManagerBasedRlEnv,
     std: float,
@@ -50,6 +118,37 @@ def track_angular_velocity(
     return reward
 
 
+def stair_lateral_yaw_drift_l2(
+    env: ManagerBasedRlEnv,
+    terrain_names: tuple[str, ...] = ("pyramid_stairs", "pyramid_stairs_inv"),
+    y_scale: float = 1.0,
+    yaw_scale: float = 1.0,
+    asset_cfg: SceneEntityCfg | None = None,
+) -> torch.Tensor:
+    """Penalize sideways velocity and yaw-rate drift only on stair terrains."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset: Entity = env.scene[asset_cfg.name]
+
+    terrain = getattr(env.scene, "terrain", None)
+    terrain_types = getattr(terrain, "terrain_types", None)
+    terrain_cfg = getattr(terrain, "cfg", None)
+    terrain_generator = getattr(terrain_cfg, "terrain_generator", None)
+    if terrain_types is None or terrain_generator is None:
+        return torch.zeros(env.num_envs, device=env.device)
+
+    sub_terrain_names = list(terrain_generator.sub_terrains.keys())
+    mask = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device)
+    for name in terrain_names:
+        if name in sub_terrain_names:
+            mask |= terrain_types == sub_terrain_names.index(name)
+
+    y_vel = asset.data.root_link_lin_vel_b[:, 1]
+    yaw_vel = asset.data.root_link_ang_vel_b[:, 2]
+    penalty = y_scale * torch.square(y_vel) + yaw_scale * torch.square(yaw_vel)
+    return _finite(torch.where(mask, penalty, torch.zeros_like(penalty)))
+
+
 def base_height_l2(
     env: ManagerBasedRlEnv,
     target_height: float = 0.36,
@@ -809,3 +908,238 @@ def pitch_control_penalty(env, max_pitch_rad: float = 0.50, asset_cfg=None) -> t
     excessive_pitch = torch.clamp(torch.abs(g_x) - g_x_threshold, min=0.0)
     reward = torch.square(excessive_pitch)
     return reward
+
+
+def tracking_lin_vel_error(env, command_name: str = "twist", asset_cfg=None) -> torch.Tensor:
+    """Current-step xy velocity tracking error."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset = env.scene[asset_cfg.name]
+    cmd = env.command_manager.get_command(command_name)
+    return _finite(torch.linalg.norm(asset.data.root_link_lin_vel_b[:, :2] - cmd[:, :2], dim=1))
+
+
+def tracking_yaw_vel_error(env, command_name: str = "twist", asset_cfg=None) -> torch.Tensor:
+    """Current-step yaw velocity tracking error."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset = env.scene[asset_cfg.name]
+    cmd = env.command_manager.get_command(command_name)
+    return _finite(torch.abs(asset.data.root_link_ang_vel_b[:, 2] - cmd[:, 2]))
+
+
+def tracking_lin_vel_x_error(env, command_name: str = "twist", asset_cfg=None) -> torch.Tensor:
+    """Current-step x velocity tracking error."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset = env.scene[asset_cfg.name]
+    cmd = env.command_manager.get_command(command_name)
+    return _finite(torch.abs(asset.data.root_link_lin_vel_b[:, 0] - cmd[:, 0]))
+
+
+def tracking_lin_vel_y_error(env, command_name: str = "twist", asset_cfg=None) -> torch.Tensor:
+    """Current-step y velocity tracking error."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset = env.scene[asset_cfg.name]
+    cmd = env.command_manager.get_command(command_name)
+    return _finite(torch.abs(asset.data.root_link_lin_vel_b[:, 1] - cmd[:, 1]))
+
+
+def tracking_lin_vel_along_command_error(
+    env,
+    command_name: str = "twist",
+    asset_cfg: SceneEntityCfg | None = None,
+) -> torch.Tensor:
+    """Error of velocity projected onto the commanded xy direction."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset = env.scene[asset_cfg.name]
+    cmd = env.command_manager.get_command(command_name)
+    cmd_xy = cmd[:, :2]
+    cmd_speed = torch.linalg.norm(cmd_xy, dim=1)
+    direction = cmd_xy / torch.clamp(cmd_speed.unsqueeze(1), min=1.0e-6)
+    actual_along = torch.sum(asset.data.root_link_lin_vel_b[:, :2] * direction, dim=1)
+    return _finite(torch.abs(actual_along - cmd_speed))
+
+
+def actual_lin_vel_orthogonal_command_mean(
+    env,
+    command_name: str = "twist",
+    asset_cfg: SceneEntityCfg | None = None,
+) -> torch.Tensor:
+    """Absolute velocity component perpendicular to the commanded xy direction."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset = env.scene[asset_cfg.name]
+    cmd = env.command_manager.get_command(command_name)
+    cmd_xy = cmd[:, :2]
+    cmd_speed = torch.linalg.norm(cmd_xy, dim=1)
+    direction = cmd_xy / torch.clamp(cmd_speed.unsqueeze(1), min=1.0e-6)
+    actual = asset.data.root_link_lin_vel_b[:, :2]
+    actual_along = torch.sum(actual * direction, dim=1, keepdim=True) * direction
+    orthogonal = actual - actual_along
+    return _finite(torch.linalg.norm(orthogonal, dim=1))
+
+
+def command_lin_vel_mean(env, command_name: str = "twist") -> torch.Tensor:
+    """Current commanded xy speed magnitude."""
+    cmd = env.command_manager.get_command(command_name)
+    return torch.linalg.norm(cmd[:, :2], dim=1)
+
+
+def command_yaw_vel_abs_mean(env, command_name: str = "twist") -> torch.Tensor:
+    """Current commanded yaw speed magnitude."""
+    cmd = env.command_manager.get_command(command_name)
+    return torch.abs(cmd[:, 2])
+
+
+def actual_lin_vel_mean(env, asset_cfg=None) -> torch.Tensor:
+    """Current actual xy speed magnitude."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset = env.scene[asset_cfg.name]
+    return _finite(torch.linalg.norm(asset.data.root_link_lin_vel_b[:, :2], dim=1))
+
+
+def tracking_lin_vel_error_band_mean(
+    env,
+    command_name: str = "twist",
+    min_speed: float = 0.0,
+    max_speed: float = 10.0,
+    asset_cfg=None,
+) -> torch.Tensor:
+    """Broadcast the masked mean xy error for a command speed band."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset = env.scene[asset_cfg.name]
+    cmd = env.command_manager.get_command(command_name)
+    speed = torch.linalg.norm(cmd[:, :2], dim=1)
+    err = _finite(torch.linalg.norm(asset.data.root_link_lin_vel_b[:, :2] - cmd[:, :2], dim=1))
+    active = torch.logical_and(speed >= min_speed, speed < max_speed)
+    denom = active.float().sum().clamp_min(1.0)
+    mean_err = torch.sum(torch.where(active, err, torch.zeros_like(err))) / denom
+    return torch.full_like(err, mean_err)
+
+
+def tracking_lin_vel_axis_error_band_mean(
+    env,
+    axis: int,
+    command_name: str = "twist",
+    min_speed: float = 0.0,
+    max_speed: float = 10.0,
+    asset_cfg=None,
+) -> torch.Tensor:
+    """Broadcast the masked mean x/y error for a command speed band."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset = env.scene[asset_cfg.name]
+    cmd = env.command_manager.get_command(command_name)
+    speed = torch.linalg.norm(cmd[:, :2], dim=1)
+    err = _finite(torch.abs(asset.data.root_link_lin_vel_b[:, axis] - cmd[:, axis]))
+    active = torch.logical_and(speed >= min_speed, speed < max_speed)
+    denom = active.float().sum().clamp_min(1.0)
+    mean_err = torch.sum(torch.where(active, err, torch.zeros_like(err))) / denom
+    return torch.full_like(err, mean_err)
+
+
+def command_band_active(
+    env,
+    command_name: str = "twist",
+    min_speed: float = 0.0,
+    max_speed: float = 10.0,
+) -> torch.Tensor:
+    """Fraction helper for command speed bands."""
+    cmd = env.command_manager.get_command(command_name)
+    speed = torch.linalg.norm(cmd[:, :2], dim=1)
+    return torch.logical_and(speed >= min_speed, speed < max_speed).float()
+
+
+def wheel_raw_action_abs_mean(env, action_name: str = "wheel_joint_vel") -> torch.Tensor:
+    """Mean absolute raw wheel action."""
+    action = env.action_manager.get_term(action_name).raw_action
+    return torch.mean(torch.abs(action), dim=1)
+
+
+def wheel_target_vel_abs_mean(env, action_name: str = "wheel_joint_vel") -> torch.Tensor:
+    """Mean absolute processed wheel velocity target."""
+    term = env.action_manager.get_term(action_name)
+    target = getattr(term, "_processed_actions")
+    return torch.mean(torch.abs(target), dim=1)
+
+
+def wheel_actual_vel_abs_mean(env, asset_cfg: SceneEntityCfg | None = None) -> torch.Tensor:
+    """Mean absolute actual wheel joint velocity."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot", joint_names=(".*_wheel_joint",))
+    asset = env.scene[asset_cfg.name]
+    joint_ids = asset.find_joints(asset_cfg.joint_names)[0]
+    return _finite(torch.mean(torch.abs(asset.data.joint_vel[:, joint_ids]), dim=1))
+
+
+def wheel_target_actual_vel_error_mean(
+    env,
+    action_name: str = "wheel_joint_vel",
+    asset_cfg: SceneEntityCfg | None = None,
+) -> torch.Tensor:
+    """Mean absolute error between processed wheel target and actual wheel velocity."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot", joint_names=(".*_wheel_joint",))
+    term = env.action_manager.get_term(action_name)
+    target = getattr(term, "_processed_actions")
+    asset = env.scene[asset_cfg.name]
+    joint_ids = asset.find_joints(asset_cfg.joint_names)[0]
+    actual = asset.data.joint_vel[:, joint_ids]
+    return _finite(torch.mean(torch.abs(target - actual), dim=1))
+
+
+def wheel_actual_to_target_vel_ratio_mean(
+    env,
+    action_name: str = "wheel_joint_vel",
+    asset_cfg: SceneEntityCfg | None = None,
+) -> torch.Tensor:
+    """Mean |actual wheel velocity| / |target wheel velocity|, clipped for readable logs."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot", joint_names=(".*_wheel_joint",))
+    term = env.action_manager.get_term(action_name)
+    target = getattr(term, "_processed_actions")
+    asset = env.scene[asset_cfg.name]
+    joint_ids = asset.find_joints(asset_cfg.joint_names)[0]
+    actual = asset.data.joint_vel[:, joint_ids]
+    ratio = torch.abs(actual) / torch.clamp(torch.abs(target), min=0.1)
+    return _finite(torch.mean(torch.clamp(ratio, max=3.0), dim=1))
+
+
+def wheel_target_actual_sign_agreement(
+    env,
+    action_name: str = "wheel_joint_vel",
+    asset_cfg: SceneEntityCfg | None = None,
+) -> torch.Tensor:
+    """Fraction of wheel targets and actual velocities with matching sign."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot", joint_names=(".*_wheel_joint",))
+    term = env.action_manager.get_term(action_name)
+    target = getattr(term, "_processed_actions")
+    asset = env.scene[asset_cfg.name]
+    joint_ids = asset.find_joints(asset_cfg.joint_names)[0]
+    actual = asset.data.joint_vel[:, joint_ids]
+    active = torch.abs(target) > 0.1
+    same_sign = torch.sign(target) == torch.sign(actual)
+    return torch.sum((active & same_sign).float(), dim=1) / torch.clamp(torch.sum(active.float(), dim=1), min=1.0)
+
+
+def upright_metric(env, asset_cfg=None) -> torch.Tensor:
+    """1 means upright, 0 means fully inverted according to projected gravity."""
+    if asset_cfg is None:
+        asset_cfg = SceneEntityCfg("robot")
+    asset = env.scene[asset_cfg.name]
+    return torch.clamp(-asset.data.projected_gravity_b[:, 2], 0.0, 1.0)
+
+
+def base_ground_contact_metric(env, sensor_name: str) -> torch.Tensor:
+    """Per-step base contact flag for diagnosing reset-biased metrics."""
+    sensor = env.scene[sensor_name]
+    contact = sensor.data.found > 0
+    while contact.ndim > 1:
+        contact = torch.any(contact, dim=-1)
+    return contact.float()

05_software/train/rc_mjlab/src/robot/robot_cfg.py

@@ -53,11 +53,11 @@ WHEEL_ACTUATOR_CFG = BuiltinVelocityActuatorCfg(
 )
 
 INIT_STATE = EntityCfg.InitialStateCfg(
-    pos=(0.0, 0.0, 0.40),
+    pos=(0.0, 0.0, 0.42),
     joint_pos={
         ".*_hip_abduction_joint": 0.0,
-        ".*_hip_pitch_joint": 0.9,
+        ".*_hip_pitch_joint": 0.550,
-        ".*_knee_joint": -1.8,
+        ".*_knee_joint": -1.125,
         ".*_wheel_joint": 0.0,
     },
     joint_vel={".*": 0.0},

05_software/train/rc_mjlab/src/robot/terrains/competition_terrains.py

@@ -24,9 +24,9 @@ _COLOR_PURPLE = (0.60, 0.20, 0.80)
 
 @dataclass(kw_only=True)
 class RCWallTerrainCfg(SubTerrainCfg):
-    """Triple transverse wall obstacle terrain representing repeated race high walls.
+    """Repeated transverse wall obstacle terrain for high-wall gait training.
 
-    The robot must sprint from the flat platform, vault over three walls, and proceed.
+    The robot must repeatedly step/vault over transverse walls and proceed.
     As difficulty scales from 0 to 1, the wall height increases linearly from
     wall_height_range[0] to wall_height_range[1].
     
@@ -41,9 +41,9 @@ class RCWallTerrainCfg(SubTerrainCfg):
     wall_length_frac: float = 0.8
     """Wall length fraction of the terrain width (leaving gaps for visualization/debugging)."""
     platform_width: float = 1.5
-    """Sprint platform width (m)."""
+    """Nominal start platform width (m)."""
-    wall_centers_x: tuple[float, float, float] = (2.9, 4.45, 6.0)
+    wall_centers_x: tuple[float, ...] = (2.1, 3.2, 4.3, 5.4, 6.5)
-    """Wall center positions along x, spaced to keep a short sprint, two recovery gaps, and exit room."""
+    """Wall center positions along x."""
 
     def function(
         self,
@@ -69,7 +69,7 @@ class RCWallTerrainCfg(SubTerrainCfg):
             origin = np.array([self.size[0] / 2, self.size[1] / 2, 0.0])
             return TerrainOutput(origin=origin, geometries=geometries)
 
-        # -- Wall geometry: three transverse walls oriented along y-axis --
+        # -- Wall geometry: transverse walls oriented along y-axis --
         wall_length = self.wall_length_frac * self.size[1]
         cy = self.size[1] / 2
 
@@ -87,8 +87,7 @@ class RCWallTerrainCfg(SubTerrainCfg):
             )
             geometries.append(TerrainGeometry(geom=wall_geom, color=wall_color))
 
-        # Spawn origin is set to the left platform area to allow a short sprint
+        # Spawn origin is set to the left platform area.
-        # before the first wall and limited recovery space between subsequent walls.
         origin = np.array([1.5, cy, 0.0])
         return TerrainOutput(origin=origin, geometries=geometries)
 

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 部署和比赛版本。机械资料、第一代软件闭环和前两版新版训练配置已经完成整理。
+当前 `16dof` 分支用于整理 16DOF 机械、强化学习训练、Sim2Sim、Sim2Real、ROS 2 部署和比赛版本。机械资料、比赛训练架构、后期 MuJoCo/Sim2Sim 和导航打点工具已经完成整理。
 
 ## 平台概览
 
@@ -35,6 +35,10 @@ RC_WheelLeg/
 - [x] 整理 IK 真机控制与第一代 Python Sim2Real
 - [x] 整理第一份新版 MJCF 与 mjlab 训练框架
 - [x] 整理第二版 Sim2Real 随机化训练配置
+- [x] 整理比赛最终训练代码架构
+- [x] 整理后期 MuJoCo 姿态、IK、动力学和 MPC 工具
+- [x] 整理后期 Sim2Sim、路线检查与比赛 Rough ONNX 策略
+- [x] 整理导航地图、打点工具、路线迭代和抽样 PCD
 - [ ] 核对比赛机械与仿真模型参数
 - [ ] 整理 URDF/MJCF 机器人描述
 - [ ] 整理后续统一训练、ROS 2 和比赛版本