[real] 整理 Python Sim2Real v2

.gitignore

@@ -29,6 +29,7 @@ log/
 !05_software/real/sim2real/vendored/odin1_imu/build/
 !05_software/real/sim2real/vendored/odin1_imu/build/libodin1_imu_bridge.so
 !05_software/real/sim2real/vendored/odin1_imu/lib/*.a
+!05_software/real/sim2real_v2/vendored/odin1_imu/lib/*.a
 
 # Training outputs
 logs/
@@ -36,6 +37,9 @@ checkpoints/
 wandb/
 sim2sim_log_*.txt
 **/sim2sim_temp.xml
+**/route_check_runs/
+**/route_experiments/suite_*/
+**/tools/nav_tools/points/auto_candidates/
 
 # IDE and operating system files
 .idea/

01_doc/version_history.md

@@ -11,6 +11,19 @@
 | `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.9.0` | Python Sim2Real v2 | 反馈新鲜度、Odin odom 诊断、Web 调试和安全监控增强 |
+
+> 原先临时归档为 `v0.9.0` 的最终 ROS 2/C++ 比赛部署已保存在 `backup/final-ros2-v0.9.0` 分支和 `backup-v0.9.0-ros2-final` 标签中，重排完成后将正式归入 `v1.0.0`。
+
+## `v0.9.0` 的 Python Sim2Real v2
+
+- 归档 `real/sim2real_v2` 真机部署版本，保持 `53D -> 16D` 策略观测和动作契约。
+- 增加电机反馈新鲜度判断、Odin odom 诊断、命令限加速度平滑和 Web 运行时诊断。
+- 保留 Python 策略运行时、ONNX/PT 模型、MJCF、Odin 接口、Web 工具和安全保护链路。
+- 排除运行日志、测试日志、临时 XML 和开发交接草稿；后续 ROS 2/C++ 版本另行归档。
 
 ## `v0.4.0` 的模型变化
 
@@ -42,3 +55,29 @@
 - 该 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,10 +5,11 @@
 ```text
 05_software/
 ├─ train/
-│  └─ rc_mjlab/       # 训练、MJCF、Sim2Sim 和本地 mjlab 依赖
+│  └─ rc_mjlab/       # 训练、MJCF、MuJoCo、Sim2Sim 和本地 mjlab 依赖
 └─ real/
    ├─ ik_real/        # IK 轨迹与早期真机控制
-   └─ sim2real/       # 第一代 Python 策略真机部署
+   ├─ sim2real/       # 第一代 Python 策略真机部署
+   └─ sim2real_v2/    # Python Sim2Real v2
 ```
 
 ## 数据流
@@ -18,15 +19,17 @@ MJCF + mjlab task
         |
         v
    PPO 训练策略
+        |
+        +----> MuJoCo 姿态 / IK / MPC 调试
         |
         +----> Sim2Sim 策略验证
         |
-        +----> Python Sim2Real ----> 电机 / IMU
+        +----> Python Sim2Real / v2 ----> 电机 / IMU
 
 IK real --------------------------------> 电机
 ```
 
-`rc_mjlab` 是自包含工程。训练、MJCF、Sim2Sim 和策略权重通过相对路径绑定，因此保留其内部布局，没有为了目录外观拆散。第一代完整闭环见 `v0.3.0`，第一份新版 MJCF 与训练框架见 `v0.4.0`，随机化增强版见 `v0.5.0`，比赛最终训练架构见 `v0.6.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`，Python Sim2Real v2 对应重排主线的 `v0.9.0`。
 
 详细说明见：
 

05_software/real/README.md

@@ -1,6 +1,6 @@
-# 第一代真机控制
+# 真机控制版本演进
 
-本目录保存 16DOF 轮足机器人的早期真机控制实现。
+本目录保存 16DOF 轮足机器人从早期 Python 闭环到 ROS 2 部署的真机控制演进。
 
 ## `ik_real`
 
@@ -20,6 +20,12 @@
 
 部署说明见 [`sim2real/README.md`](sim2real/README.md) 与 [`sim2real/DEPLOYMENT.md`](sim2real/DEPLOYMENT.md)。
 
+## `sim2real_v2`
+
+Python Sim2Real v2，保留 `53D -> 16D` 策略接口，并增加电机反馈新鲜度、Odin odom 诊断、命令平滑、Web 运行时诊断和安全监控工具。该版本对应重排主线的 `v0.9.0`。
+
+部署说明见 [`sim2real_v2/README.md`](sim2real_v2/README.md) 与 [`sim2real_v2/DEPLOYMENT.md`](sim2real_v2/DEPLOYMENT.md)。
+
 ## 实机记录
 
 [![第一代 Sim2Real 真机验证](../../06_assets/images/early_sim2real_preview.jpg)](../../06_assets/videos/early_sim2real.mp4)

05_software/real/sim2real_v2/DEPLOYMENT.md

@@ -0,0 +1,168 @@
+# 真机部署与验证指南
+
+本文档面向当前目录 `shiji/sim2real (10)/sim2real`，用于减少上机排错时间。默认不修改已验证的电机映射、方向、零位和策略观测。
+
+## 1. 前期环境
+
+推荐在 Orin / Linux 上运行：
+
+```bash
+cd sim2real
+python -m pip install -r requirements-orin.txt
+```
+
+确认设备：
+
+```bash
+ip link show can0
+ip link show can1
+ls -l /dev/ttyACM0
+```
+
+确认 `config.yaml`：
+
+- `can1_port` 和 `can2_port` 对应实际 SocketCAN 设备。
+- `control_freq` 默认保持 `50`。
+- `remote.port` 默认 `/dev/ttyACM0`。
+- `policy.action_scale` 必须是 16 维。
+- 默认策略文件优先使用 `policies/model_rough.onnx`；如果只有 `.pt`，先用 `tools/export_onnx.py` 导出。
+- `controller.kp_leg/kd_leg/kd_wheel` 不要在未记录实验的情况下大改。
+
+## 2. 无硬件/低风险检查
+
+先做文件和模型契约检查：
+
+```bash
+python tools/export_onnx.py --pt policies/model_rough.pt --onnx policies/model_rough.onnx
+python tools/alignment_check.py --policy policies/model_rough.onnx --manifest deployment_manifest.yaml
+python tools/standalone_check.py
+python -m py_compile tools/logger.py interface/motor_driver.py interface/imu_client.py interface/real_io.py web/session.py
+```
+
+`alignment_check.py` 会同时检查：
+
+- 策略 obs/action 维度。
+- action scale。
+- default pose。
+- joint order。
+- wheel indices。
+- `config.yaml` 与 `deployment_manifest.yaml` 的控制频率和 command filter 是否一致。
+- ONNX 策略的 obs/action 维度是否仍为 `53D/16D`。
+
+如果有 Node 环境，可检查前端语法：
+
+```bash
+node --check web/static/app.js
+```
+
+## 3. 上电前检查
+
+上电前确认：
+
+- 机器人架空或有可靠支撑。
+- 16 个电机 CAN 线和电源线固定。
+- Odin1 连接稳定，启动时机器人尽量静止，利于重力对齐。
+- Web 急停可见，遥控软急停通道可用。
+- CAN 设备名和遥控串口名与 `config.yaml` 一致。
+
+## 4. Web 启动
+
+```bash
+python web/server.py --host 0.0.0.0 --port 8080
+```
+
+浏览器打开：
+
+```text
+http://<orin-ip>:8080
+```
+
+推荐先只看状态，不急着释放遥控。
+
+## 5. 标准上机流程
+
+1. 点击 `Connect`，确认 IMU/Odin 和 CAN 初始化正常。
+2. 点击 `Enable Motors`，确认 16 个电机都有反馈。
+3. 点击 `Startup`，从当前实测姿态过渡到站立。
+4. 进入 `STAND_HOLD` 后观察 IMU age、Motor Fresh、Loop Profile。
+5. 点击 `Start Runtime`，进入 50Hz 策略循环。
+6. 策略 release 后再点击遥控接管。
+7. 小幅给命令，先测试前后、转向，再测试组合动作。
+
+## 6. 本轮新增诊断如何看
+
+`Loop Profile`：
+
+- `read_state_ms` 高：优先查电机接收、CAN 队列、Odin 获取是否阻塞。
+- `policy_ms` 高：优先查策略推理和 CPU 负载。
+- `send_actions_ms` 高：优先查 CAN 发送和 USB-CAN 适配器。
+- `log_ms` 高：说明日志队列或磁盘仍可能有压力。
+
+`Motor Fresh`：
+
+- 理想状态是 `16/16 (cnt 16, val 0)` 或接近。
+- `cnt` 高说明 `update_count` 正常增长，这是最可靠的电机反馈证据。
+- 如果机器人静止时 `val` 为 0 是正常现象，不应据此判断丢电机。
+
+`Odin Odom`：
+
+- 显示 `STANDARD/HIGHFREQ/TF`、age、local x/y/yaw。
+- 当前只用于诊断和全局坐标显示，不参与策略输入。
+- odom 不可用时，当前 locomotion 仍应可以运行。
+- 如果显示 `JUMP`，说明 odom 局部位置或 yaw 出现突变，先不要把它用于闭环导航。
+
+`Latest Target`：
+
+- 显示当前电机目标来源，例如 `runtime_policy`、`runtime_zero_hold`、`runtime_release_hold`。
+- `age` 应随 runtime 正常刷新；如果明显超过控制周期很多，说明目标更新链路卡住。
+- `d` 是相邻目标最大变化量，可用于观察停车/起步是否有目标突变。
+
+`Obs / Action`：
+
+- `obs` 接近 `100` 时，说明观测可能接近 clip 边界。
+- `raw` 接近 `10` 时，说明策略输出可能接近 raw action 裁剪边界。
+- `scaled` 长期很大时，检查 action scale、目标限幅和 safety clip。
+
+`cmd/raw cmd`：
+
+- `raw cmd` 是 Web/遥控原始输入。
+- `cmd` 是经过 `command_filter` 限加速度后的策略命令。
+- 如果机器人响应慢，先看两者差值是否由命令滤波造成。
+
+## 7. 如果出现前后晃动
+
+先不要直接改控制频率。按顺序排查：
+
+1. 看 `Loop Overruns` 是否增长。
+2. 看 `Loop Profile` 最慢阶段。
+3. 看 `imu_age` 是否超过 30-60ms。
+4. 看 `Motor Fresh` 是否掉到 16 以下。
+5. 看停止时 `cmd` 是否真的回到 0。
+6. 看 `runtime_released`、`release_alpha` 和 `track_err` 是否异常。
+
+只有确认 50Hz 长期跑不稳时，才把 `control_freq: 40` 作为诊断实验，而不是默认方案。训练/部署频率不一致可能引入新的 sim2real gap。
+
+## 8. 日志
+
+每次运行会生成：
+
+- `state.csv`：高频状态流，后台线程写入。
+- `events.jsonl`：事件流，关键事件会即时 flush。
+
+重点搜索：
+
+```bash
+grep LOOP_OVERRUN web/logs/*/events.jsonl
+grep SAFETY web/logs/*/events.jsonl
+grep GUARD web/logs/*/events.jsonl
+```
+
+## 9. 当前不建议改动的内容
+
+- 16 个电机映射、方向、零位。
+- 策略 53D 观测顺序和缩放。
+- 16D 动作顺序和 action scale。
+- 默认 `50Hz` 控制频率。
+- 已经真机跑通过的遥控方向配置。
+
+这些内容只有在有新日志和明确现象时再改，避免把已验证链路打散。

05_software/real/sim2real_v2/FACTS_AND_ASSUMPTIONS.md

@@ -0,0 +1,49 @@
+# `FACTS_AND_ASSUMPTIONS`
+
+## 已确认
+
+- 当前部署模型：默认优先使用 `sim2real/policies/model_rough.onnx`
+- 当前源模型/fallback：`sim2real/policies/model_rough.pt`
+- 源模型：`model_2000.pt`
+- actor 输入：`53D`
+- actor 输出：`16D`
+- 当前 actor 不吃 `base_lin_vel`
+- 当前 actor 不吃 `height_scan`
+
+## 当前观测顺序
+
+1. `base_ang_vel * 0.25`
+2. `projected_gravity`
+3. `command`
+4. `joint_pos_rel`（12）
+5. `joint_vel_rel * 0.05`（12）
+6. `wheel_vel * 0.05`（4）
+7. `last_actions`（16）
+
+## 当前控制定义
+
+- 控制频率：`50Hz`
+- 腿缩放：`0.125 / 0.25`
+- 轮缩放：`5.0`
+- 腿 LPF：`5Hz`
+- 轮 LPF：`15Hz`
+
+## 当前仍依赖现场一致的部分
+
+- IMU 安装方向与上一版校正一致
+- 当前 MJCF / 电机参数对应这次重新训练后的模型
+- 电机零位、方向、接线已按当前硬件修正
+
+## 本次实现边界
+
+不再支持：
+
+- `crawl` 模型
+- 多策略切换
+- `318D` 历史输入
+- 旧版 `startup.start_pose`
+
+## 本次排查结论
+
+代码应只围绕当前 rough 模型运行。  
+如果后续模型结构再改，必须重新核对观测、动作缩放、控制频率和部署文档。

05_software/real/sim2real_v2/ORIN_NANO_DEPLOYMENT.md

@@ -0,0 +1,45 @@
+# `Orin Nano` 部署说明
+
+## 是否必须转 ONNX
+
+不必须。
+
+当前优先级仍然是：
+
+1. 先保证观测、动作、站立控制对齐
+2. 再测 `50Hz` 实际环路稳定性
+3. 最后才决定是否转 `ONNX/TensorRT`
+
+## 当前代码重点
+
+- `stand_balance` 已加入 `main.py` 和 `web/session.py`
+- 启动后先站稳，再允许策略接管
+- `PolicyRunner.step()` 仍保留零命令抑制开关，默认开启
+
+## Orin 上先测什么
+
+- 机器人能否在不启动策略时，仅靠 `startup + stand_balance` 稳定站住
+- `loop_dt_ms`
+- `imu_age_ms`
+- 电机 stale
+- policy forward 耗时
+
+## 纯 Python 部署命令
+
+默认前提：当前目录就是 `sim2real/`
+
+```bash
+python3 -m pip install -r requirements-orin.txt
+python3 tools/alignment_check.py --policy policies/model_rough.pt --manifest deployment_manifest.yaml
+python3 tools/standalone_check.py
+python3 main.py --dry-run
+python3 main.py
+python3 web/server.py --host 0.0.0.0 --port 8080
+```
+
+## 首轮实机建议
+
+1. 先不启动策略
+2. 只验证 `startup -> stand_balance`
+3. 站稳后再启动策略
+4. 只给很小的 `vx / vy / yaw`

05_software/real/sim2real_v2/README.md

@@ -0,0 +1,99 @@
+# Python Sim2Real v2
+
+本目录归档 `real/sim2real_v2` 版本，对应 Python 真机部署从第一代闭环继续演进后的版本节点。该阶段保持 `53D -> 16D` 策略契约，重点完善电机反馈判断、IMU/Odin 可观测性、Web 调试和运行时诊断；后续 ROS 2/C++ 版本另行归档。
+
+## 当前控制链路
+
+- 策略模型：默认优先使用 `policies/model_rough.onnx`，`.pt` 仅作为导出源和 fallback
+- 策略输入：单帧 `53D`
+- 策略输出：`16D`
+- 控制频率：默认 `50Hz`
+- 电机数量：16 个 RS02/RobStride 电机
+- 电机映射、方向、零位：以 `interface/motor_mapping.py` 为准，当前视为真机验证过，不在本轮改造中修改
+- Web 入口：`python web/server.py --host 0.0.0.0 --port 8080`
+
+策略观测顺序保持不变：
+
+1. `base_ang_vel * 0.25`
+2. `projected_gravity`
+3. `command`
+4. `joint_pos[:12] - default_dof_pos[:12]`
+5. `joint_vel[:12] * 0.05`
+6. `wheel_vel[12:] * 0.05`
+7. `last_actions`
+
+Odin odom 现在只作为诊断和全局状态显示接入，不进入策略观测，避免破坏已训练模型的输入契约。
+
+## 本轮改造内容
+
+- 日志写入改为后台线程，降低 `state.csv` 高频写入对 50Hz 控制循环的阻塞。
+- 电机 fresh/stale 判断优先使用驱动层 `update_count`，避免静止电机反馈正常但位置速度不变时被误判为丢电机。
+- `wait_feedback_ready()` 同步识别 `update_count`，降低使能后零位静止电机被误判 missing 的概率。
+- `IMUClient` 增加 `get_latest_odom()`，读取 Odin odom 的位置、姿态、线速度、角速度、类型和 age。
+- `RealIO.read_state()` 增加只读 `odom` 字段，但不改变策略观测。
+- 增加 `OdomTracker`，把 Odin raw odom 转成启动点局部坐标 `local x/y/yaw`，并做跳变检测。
+- 增加 `LatestTarget` 诊断，记录当前下发目标来源、age、目标变化量，为后续双 loop 解耦铺路。
+- Web runtime 状态增加 `loop_profile`、odom、电机 fresh 计数、每电机 update_count。
+- Web Diagnostics 面板增加 loop overrun、最慢阶段、电机 fresh 来源、Odin odom 状态。
+- 增加 `command_filter`，对 Web/遥控命令统一做限加速度平滑，降低起停冲击；Web 同时显示 raw cmd 和 filtered cmd。
+- 策略推理入口改为 ONNXRuntime 优先，默认查找 `policies/model_rough.onnx`；不存在时兼容回退到 `policies/model_rough.pt`。
+- ONNXRuntime 使用 CPU 单线程顺序执行，减少推理线程池和 motor/status/logger 线程抢占。
+
+## 重点诊断字段
+
+Web 中重点看这些项：
+
+- `loop_dt`：控制循环总耗时，50Hz 下目标约 `20ms`。
+- `Loop Overruns`：runtime 超时累计次数和最大超时。
+- `Loop Profile`：显示本轮最慢阶段，例如 `read_state_ms`、`policy_ms`、`send_actions_ms`。
+- `Latest Target`：显示目标来源、age 和本次目标最大变化量；age 异常增大说明目标更新链路卡住。
+- `Obs / Action`：显示观测绝对值最大值、raw action 最大值和 scaled action 最大值，用来发现输入爆炸或动作饱和。
+- `Motor Fresh`：格式为 `fresh/16 (cnt x, val y)`；`cnt` 表示通过 `update_count` 确认的新反馈数量。
+- `Odin Odom`：显示 odom 类型、age、local x/y/yaw；没有 odom 时不影响控制。
+- `cmd/raw cmd`：`cmd` 是进入策略的滤波后命令，`raw cmd` 是 Web/遥控原始命令。
+- 电机列表状态点：绿色代表 stale count 为 0，黄色代表短时未刷新，红色代表连续 stale。
+
+## 当前仍需实机重点确认
+
+- 如果继续出现 `LOOP_OVERRUN`，先看 Web 的 `Loop Profile`，不要直接调低控制频率。
+- 如果 `read_state_ms` 慢，重点排查 SocketCAN/CAN 队列、Odin bridge 或电机反馈处理。
+- 如果 `policy_ms` 慢，重点排查 ONNXRuntime 推理耗时、CPU 占用和是否有后台进程抢占。
+- 如果 `send_actions_ms` 慢，重点排查 CAN 发送阻塞或 USB-CAN 适配器。
+- 如果 `Obs / Action` 中 obs 接近 `100` 或 raw action 接近 `10`，说明策略输入/输出可能在饱和边界，需要优先检查观测缩放、IMU、关节速度和命令。
+- 如果 `Motor Fresh` 不是 16/16，但 `update_counts` 在增长，需要检查 Web stale 阈值而不是电机丢失。
+- 如果 odom age 长时间不更新，只影响全局坐标/诊断，不应影响当前策略控制。
+
+## 启动命令
+
+在 Orin / Linux 真机上：
+
+```bash
+python -m pip install -r requirements-orin.txt
+python tools/export_onnx.py --pt policies/model_rough.pt --onnx policies/model_rough.onnx
+python tools/alignment_check.py --policy policies/model_rough.onnx --manifest deployment_manifest.yaml
+python tools/standalone_check.py
+python web/server.py --host 0.0.0.0 --port 8080
+```
+
+Web 流程：
+
+1. Connect
+2. Enable Motors
+3. Startup
+4. Start Runtime
+5. 策略 release 后，如需要，点击遥控接管
+6. 实时观察 Diagnostics、Motors、Plots
+
+## 安全边界
+
+当前保留原有保护链路：
+
+- Web 急停
+- 遥控软急停
+- runtime guard
+- safety monitor
+- NaN/Inf 检查
+- 电机 stale holdover
+- damping brake
+
+这轮没有放宽安全边界，也没有调整电机限位、零位、方向、默认增益和策略动作缩放。

05_software/real/sim2real_v2/config.yaml

@@ -0,0 +1,113 @@
+can1_port: "can0"
+can2_port: "can1"
+motor_model: "rs-02"
+debug: false
+
+control_freq: 50
+policy_freq: 50
+motor_freq: 200
+status_freq: 10
+policy_stale_warn_ms: 60.0
+policy_timeout_ms: 150.0
+imu_lib_path: null
+
+controller:
+  # Runtime policy gains should stay softer than stand/startup gains. This
+  # follows the same separation used by rl_sar fixed_kp/fixed_kd vs rl_kp/rl_kd
+  # and avoids high-stiffness zero-command oscillation after policy release.
+  kp_leg: 50.0
+  kd_leg: 1.5
+  hold_kp_leg: 80.0
+  hold_kd_leg: 4.0
+  kd_wheel: 1.0
+  max_vx: 0.8
+  max_vy: 0.3
+  max_yaw_rate: 0.5
+
+policy:
+  enable_zero_cmd_suppression: true
+  hold_zero_command_pose: true
+  command_release_s: 0.35
+  require_active_command_to_release: true
+  zero_cmd_use_yaw_rate: false
+  clip_obs: 100.0
+  action_scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0]
+  release_command_hold_s: 0.12
+  release_posture_max_err: 0.35
+  release_target_blend_s: 0.30
+
+stand_balance:
+  enabled: true
+  height: 0.33
+  kp_roll: 0.85
+  pitch_compensation_enabled: false
+  kp_pitch: 0.25
+  kd_roll_rate: 0.03
+  kd_pitch_rate: 0.03
+  pitch_deadband_deg: 2.0
+  pitch_corr_clip: 0.04
+  pitch_corr_filter_alpha: 0.10
+  pitch_front_sign: -1.0
+  lateral_lean_gain: 0.0
+  hip_abduction_clip: 0.45
+  hip_pitch_clip: [-1.0, 2.5]
+  knee_clip: [-2.6, -0.3]
+  stable_roll_deg: 6.0
+  stable_pitch_deg: 8.0
+  stable_gyro_deg_s: 45.0
+  enter_hold_s: 1.0
+  profile_h: [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]
+  profile_hip: [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]
+  profile_knee: [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]
+
+startup:
+  enabled: true
+  wait_for_enter_before_rise: false
+  soft_hold_duration: 1.0
+  ramp_kp_time: 1.0
+  transition_time_min: 2.0
+  transition_time_max: 6.0
+  transition_seconds_per_rad: 1.5
+  timeout_extra: 3.0
+  imu_fresh_wait_s: 1.0
+  hold_time: 1.0
+  settle_pos_threshold: 0.30
+  settle_vel_threshold: 0.6
+  progress_log_interval: 0.5
+  max_dev_warn: 1.5
+  max_dev_abort: 3.0
+  require_user_confirm: true
+
+safety:
+  enabled: true
+  max_target_offset: 0.6
+  hard_target_offset: 1.2
+  max_ang_vel: 10.0
+  max_tilt_z: -0.3
+  clip_to_brake: 0
+  imu_age_warn_ms: 60.0
+  imu_age_stop_ms: 200.0
+
+remote:
+  enabled: true
+  port: "/dev/ttyACM0"
+  baudrate: 100000
+  timeout: 0.02
+  axis_deadzone: 50
+  active_threshold: 50
+  axis_full_scale: 660.0
+  max_vx: 0.3
+  max_vy: 0.3
+  max_yaw_rate: 0.3
+  invert_vx: true
+  invert_vy: false
+  invert_yaw: true
+
+command_filter:
+  enabled: true
+  max_vx_acc: 1.0
+  max_vy_acc: 1.0
+  max_yaw_acc: 1.5
+
+log_dir: "logs"
+log_every: 1

05_software/real/sim2real_v2/deployment_manifest.yaml

@@ -0,0 +1,102 @@
+model:
+  path: "policies/model_rough.onnx"
+  source_pt: "policies/model_rough.pt"
+  backend: "onnxruntime"
+  obs_dim: 53
+  action_dim: 16
+  clip_obs: 100.0
+  enable_zero_cmd_suppression: true
+
+observation:
+  terms:
+    - name: base_ang_vel
+      dim: 3
+      scale: 0.25
+    - name: projected_gravity
+      dim: 3
+    - name: command
+      dim: 3
+    - name: joint_pos_rel
+      dim: 12
+    - name: joint_vel_rel
+      dim: 12
+      scale: 0.05
+    - name: wheel_vel
+      dim: 4
+      scale: 0.05
+    - name: last_actions
+      dim: 16
+
+action:
+  joint_order:
+    - fl_hip_abduction
+    - fl_hip_pitch
+    - fl_knee
+    - fr_hip_abduction
+    - fr_hip_pitch
+    - fr_knee
+    - rl_hip_abduction
+    - rl_hip_pitch
+    - rl_knee
+    - rr_hip_abduction
+    - rr_hip_pitch
+    - rr_knee
+    - fl_wheel
+    - fr_wheel
+    - rl_wheel
+    - rr_wheel
+  wheel_indices: [12, 13, 14, 15]
+  scale:
+    - 0.125
+    - 0.25
+    - 0.25
+    - 0.125
+    - 0.25
+    - 0.25
+    - 0.125
+    - 0.25
+    - 0.25
+    - 0.125
+    - 0.25
+    - 0.25
+    - 5.0
+    - 5.0
+    - 5.0
+    - 5.0
+  default_dof_pos:
+    - 0.0
+    - 0.9
+    - -1.8
+    - 0.0
+    - 0.9
+    - -1.8
+    - 0.0
+    - 0.9
+    - -1.8
+    - 0.0
+    - 0.9
+    - -1.8
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+
+control:
+  control_freq_hz: 50
+  runtime_kp_leg: 50.0
+  runtime_kd_leg: 1.5
+  hold_kp_leg: 80.0
+  hold_kd_leg: 4.0
+  kd_wheel: 1.0
+  leg_lpf_hz: 5
+  wheel_lpf_hz: 15
+  command_filter:
+    enabled: true
+    max_vx_acc: 1.0
+    max_vy_acc: 1.0
+    max_yaw_acc: 1.5
+
+safety:
+  zero_cmd_lin_thresh: 0.05
+  zero_cmd_yaw_thresh: 0.05
+  zero_yaw_rate_thresh: 0.10

05_software/real/sim2real_v2/input_dev/keyboard.py

@@ -0,0 +1,89 @@
+"""键盘控制器 — 兼容 sim2sim/input_dev/keyboard.py 的接口与平滑参数。"""
+import numpy as np
+
+try:
+    from pynput import keyboard
+    PYNPUT_AVAILABLE = True
+except ImportError:
+    PYNPUT_AVAILABLE = False
+    keyboard = None  # type: ignore
+
+
+class KeyboardCommandController:
+    """方向键 + AD 键的键盘指令源。
+    指令: [vx, vy, yaw_rate]，平滑加减速；空格触发急停标志。
+    """
+
+    def __init__(self,
+                 max_x_vel: float = 0.8,
+                 max_y_vel: float = 0.3,
+                 max_yaw_vel: float = 0.5,
+                 acc_step: float = 0.05,
+                 dec_step: float = 0.1):
+        if not PYNPUT_AVAILABLE:
+            raise RuntimeError("pynput 不可用，无法使用键盘控制；改用其他输入源。")
+
+        self.current_cmd = np.zeros(3, dtype=np.float32)
+        self.max_x_vel = max_x_vel
+        self.max_y_vel = max_y_vel
+        self.max_yaw_vel = max_yaw_vel
+        self.acc_step = acc_step
+        self.dec_step = dec_step
+
+        self._pressed = set()
+        self._estop = False
+        self.listener = keyboard.Listener(
+            on_press=self._on_press, on_release=self._on_release
+        )
+
+    def start(self):
+        self.listener.start()
+        print("[Keyboard] 启动。↑↓ 前后, ←→ 转向, A/D 横移, SPACE 急停")
+
+    def stop(self):
+        try:
+            self.listener.stop()
+        except Exception:
+            pass
+
+    def _on_press(self, key):
+        self._pressed.add(key)
+        if key == keyboard.Key.space:
+            self._estop = True
+
+    def _on_release(self, key):
+        self._pressed.discard(key)
+
+    def is_estop_triggered(self) -> bool:
+        return self._estop
+
+    def reset_estop(self):
+        self._estop = False
+
+    def get_command(self) -> np.ndarray:
+        target = np.zeros(3, dtype=np.float32)
+        if keyboard.Key.up in self._pressed:
+            target[0] += self.max_x_vel
+        if keyboard.Key.down in self._pressed:
+            target[0] -= self.max_x_vel
+        if keyboard.Key.left in self._pressed:
+            target[2] += self.max_yaw_vel
+        if keyboard.Key.right in self._pressed:
+            target[2] -= self.max_yaw_vel
+        try:
+            if keyboard.KeyCode.from_char('a') in self._pressed:
+                target[1] += self.max_y_vel
+            if keyboard.KeyCode.from_char('d') in self._pressed:
+                target[1] -= self.max_y_vel
+        except Exception:
+            pass
+
+        for i, max_v in enumerate((self.max_x_vel, self.max_y_vel, self.max_yaw_vel)):
+            step = self.acc_step if target[i] != 0 else self.dec_step
+            if i == 2:
+                step *= 2.0
+            if self.current_cmd[i] < target[i]:
+                self.current_cmd[i] = min(self.current_cmd[i] + step, target[i])
+            else:
+                self.current_cmd[i] = max(self.current_cmd[i] - step, target[i])
+        return self.current_cmd.copy()

05_software/real/sim2real_v2/input_dev/remote_uart.py

@@ -0,0 +1,256 @@
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Optional
+
+import numpy as np
+import serial
+
+SBUS_FRAME_SIZE = 25
+SBUS_RC_MID = 1024
+SBUS_AXIS_SCALE = 660.0
+
+SWITCH_LOW = -1
+SWITCH_MID = 0
+SWITCH_HIGH = 1
+
+
+@dataclass
+class RemoteSwitchState:
+    ch7: int = SWITCH_MID
+
+
+@dataclass
+class RemoteControlState:
+    ch1: int = 0
+    ch2: int = 0
+    ch3: int = 0
+    ch4: int = 0
+    switches: RemoteSwitchState = field(default_factory=RemoteSwitchState)
+    frame_ok: bool = False
+
+    def active_axes(self, threshold: int = 50) -> dict[str, bool]:
+        return {
+            "ch1": abs(self.ch1) > threshold,
+            "ch2": abs(self.ch2) > threshold,
+            "ch3": abs(self.ch3) > threshold,
+            "ch4": abs(self.ch4) > threshold,
+        }
+
+    @property
+    def estop_requested(self) -> bool:
+        return self.switches.ch7 == SWITCH_HIGH
+
+    def as_dict(self) -> dict:
+        return {
+            "ch1": int(self.ch1),
+            "ch2": int(self.ch2),
+            "ch3": int(self.ch3),
+            "ch4": int(self.ch4),
+            "switches": {"ch7": int(self.switches.ch7)},
+            "frame_ok": bool(self.frame_ok),
+            "estop_requested": bool(self.estop_requested),
+        }
+
+
+class RemoteUartReceiver:
+    def __init__(self, port: str, baudrate: int = 100000, timeout: float = 0.02, axis_deadzone: int = 50):
+        self.port = port
+        self.baudrate = int(baudrate)
+        self.timeout = float(timeout)
+        self.axis_deadzone = int(axis_deadzone)
+        self.serial: Optional[serial.Serial] = None
+        self._buffer = bytearray()
+        self.state = RemoteControlState()
+
+    def open(self) -> None:
+        if self.serial and self.serial.is_open:
+            return
+        self.serial = serial.Serial(
+            port=self.port,
+            baudrate=self.baudrate,
+            timeout=self.timeout,
+            bytesize=serial.EIGHTBITS,
+            parity=serial.PARITY_EVEN,
+            stopbits=serial.STOPBITS_TWO,
+        )
+
+    def close(self) -> None:
+        if self.serial and self.serial.is_open:
+            self.serial.close()
+
+    def poll(self) -> RemoteControlState:
+        if not self.serial or not self.serial.is_open:
+            raise RuntimeError("remote uart is not open")
+
+        waiting = self.serial.in_waiting
+        if waiting:
+            self._buffer.extend(self.serial.read(waiting))
+
+        while len(self._buffer) >= SBUS_FRAME_SIZE:
+            start_idx = self._buffer.find(0x0F)
+            if start_idx < 0:
+                self._buffer.clear()
+                break
+            if start_idx > 0:
+                del self._buffer[:start_idx]
+            if len(self._buffer) < SBUS_FRAME_SIZE:
+                break
+            frame = bytes(self._buffer[:SBUS_FRAME_SIZE])
+            del self._buffer[:SBUS_FRAME_SIZE]
+            parsed = self._parse_frame(frame)
+            if parsed is not None:
+                self.state = parsed
+        return self.state
+
+    def _parse_frame(self, frame: bytes) -> Optional[RemoteControlState]:
+        if len(frame) != SBUS_FRAME_SIZE or frame[0] != 0x0F:
+            return None
+
+        channels = [0] * 16
+        channels[0] = (frame[1] | (frame[2] << 8)) & 0x07FF
+        channels[1] = ((frame[2] >> 3) | (frame[3] << 5)) & 0x07FF
+        channels[2] = ((frame[3] >> 6) | (frame[4] << 2) | (frame[5] << 10)) & 0x07FF
+        channels[3] = ((frame[5] >> 1) | (frame[6] << 7)) & 0x07FF
+        channels[4] = ((frame[6] >> 4) | (frame[7] << 4)) & 0x07FF
+        channels[5] = ((frame[7] >> 7) | (frame[8] << 1) | (frame[9] << 9)) & 0x07FF
+        channels[6] = ((frame[9] >> 2) | (frame[10] << 6)) & 0x07FF
+        channels[7] = ((frame[10] >> 5) | (frame[11] << 3)) & 0x07FF
+        channels[8] = (frame[12] | (frame[13] << 8)) & 0x07FF
+        channels[9] = ((frame[13] >> 3) | (frame[14] << 5)) & 0x07FF
+
+        if channels[0] < 100:
+            return None
+
+        state = RemoteControlState(
+            ch1=self._normalize_axis(channels[0]),
+            ch2=self._normalize_axis(channels[1]),
+            ch3=self._normalize_axis(channels[3]),
+            ch4=self._normalize_axis(channels[2]),
+            switches=RemoteSwitchState(ch7=self._decode_switch(channels[6])),
+            frame_ok=True,
+        )
+        if any(abs(value) > 800 for value in (state.ch1, state.ch2, state.ch3, state.ch4)):
+            return None
+        return state
+
+    def _normalize_axis(self, value: int) -> int:
+        mapped = int(round((value - SBUS_RC_MID) * SBUS_AXIS_SCALE / 800.0))
+        return 0 if abs(mapped) <= self.axis_deadzone else mapped
+
+    @staticmethod
+    def _decode_switch(value: int) -> int:
+        if value < 500:
+            return SWITCH_LOW
+        if value > 1500:
+            return SWITCH_HIGH
+        return SWITCH_MID
+
+
+class RemoteCommandMapper:
+    def __init__(
+        self,
+        *,
+        max_vx: float,
+        max_vy: float,
+        max_yaw: float,
+        active_threshold: int = 50,
+        axis_full_scale: float = SBUS_AXIS_SCALE,
+        invert_vx: bool = False,
+        invert_vy: bool = False,
+        invert_yaw: bool = False,
+    ):
+        self.max_vx = float(max_vx)
+        self.max_vy = float(max_vy)
+        self.max_yaw = float(max_yaw)
+        self.active_threshold = int(active_threshold)
+        self.axis_full_scale = max(float(axis_full_scale), 1.0)
+        self.invert_vx = bool(invert_vx)
+        self.invert_vy = bool(invert_vy)
+        self.invert_yaw = bool(invert_yaw)
+
+    def map_command(self, state: RemoteControlState) -> np.ndarray:
+        vx = self._axis_to_velocity(state.ch2, self.max_vx, self.invert_vx)
+        vy = self._axis_to_velocity(state.ch4, self.max_vy, self.invert_vy)
+        yaw = self._axis_to_velocity(state.ch1, self.max_yaw, self.invert_yaw)
+        return np.array([vx, vy, yaw], dtype=np.float32)
+
+    def is_command_active(self, state: RemoteControlState) -> bool:
+        return any(abs(value) > self.active_threshold for value in (state.ch1, state.ch2, state.ch4))
+
+    def _axis_to_velocity(self, raw_value: int, limit: float, invert: bool) -> float:
+        if abs(raw_value) <= self.active_threshold:
+            return 0.0
+        scaled = max(-1.0, min(1.0, raw_value / self.axis_full_scale))
+        if invert:
+            scaled = -scaled
+        return float(scaled * limit)
+
+
+class RemoteCommandSource:
+    def __init__(
+        self,
+        *,
+        port: str,
+        max_vx: float,
+        max_vy: float,
+        max_yaw: float,
+        baudrate: int = 100000,
+        timeout: float = 0.02,
+        axis_deadzone: int = 50,
+        active_threshold: int = 50,
+        axis_full_scale: float = SBUS_AXIS_SCALE,
+        invert_vx: bool = False,
+        invert_vy: bool = False,
+        invert_yaw: bool = False,
+    ):
+        self.receiver = RemoteUartReceiver(
+            port=port,
+            baudrate=baudrate,
+            timeout=timeout,
+            axis_deadzone=axis_deadzone,
+        )
+        self.mapper = RemoteCommandMapper(
+            max_vx=max_vx,
+            max_vy=max_vy,
+            max_yaw=max_yaw,
+            active_threshold=active_threshold,
+            axis_full_scale=axis_full_scale,
+            invert_vx=invert_vx,
+            invert_vy=invert_vy,
+            invert_yaw=invert_yaw,
+        )
+        self.last_state = RemoteControlState()
+        self.last_command = np.zeros(3, dtype=np.float32)
+
+    @property
+    def port(self) -> str:
+        return self.receiver.port
+
+    def open(self) -> None:
+        self.receiver.open()
+
+    def close(self) -> None:
+        self.receiver.close()
+
+    def poll(self) -> RemoteControlState:
+        self.last_state = self.receiver.poll()
+        self.last_command = self.mapper.map_command(self.last_state)
+        return self.last_state
+
+    def get_command(self) -> np.ndarray:
+        return self.last_command.copy()
+
+    def is_command_active(self) -> bool:
+        return self.mapper.is_command_active(self.last_state)
+
+    def get_status(self) -> dict:
+        status = self.last_state.as_dict()
+        status.update(
+            {
+                "port": self.port,
+                "cmd": self.last_command.tolist(),
+                "command_active": bool(self.is_command_active()),
+            }
+        )
+        return status

05_software/real/sim2real_v2/interface/imu_client.py

@@ -0,0 +1,229 @@
+"""Odin1 IMU 客户端封装。
+
+核心改动相对 sim_rl/odin1/python/odin1_imu.py：
+- 自动加载默认 .so 路径，调用方只需要 IMUClient(lib_path=...)
+- 启动后做一次"重力对齐" — 用静止时的加速度计读数初始化 Mahony 滤波器，
+  把首步姿态偏差从可能的 5°+ 降到 0.3° 内。这是方法论 D4 的关键一步。
+- 数据老化检测：若 imu_age_ms > stale_threshold 则报警（不阻塞）。
+"""
+import sys
+import time
+from pathlib import Path
+from typing import Dict, Optional
+
+import numpy as np
+
+
+class IMUClient:
+    """Odin1 IMU 包装。
+
+    Args:
+        lib_path: libodin1_imu_bridge.so 的绝对路径；None 则按方法论 1.2 中
+                  约定的相对位置寻找。
+        gravity_align_samples: 启动时取多少帧加速度计平均值用于姿态初始化
+        stale_threshold_ms: 单帧数据超过该 age 视为陈旧
+    """
+
+    def __init__(self, lib_path: Optional[str] = None, gravity_align_samples: int = 50,
+                 stale_threshold_ms: float = 50.0, dry_run: bool = False):
+        self._mock_mode = dry_run
+        self._initial_gravity = None
+        self._last_seq = -1
+        self._last_fresh_time = 0.0
+        self._last_odom_stamp = -1
+        self._last_odom_fresh_time = 0.0
+
+        if self._mock_mode:
+            print("[IMUClient] 启动 Mock IMU 模式 (不加载物理 IMU 驱动)。")
+            self._client = None
+            return
+
+        # 优先级 1: vendored/odin1_imu（独立部署模式）
+        # 优先级 2: ../../odin1/odin1/python（开发模式，即 sim_rl/odin1/odin1/python）
+        sim2real_root = Path(__file__).resolve().parents[1]
+        candidates = [
+            sim2real_root / "vendored" / "odin1_imu",
+            sim2real_root.parents[1] / "odin1" / "odin1" / "python",
+        ]
+        for cand in candidates:
+            if cand.exists() and str(cand) not in sys.path:
+                sys.path.insert(0, str(cand))
+                break
+        try:
+            from odin1_imu import Odin1ImuClient  # type: ignore
+            # lib_path 默认查找：vendored/odin1_imu/build/libodin1_imu_bridge.so → 开发路径
+            if lib_path is None:
+                so_candidates = [
+                    sim2real_root / "vendored" / "odin1_imu" / "build" / "libodin1_imu_bridge.so",
+                    sim2real_root / "vendored" / "odin1_imu" / "libodin1_imu_bridge.so",
+                    sim2real_root.parents[1] / "odin1" / "odin1" / "build" / "libodin1_imu_bridge.so",
+                ]
+                for so in so_candidates:
+                    if so.exists():
+                        lib_path = str(so)
+                        break
+
+            self._client = Odin1ImuClient(lib_path=lib_path)
+            self._gravity_align_samples = gravity_align_samples
+            self._stale_threshold_ms = stale_threshold_ms
+        except Exception as e:
+            print(f"[IMUClient] 错误: 无法初始化实机 IMU 驱动 ({type(e).__name__}: {e})。实机部署下拒绝启动。")
+            raise
+
+    def version(self) -> str:
+        if self._mock_mode:
+            return "MockIMU-v1.0"
+        return self._client.version()
+
+    def start(self, timeout_ms: int = 8000):
+        """启动 IMU 流，并采集若干帧用于重力对齐。"""
+        if self._mock_mode:
+            self._initial_gravity = np.array([0.0, 0.0, 9.81], dtype=np.float32)
+            self._last_fresh_time = time.time()
+            self._last_odom_fresh_time = time.time()
+            return
+        self._client.start(timeout_ms=timeout_ms)
+        self._wait_for_stream()
+        self._initial_gravity = self._collect_gravity_samples()
+        self._last_fresh_time = time.time()
+
+    def stop(self):
+        if self._mock_mode:
+            return
+        try:
+            self._client.stop()
+        except Exception:
+            pass
+
+    @property
+    def initial_gravity(self) -> Optional[np.ndarray]:
+        """启动后的初始重力向量（机身坐标系），用于初始化 Mahony 四元数。"""
+        return self._initial_gravity
+
+    def get_latest(self):
+        """返回 (gyro[3], accel[3], age_ms, fresh)；fresh=False 表示无新数据。"""
+        if self._mock_mode:
+            now = time.time()
+            age_ms = float(getattr(self, "_debug_mock_age_ms", 0.0))
+            fresh = True
+            return (np.zeros(3, dtype=np.float32),
+                    np.array([0.0, 0.0, 9.81], dtype=np.float32),
+                    age_ms, fresh)
+
+        sample = self._client.get_latest()
+        if sample is None:
+            return (np.zeros(3, dtype=np.float32),
+                    np.array([0.0, 0.0, 9.81], dtype=np.float32),
+                    -1.0, False)
+        gyro = np.array([sample.gyro_x, sample.gyro_y, sample.gyro_z], dtype=np.float32)
+        accel = np.array([sample.accel_x, sample.accel_y, sample.accel_z], dtype=np.float32)
+        # 用 stamp_ns 判断是否有新数据，因为 sequence 字段在 C++ 中可能没有赋值，导致永远为 0
+        stamp = getattr(sample, "stamp_ns", 0)
+        now = time.time()
+        if stamp != self._last_seq:
+            self._last_seq = stamp
+            self._last_fresh_time = now
+            fresh = True
+        else:
+            fresh = False
+        age_ms = (now - self._last_fresh_time) * 1000.0
+        return gyro, accel, age_ms, fresh
+
+    def get_latest_odom(self) -> Optional[Dict[str, object]]:
+        """Return latest Odin odom for diagnostics only; policy observations stay unchanged."""
+        if self._mock_mode:
+            now = time.time()
+            # 模拟一个围绕 (0, 0) 的圆形轨迹，用于测试 Web UI Canvas 绘图
+            theta = now * 0.2
+            x = 0.5 * np.cos(theta)
+            y = 0.5 * np.sin(theta)
+            return {
+                "type": "STANDARD",
+                "stamp_ns": int(now * 1e9),
+                "fresh": True,
+                "age_ms": 0.0,
+                "pos": [float(x), float(y), 0.33],
+                "quat_wxyz": [1.0, 0.0, 0.0, 0.0],
+                "linear_vel": [0.0, 0.0, 0.0],
+                "angular_vel": [0.0, 0.0, 0.2],
+            }
+
+        getter = getattr(self._client, "odom_get_latest", None)
+        if getter is None:
+            return None
+        try:
+            sample = getter()
+        except Exception:
+            return None
+        if sample is None:
+            return None
+
+        stamp = int(getattr(sample, "stamp_ns", 0))
+        now = time.time()
+        if stamp != self._last_odom_stamp:
+            self._last_odom_stamp = stamp
+            self._last_odom_fresh_time = now
+            fresh = True
+        else:
+            fresh = False
+        age_ms = (now - self._last_odom_fresh_time) * 1000.0 if self._last_odom_fresh_time else -1.0
+        return {
+            "type": _odom_type_name(int(getattr(sample, "type", -1))),
+            "stamp_ns": stamp,
+            "fresh": fresh,
+            "age_ms": age_ms,
+            "pos": [
+                float(getattr(sample, "pos_x", 0.0)),
+                float(getattr(sample, "pos_y", 0.0)),
+                float(getattr(sample, "pos_z", 0.0)),
+            ],
+            "quat_wxyz": [
+                float(getattr(sample, "orient_w", 1.0)),
+                float(getattr(sample, "orient_x", 0.0)),
+                float(getattr(sample, "orient_y", 0.0)),
+                float(getattr(sample, "orient_z", 0.0)),
+            ],
+            "linear_vel": [
+                float(getattr(sample, "linear_vel_x", 0.0)),
+                float(getattr(sample, "linear_vel_y", 0.0)),
+                float(getattr(sample, "linear_vel_z", 0.0)),
+            ],
+            "angular_vel": [
+                float(getattr(sample, "angular_vel_x", 0.0)),
+                float(getattr(sample, "angular_vel_y", 0.0)),
+                float(getattr(sample, "angular_vel_z", 0.0)),
+            ],
+        }
+
+    # ---- 内部方法 ----
+    def _wait_for_stream(self, timeout: float = 3.0):
+        deadline = time.time() + timeout
+        while time.time() < deadline:
+            if self._client.wait_for_data(timeout_ms=200):
+                # 有数据进来后清空一次队列以保证后续 get_latest 拿到的都是最新
+                while self._client.pop_sample() is not None:
+                    pass
+                return
+        raise RuntimeError("IMU 启动超时，未收到任何样本")
+
+    def _collect_gravity_samples(self) -> np.ndarray:
+        accels = []
+        for _ in range(self._gravity_align_samples):
+            sample = self._client.pop_sample()
+            if sample is None:
+                if not self._client.wait_for_data(timeout_ms=100):
+                    continue
+                sample = self._client.pop_sample()
+                if sample is None:
+                    continue
+            accels.append([sample.accel_x, sample.accel_y, sample.accel_z])
+        if not accels:
+            print("[IMU] 警告: 重力对齐期间未收到样本，使用默认重力 [0,0,-9.81]")
+            return np.array([0.0, 0.0, -9.81], dtype=np.float32)
+        gravity = np.mean(accels, axis=0).astype(np.float32)
+        print(f"[IMU] 重力对齐完成: g_body = {gravity}")
+        return gravity
+
+
+def _odom_type_name(value: int) -> str:
+    return {0: "STANDARD", 1: "HIGHFREQ", 2: "TF"}.get(value, f"UNKNOWN_{value}")

05_software/real/sim2real_v2/interface/motor_driver.py

@@ -0,0 +1,364 @@
+"""RobStride 电机驱动包装。
+
+职责：
+- 封装 ik_real 中 RobStrideDriver 的 enable/disable/clear/control_mit 调用
+- **真实的丢包检测**：旧版用「value=0 启发式」会误判（电机回机械零位时也是 0）。
+  新方案：
+    1. 调用 process_messages 前快照所有电机的 (pos, vel, torque)
+    2. 调用后比较：状态变了 → 这一帧有新反馈；状态完全没变 → 累计 stale_count
+    3. stale_count 超过阈值才沿用上一帧（方法论 3.4.2）
+  仍然不完美（电机长时间静止确实会有连续多帧 state 不变），但比 0 启发式可靠。
+- 通过 driver_factory 由调用方注入：远程 Linux 主机用 RobStrideDriver，
+  本地 Windows 调试可用 Mock。
+"""
+from dataclasses import dataclass
+import threading
+from typing import Callable, Dict, List, Optional, Tuple
+
+import numpy as np
+
+from interface.motor_mapping import MotorMapping
+
+
+@dataclass
+class MotorReading:
+    position: float
+    velocity: float
+    torque: float = 0.0
+    fresh: bool = False  # True 表示本帧驱动板有新反馈
+
+
+class HardwareIO:
+    """统一的电机+IMU总线接口（不含策略），主控调用这一层。
+
+    Args:
+        driver_factory: () -> (drv1, drv2)，由调用方注入；返回的对象需要满足：
+            connect()/disconnect()/disable(name)/enable(name)/clear_warnings(name)
+            add_motor(name, mid, model)/process_messages()
+            control_mit(name, q, dq, kp, kd, tau)
+            .motors: dict[name -> motor], motor.state.position / .velocity / .torque
+        config: yaml 解析后的字典
+    """
+
+    def __init__(self, driver_factory: Callable[[str, str, bool], Tuple[object, object]],
+                 motor_model: str, can1_port: str, can2_port: str, debug: bool = False,
+                 stale_frames_to_holdover: int = 2):
+        self.mapper = MotorMapping()
+        drv1, drv2 = driver_factory(can1_port, can2_port, debug)
+        self.driver_can1 = drv1
+        self.driver_can2 = drv2
+        self.motor_model = motor_model
+        self.stale_frames_to_holdover = stale_frames_to_holdover
+
+        # 上一帧反馈（按 (bus, can_id) 索引），用于丢包兜底
+        self._last_pos: Dict[Tuple[int, int], float] = {}
+        self._last_vel: Dict[Tuple[int, int], float] = {}
+        self._last_torque: Dict[Tuple[int, int], float] = {}
+        self._last_real_pos: Dict[Tuple[int, int], float] = {}
+        # 每个电机连续多少帧没收到新反馈
+        self._stale_counts: Dict[Tuple[int, int], int] = {}
+        # 第一次必须读到才能解锁，避免初始化时直接用零位发送大力矩
+        self._initialized = False
+
+        self.lock = threading.Lock()
+
+        # 累计诊断
+        self.holdover_total = 0  # 累计被沿用上一帧的次数
+
+    # ---- 总线管理 ----
+    def connect(self):
+        self.driver_can1.connect()
+        self.driver_can2.connect()
+        for jk in self.mapper.SIM_JOINT_ORDER:
+            leg, joint = jk
+            bus, mid = self.mapper.CAN_ID_MAP[jk]
+            name = f"{leg}_{joint}"
+            drv = self.driver_can1 if bus == 1 else self.driver_can2
+            drv.add_motor(name, mid, self.motor_model)
+            self._stale_counts[(bus, mid)] = 0
+
+    def disconnect(self):
+        try:
+            self.driver_can1.disconnect()
+        finally:
+            self.driver_can2.disconnect()
+
+    def enable_all(self):
+        for drv in (self.driver_can1, self.driver_can2):
+            for name in drv.motors:
+                drv.clear_warnings(name)
+                drv.enable(name)
+
+    def disable_all(self):
+        for drv in (self.driver_can1, self.driver_can2):
+            for name in drv.motors:
+                drv.disable(name)
+
+    # ---- 状态读取 ----
+    def _snapshot_state(self) -> Dict[Tuple[int, int], Tuple[float, float, float, int, float, int, int]]:
+        """快照所有电机的 (pos, vel, torque, update_count, temperature, fault_code, mode_state)，process_messages 前后比较即可判 fresh。"""
+        snap: Dict[Tuple[int, int], Tuple[float, float, float, int, float, int, int]] = {}
+        for drv_idx, drv in enumerate((self.driver_can1, self.driver_can2)):
+            bus = drv_idx + 1
+            for name, motor in drv.motors.items():
+                parts = name.split("_", 1)
+                if len(parts) != 2:
+                    continue
+                key = (parts[0], parts[1])
+                if key not in self.mapper.CAN_ID_MAP:
+                    continue
+                _, mid = self.mapper.CAN_ID_MAP[key]
+                s = motor.state
+                snap[(bus, mid)] = (
+                    s.position,
+                    s.velocity,
+                    s.torque,
+                    getattr(s, "update_count", 0),
+                    getattr(s, "temperature", 0.0),
+                    getattr(s, "fault_code", 0),
+                    getattr(s, "mode_state", 0)
+                )
+        return snap
+
+    def read_state(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray, Dict[str, object]]:
+        """返回 (sim_joint_pos[16], sim_joint_vel[16], sim_joint_torque[16], debug_info)。"""
+        with self.lock:
+            # 1) 抓取上一次的状态作为「pre」快照（基线）
+            pre = self._snapshot_state()
+
+            # 2) 拉取本帧反馈
+            self.driver_can1.process_messages()
+            self.driver_can2.process_messages()
+
+            # 3) 抓取「post」快照
+            post = self._snapshot_state()
+
+            # 4) 比较：state 元组变了 → 本帧有新反馈，stale_count 清零；否则 stale_count++
+            per_motor_fresh: Dict[Tuple[int, int], bool] = {}
+            fresh_by_update_count = 0
+            fresh_by_value_change = 0
+            update_counts: Dict[Tuple[int, int], int] = {}
+            for key in post:
+                pre_fields = pre.get(key)
+                post_fields = post[key]
+                pre_count = pre_fields[3] if pre_fields is not None and len(pre_fields) >= 4 else 0
+                post_count = post_fields[3] if len(post_fields) >= 4 else 0
+                update_counts[key] = int(post_count)
+                if post_count > pre_count:
+                    fresh = True
+                    fresh_by_update_count += 1
+                elif pre_fields is not None and post_fields[:3] != pre_fields[:3]:
+                    fresh = True
+                    fresh_by_value_change += 1
+                else:
+                    fresh = False
+                per_motor_fresh[key] = fresh
+                if fresh:
+                    self._stale_counts[key] = 0
+                else:
+                    self._stale_counts[key] += 1
+
+            # 5) 取出本帧 pos/vel；若该电机连续多帧没刷新，沿用上一帧（方法论 3.4.2）
+            real_pos: Dict[Tuple[int, int], float] = {}
+            real_vel: Dict[Tuple[int, int], float] = {}
+            real_torque: Dict[Tuple[int, int], float] = {}
+            real_temp: Dict[Tuple[int, int], float] = {}
+            real_fault: Dict[Tuple[int, int], int] = {}
+            real_mode: Dict[Tuple[int, int], int] = {}
+            holdover_this_frame = 0
+            for key, snap_val in post.items():
+                pos, vel, tor, _, temp, fault, mode = snap_val
+                real_temp[key] = temp
+                real_fault[key] = fault
+                real_mode[key] = mode
+                if (not per_motor_fresh[key]) and self._stale_counts[key] >= self.stale_frames_to_holdover:
+                    # 长时间不刷新视作丢包：沿用上一帧
+                    if key in self._last_pos:
+                        real_pos[key] = self._last_pos[key]
+                        real_vel[key] = self._last_vel[key]
+                        real_torque[key] = self._last_torque[key]
+                        holdover_this_frame += 1
+                    else:
+                        real_pos[key] = pos
+                        real_vel[key] = vel
+                        real_torque[key] = tor
+                else:
+                    real_pos[key] = pos
+                    real_vel[key] = vel
+                    real_torque[key] = tor
+
+            self.holdover_total += holdover_this_frame
+            # 缓存本帧（即便部分是 holdover 也缓存）
+            self._last_pos = real_pos.copy()
+            self._last_vel = real_vel.copy()
+            self._last_torque = real_torque.copy()
+            self._last_real_pos = real_pos.copy()
+            if not self._initialized:
+                self._initialized = True
+
+            cur_pos = self.mapper.real_to_sim(real_pos)
+            cur_vel = self.mapper.real_vel_to_sim(real_vel)
+            cur_torque = self.mapper.real_vel_to_sim(real_torque)
+
+            # 诊断信息
+            stale_max = max(self._stale_counts.values()) if self._stale_counts else 0
+            n_stale_motors = sum(1 for c in self._stale_counts.values()
+                                 if c >= self.stale_frames_to_holdover)
+            # 按 SIM_JOINT_ORDER 排列的每个电机连续丢帧数
+            per_motor_stale = [
+                self._stale_counts.get(self.mapper.CAN_ID_MAP[jk], 99)
+                for jk in self.mapper.SIM_JOINT_ORDER
+            ]
+            return cur_pos, cur_vel, cur_torque, {
+                "holdover_this_frame": holdover_this_frame,
+                "stale_max": stale_max,
+                "n_stale_motors": n_stale_motors,
+                "fresh_count": sum(1 for v in per_motor_fresh.values() if v),
+                "per_motor_stale": per_motor_stale,
+                "fresh_by_update_count": fresh_by_update_count,
+                "fresh_by_value_change": fresh_by_value_change,
+                "update_counts": [
+                    update_counts.get(self.mapper.CAN_ID_MAP[jk], 0)
+                    for jk in self.mapper.SIM_JOINT_ORDER
+                ],
+                "temperatures": [
+                    real_temp.get(self.mapper.CAN_ID_MAP[jk], 0.0)
+                    for jk in self.mapper.SIM_JOINT_ORDER
+                ],
+                "fault_codes": [
+                    real_fault.get(self.mapper.CAN_ID_MAP[jk], 0)
+                    for jk in self.mapper.SIM_JOINT_ORDER
+                ],
+                "mode_states": [
+                    real_mode.get(self.mapper.CAN_ID_MAP[jk], 0)
+                    for jk in self.mapper.SIM_JOINT_ORDER
+                ],
+            }
+        
+    def passive_poll(self):
+        """发送全 0 (0刚度0阻尼0力矩) 的 MIT 指令给所有电机。
+        目的：在 ENABLED 状态下，不产生力矩地索要反馈（因为 RobStride 在 MIT 模式下必须有指令才反馈）。"""
+        with self.lock:
+            for jk in self.mapper.SIM_JOINT_ORDER:
+                bus, mid = self.mapper.CAN_ID_MAP[jk]
+                name = f"{jk[0]}_{jk[1]}"
+                drv = self.driver_can1 if bus == 1 else self.driver_can2
+                if name in drv.motors:
+                    drv.control_mit(name, 0.0, 0.0, 0.0, 0.0, 0.0)
+
+    # ---- 控制下发 ----
+    def send_control(self, target_angles: np.ndarray, kp_leg: float, kd_leg: float,
+                     kd_wheel: float):
+        """与 sim2sim 的 PD 模型对齐：
+        - 腿: position 控制，目标角度由 target_angles[:12] 给出，kp/kd 来自配置
+        - 轮: velocity 控制，目标速度由 target_angles[12:] 给出，kd 阻尼
+        """
+        with self.lock:
+            if target_angles.shape != (16,):
+                raise ValueError("target_angles must be (16,)")
+
+            real_targets = self.mapper.sim_to_real(
+                target_angles.astype(np.float32),
+                current_real_pos=self._last_real_pos,
+            )
+            
+            # 轮毂速度目标暂且用 0，如果 target_angles 里包含了速度，就在 policy 那里处理，
+            # 这里的 target_angles 是 pose 目标，轮毂作为连续旋转关节其实位置控制没有意义。
+            # 为了兼容旧代码，这里构造一个 16 维的 velocity array，只有后 4 个是目标（如果当作速度的话）。
+            vel_targets = np.zeros(16, dtype=np.float32)
+            vel_targets[12:] = target_angles[12:].astype(np.float32)
+            real_wheel = self.mapper.sim_vel_to_real(vel_targets)
+
+            for jk in self.mapper.SIM_JOINT_ORDER:
+                leg, joint = jk
+                bus, mid = self.mapper.CAN_ID_MAP[jk]
+                name = f"{leg}_{joint}"
+                drv = self.driver_can1 if bus == 1 else self.driver_can2
+                if name not in drv.motors:
+                    continue
+
+                if joint == "wheel":
+                    v = real_wheel[(bus, mid)]
+                    drv.control_mit(name, 0.0, v, 0.0, kd_wheel, 0.0)
+                else:
+                    q = real_targets[(bus, mid)]
+                    drv.control_mit(name, q, 0.0, kp_leg, kd_leg, 0.0)
+
+    def damping_brake(self, kd_leg: float, kd_wheel: float):
+        """急停模式：所有关节卸载刚度，仅保留阻尼。
+        对应 270_SimToReal 方法论 97.11 Level 2 "刹车"。
+        """
+        with self.lock:
+            for jk in self.mapper.SIM_JOINT_ORDER:
+                leg, joint = jk
+                bus, _ = self.mapper.CAN_ID_MAP[jk]
+                name = f"{leg}_{joint}"
+                drv = self.driver_can1 if bus == 1 else self.driver_can2
+                if name not in drv.motors:
+                    continue
+                kd = kd_wheel if joint == "wheel" else kd_leg
+                drv.control_mit(name, 0.0, 0.0, 0.0, kd, 0.0)
+
+    def wait_feedback_ready(self, max_attempts: int = 20,
+                            poll_interval: float = 0.05) -> Tuple[bool, list]:
+        """enable 后调用：尝试 max_attempts 次读总线，等所有 16 个电机
+        都至少给出一帧反馈。
+        返回 (all_ready, missing_motors)；missing_motors 是 (bus, mid, name) 列表。
+        """
+        import time
+        seen: Dict[Tuple[int, int], bool] = {
+            self.mapper.CAN_ID_MAP[jk]: False for jk in self.mapper.SIM_JOINT_ORDER
+        }
+        # 用第一次读到的 (pos, vel, torque) 三元组的"非零"或"已变化"作为反馈到达的判据。
+        # 启动瞬间所有 motor.state 默认全 0，要么收到反馈让其变化，要么收到反馈但值确实是 0。
+        # 退化情况下电机静止时 vel=0 且 pos=机械零位也=0，那种情况只能等多帧确认。
+        snap_prev = self._snapshot_state()
+        for attempt in range(max_attempts):
+            with self.lock:
+                self.driver_can1.process_messages()
+                self.driver_can2.process_messages()
+                snap_cur = self._snapshot_state()
+            for key, fields_cur in snap_cur.items():
+                if seen[key]:
+                    continue
+                fields_prev = snap_prev.get(key)
+                # 任一字段不为 0 → 一定有反馈（因为初始值都是 0）
+                if any(v != 0.0 for v in fields_cur):
+                    seen[key] = True
+                # 与上一次快照不同 → 一定有反馈（即便都很小）
+                elif fields_prev is not None and (fields_cur[3] > fields_prev[3] or fields_cur != fields_prev):
+                    seen[key] = True
+            snap_prev = snap_cur
+            if all(seen.values()):
+                return True, []
+            time.sleep(poll_interval)
+
+        # 超时：列出仍未反馈的电机
+        missing = []
+        rev_can = {v: k for k, v in self.mapper.CAN_ID_MAP.items()}
+        for key, ok in seen.items():
+            if not ok:
+                leg, joint = rev_can[key]
+                missing.append((key[0], key[1], f"{leg}_{joint}"))
+        return False, missing
+
+    def read_measured_pose(self) -> np.ndarray:
+        """返回 (16,) 当前实测 sim 坐标系下的关节位置。
+        会先 process_messages 一次保证拿到本帧。
+        """
+        self.driver_can1.process_messages()
+        self.driver_can2.process_messages()
+        real_pos: Dict[Tuple[int, int], float] = {}
+        for drv_idx, drv in enumerate((self.driver_can1, self.driver_can2)):
+            bus = drv_idx + 1
+            for name, motor in drv.motors.items():
+                parts = name.split("_", 1)
+                if len(parts) != 2:
+                    continue
+                key = (parts[0], parts[1])
+                if key not in self.mapper.CAN_ID_MAP:
+                    continue
+                _, mid = self.mapper.CAN_ID_MAP[key]
+                real_pos[(bus, mid)] = motor.state.position
+        self._last_real_pos = real_pos.copy()
+        return self.mapper.real_to_sim(real_pos)

05_software/real/sim2real_v2/interface/motor_mapping.py

@@ -0,0 +1,123 @@
+"""仿真→实机电机映射。
+
+数据来源：sim_rl/ik_real/sim_to_real_deploy_beifen.py 和
+sim_rl/sim2real/motor_mapping.py 中的 sign / offset / can_id 表（已在实机上验证）。
+关节顺序与 rc_mjlab/sim2sim 完全一致：[12 个腿关节] + [4 个轮子]。
+"""
+from typing import Dict, Tuple
+
+import numpy as np
+
+
+class MotorMapping:
+    TWO_PI = float(2.0 * np.pi)
+    LEG_NAMES = ("fl", "fr", "rl", "rr")
+    JOINT_NAMES = ("hip_abduction", "hip_pitch", "knee", "wheel")
+
+    SIM_JOINT_ORDER = (
+        ("fl", "hip_abduction"), ("fl", "hip_pitch"), ("fl", "knee"),
+        ("fr", "hip_abduction"), ("fr", "hip_pitch"), ("fr", "knee"),
+        ("rl", "hip_abduction"), ("rl", "hip_pitch"), ("rl", "knee"),
+        ("rr", "hip_abduction"), ("rr", "hip_pitch"), ("rr", "knee"),
+        ("fl", "wheel"), ("fr", "wheel"), ("rl", "wheel"), ("rr", "wheel"),
+    )
+    SIM_INDEX_MAP = {jk: i for i, jk in enumerate(SIM_JOINT_ORDER)}
+
+    CAN_ID_MAP: Dict[Tuple[str, str], Tuple[int, int]] = {
+        ("fl", "hip_abduction"): (1, 1), ("fl", "hip_pitch"): (1, 2),
+        ("fl", "knee"): (1, 3),          ("fl", "wheel"): (1, 4),
+        ("fr", "hip_abduction"): (1, 5), ("fr", "hip_pitch"): (1, 6),
+        ("fr", "knee"): (1, 7),          ("fr", "wheel"): (1, 8),
+        ("rl", "hip_abduction"): (2, 1), ("rl", "hip_pitch"): (2, 2),
+        ("rl", "knee"): (2, 3),          ("rl", "wheel"): (2, 4),
+        ("rr", "hip_abduction"): (2, 5), ("rr", "hip_pitch"): (2, 6),
+        ("rr", "knee"): (2, 7),          ("rr", "wheel"): (2, 8),
+    }
+
+    DIRECTION_MAP: Dict[Tuple[str, str], int] = {
+        ("fl", "hip_abduction"): -1, ("fl", "hip_pitch"): -1,
+        ("fl", "knee"): -1,          ("fl", "wheel"): -1,
+        ("fr", "hip_abduction"): -1, ("fr", "hip_pitch"):  1,
+        ("fr", "knee"):  1,          ("fr", "wheel"):  1,
+        ("rl", "hip_abduction"):  1, ("rl", "hip_pitch"): -1,
+        ("rl", "knee"): -1,          ("rl", "wheel"): -1,
+        ("rr", "hip_abduction"):  1, ("rr", "hip_pitch"):  1,
+        ("rr", "knee"):  1,          ("rr", "wheel"):  1,
+    }
+
+    ZERO_OFFSET_MAP: Dict[Tuple[str, str], float] = {
+        ("fl", "hip_abduction"):  0.003, ("fl", "hip_pitch"):  0.030,
+        ("fl", "knee"):           0.028, ("fl", "wheel"):      0.000,
+        ("fr", "hip_abduction"):  0.004, ("fr", "hip_pitch"):  0.038,
+        ("fr", "knee"):           0.011, ("fr", "wheel"):      0.000,
+        ("rl", "hip_abduction"):  0.019, ("rl", "hip_pitch"): -0.034,
+        ("rl", "knee"):           0.025, ("rl", "wheel"):      0.000,
+        ("rr", "hip_abduction"): -0.001, ("rr", "hip_pitch"):  0.039,
+        ("rr", "knee"):           0.018, ("rr", "wheel"):      0.000,
+    }
+
+    def __init__(self):
+        self.num_motors = len(self.SIM_JOINT_ORDER)
+        self._sign = np.array([self.DIRECTION_MAP[jk] for jk in self.SIM_JOINT_ORDER], dtype=np.float32)
+        self._offset = np.array([self.ZERO_OFFSET_MAP[jk] for jk in self.SIM_JOINT_ORDER], dtype=np.float32)
+        self._default_pose = np.array(
+            [0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.0, 0.0, 0.0],
+            dtype=np.float32,
+        )
+
+    @classmethod
+    def _nearest_periodic(cls, value: float, reference: float) -> float:
+        return float(reference + ((value - reference + np.pi) % cls.TWO_PI - np.pi))
+
+    def canonicalize_sim(self, sim_angles: np.ndarray, reference: np.ndarray | None = None) -> np.ndarray:
+        """Map leg joint angles onto the 2*pi branch nearest the deployment reference pose."""
+        out = np.asarray(sim_angles, dtype=np.float32).copy()
+        ref = self._default_pose if reference is None else np.asarray(reference, dtype=np.float32)
+        for i, (_, joint) in enumerate(self.SIM_JOINT_ORDER[:12]):
+            out[i] = self._nearest_periodic(float(out[i]), float(ref[i]))
+        return out
+
+    def sim_to_real(
+        self,
+        sim_angles: np.ndarray,
+        current_real_pos: Dict[Tuple[int, int], float] | None = None,
+    ) -> Dict[Tuple[int, int], float]:
+        if len(sim_angles) != 16:
+            raise ValueError(f"expected 16 sim angles, got {len(sim_angles)}")
+        out: Dict[Tuple[int, int], float] = {}
+        for i, jk in enumerate(self.SIM_JOINT_ORDER):
+            real = float(self._sign[i] * sim_angles[i] + self._offset[i])
+            can_key = self.CAN_ID_MAP[jk]
+            if current_real_pos is not None and i < 12 and can_key in current_real_pos:
+                real = self._nearest_periodic(real, float(current_real_pos[can_key]))
+            out[can_key] = real
+        return out
+
+    def sim_vel_to_real(self, sim_vels: np.ndarray) -> Dict[Tuple[int, int], float]:
+        # 速度只受方向影响，不应用 offset。
+        out: Dict[Tuple[int, int], float] = {}
+        for i, jk in enumerate(self.SIM_JOINT_ORDER):
+            out[self.CAN_ID_MAP[jk]] = float(self._sign[i] * sim_vels[i])
+        return out
+
+    def real_to_sim(self, real_pos: Dict[Tuple[int, int], float]) -> np.ndarray:
+        out = np.zeros(16, dtype=np.float32)
+        for i, jk in enumerate(self.SIM_JOINT_ORDER):
+            v = real_pos.get(self.CAN_ID_MAP[jk])
+            if v is None:
+                continue
+            out[i] = (v - self._offset[i]) / self._sign[i]
+        return self.canonicalize_sim(out)
+
+    def real_vel_to_sim(self, real_vel: Dict[Tuple[int, int], float]) -> np.ndarray:
+        out = np.zeros(16, dtype=np.float32)
+        for i, jk in enumerate(self.SIM_JOINT_ORDER):
+            v = real_vel.get(self.CAN_ID_MAP[jk])
+            if v is None:
+                continue
+            out[i] = v / self._sign[i]
+        return out
+
+    def joint_name_at(self, idx: int) -> str:
+        leg, joint = self.SIM_JOINT_ORDER[idx]
+        return f"{leg}_{joint}_joint"

05_software/real/sim2real_v2/interface/real_io.py

@@ -0,0 +1,216 @@
+import time
+from typing import Callable, Dict, Tuple
+
+import numpy as np
+
+from interface.imu_client import IMUClient
+from interface.motor_driver import HardwareIO
+from tools.math_utils import LowPassFilter, MahonyFilter, get_gravity_orientation
+
+
+def _quat_yaw_wxyz(quat) -> float:
+    w, x, y, z = [float(v) for v in quat]
+    return float(np.arctan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)))
+
+
+def _wrap_pi(angle: float) -> float:
+    return float((angle + np.pi) % (2.0 * np.pi) - np.pi)
+
+
+class OdomTracker:
+    def __init__(self, jump_distance_m: float = 0.5, jump_yaw_rad: float = 0.8):
+        self.jump_distance_m = float(jump_distance_m)
+        self.jump_yaw_rad = float(jump_yaw_rad)
+        self.origin_pos = None
+        self.origin_yaw = 0.0
+        self.last_local_pos = None
+        self.last_local_yaw = 0.0
+
+    def reset(self):
+        self.origin_pos = None
+        self.origin_yaw = 0.0
+        self.last_local_pos = None
+        self.last_local_yaw = 0.0
+
+    def update(self, odom):
+        if odom is None:
+            return None
+        pos = np.asarray(odom.get("pos", [0.0, 0.0, 0.0]), dtype=np.float32)
+        yaw = _quat_yaw_wxyz(odom.get("quat_wxyz", [1.0, 0.0, 0.0, 0.0]))
+        if self.origin_pos is None:
+            self.origin_pos = pos.copy()
+            self.origin_yaw = yaw
+        local_pos = pos - self.origin_pos
+        local_yaw = _wrap_pi(yaw - self.origin_yaw)
+        jump_detected = False
+        jump_distance = 0.0
+        jump_yaw = 0.0
+        if self.last_local_pos is not None:
+            jump_distance = float(np.linalg.norm(local_pos[:2] - self.last_local_pos[:2]))
+            jump_yaw = abs(_wrap_pi(local_yaw - self.last_local_yaw))
+            jump_detected = jump_distance > self.jump_distance_m or jump_yaw > self.jump_yaw_rad
+        self.last_local_pos = local_pos.copy()
+        self.last_local_yaw = local_yaw
+        tracked = dict(odom)
+        tracked.update(
+            {
+                "local_pos": local_pos.tolist(),
+                "local_yaw": local_yaw,
+                "jump_detected": bool(jump_detected),
+                "jump_distance_m": jump_distance,
+                "jump_yaw_rad": jump_yaw,
+            }
+        )
+        return tracked
+
+
+class RealIO:
+    def __init__(
+        self,
+        driver_factory: Callable[[str, str, bool], Tuple[object, object]],
+        motor_model: str,
+        can1_port: str,
+        can2_port: str,
+        imu_lib_path: str,
+        control_dt: float = 0.02,
+        motor_dt: float = 0.005,
+        kp_leg: float = 80.0,
+        kd_leg: float = 2.5,
+        hold_kp_leg: float | None = None,
+        hold_kd_leg: float | None = None,
+        kd_wheel: float = 2.0,
+        debug: bool = False,
+        dry_run: bool = False,
+    ):
+        self.control_dt = control_dt
+        self.motor_dt = motor_dt
+        self.kp_leg = kp_leg
+        self.kd_leg = kd_leg
+        self.hold_kp_leg = kp_leg if hold_kp_leg is None else float(hold_kp_leg)
+        self.hold_kd_leg = kd_leg if hold_kd_leg is None else float(hold_kd_leg)
+        self.kd_wheel = kd_wheel
+
+        print("[RealIO] 初始化电机驱动...")
+        self.hw = HardwareIO(driver_factory, motor_model, can1_port, can2_port, debug)
+        print("[RealIO] 初始化 IMU...")
+        self.imu = IMUClient(lib_path=imu_lib_path, dry_run=dry_run)
+
+        # 使用 motor_dt 初始化滤波器，因为它们都在 200Hz 电机控制循环中更新
+        self.imu_filter = MahonyFilter(kp=2.0, ki=0.0, dt=motor_dt)
+        self.quat_wxyz = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32)
+
+        self.lpf_legs = LowPassFilter(cutoff_freq=5.0, dt=motor_dt, dim=12)
+        self.lpf_wheels = LowPassFilter(cutoff_freq=15.0, dt=motor_dt, dim=4)
+
+        self._last_imu_age_ms = -1.0
+        self._last_imu_fresh = False
+        self.odom_tracker = OdomTracker()
+        self._last_read_time = None
+
+    def connect(self, imu_timeout_ms: int = 8000):
+        self.hw.connect()
+        self.imu.start(timeout_ms=imu_timeout_ms)
+        if self.imu.initial_gravity is not None:
+            self.imu_filter.reset_with_accel(self.imu.initial_gravity)
+            self.quat_wxyz = self.imu_filter.q.copy()
+        self.odom_tracker.reset()
+        self._last_read_time = None
+
+    def disconnect(self):
+        try:
+            self.hw.disable_all()
+        finally:
+            self.imu.stop()
+            self.hw.disconnect()
+
+    def enable_motors(self):
+        self.hw.enable_all()
+
+    def disable_motors(self):
+        self.hw.disable_all()
+
+    def damping_brake(self):
+        self.hw.damping_brake(self.kd_leg, self.kd_wheel)
+
+    def wait_feedback_ready(self, max_attempts: int = 20, poll_interval: float = 0.05):
+        return self.hw.wait_feedback_ready(max_attempts=max_attempts, poll_interval=poll_interval)
+
+    def read_measured_pose(self) -> np.ndarray:
+        return self.hw.read_measured_pose()
+
+    def read_state(self) -> Dict[str, object]:
+        joint_pos, joint_vel, joint_torque, motor_diag = self.hw.read_state()
+        gyro, accel, age_ms, fresh = self.imu.get_latest()
+        odom = self.odom_tracker.update(self.imu.get_latest_odom())
+        self._last_imu_age_ms = age_ms
+        self._last_imu_fresh = fresh
+
+        # 动态测量 dt，以适应 POLL (5Hz) 与 RUNTIME (200Hz) 的不同频率切换
+        t_now = time.perf_counter()
+        if self._last_read_time is not None:
+            dt = t_now - self._last_read_time
+            if dt <= 0.0 or dt > 0.5:
+                dt = self.motor_dt
+        else:
+            dt = self.motor_dt
+        self._last_read_time = t_now
+
+        self.quat_wxyz = self.imu_filter.update(accel, gyro, dt=dt)
+        projected_gravity = get_gravity_orientation(self.quat_wxyz)
+
+        return {
+            "joint_pos": joint_pos,
+            "joint_vel": joint_vel,
+            "joint_torque": joint_torque,
+            "imu_gyro": gyro,
+            "imu_accel": accel,
+            "quat_wxyz": self.quat_wxyz.copy(),
+            "projected_gravity": projected_gravity,
+            "imu_age_ms": age_ms,
+            "imu_fresh": fresh,
+            "odom": odom,
+            "motor_stale": motor_diag,
+        }
+
+    def get_obs_policy(
+        self,
+        state: Dict[str, object],
+        command: np.ndarray,
+        default_dof_pos: np.ndarray,
+        last_actions_raw: np.ndarray,
+    ) -> np.ndarray:
+        gyro = state["imu_gyro"]
+        joint_pos = state["joint_pos"]
+        joint_vel = state["joint_vel"]
+        projected_gravity = state["projected_gravity"]
+
+        base_ang_vel = (gyro * 0.25).astype(np.float32)
+        joint_pos_rel = (joint_pos[:12] - default_dof_pos[:12]).astype(np.float32)
+        joint_vel_leg = (joint_vel[:12] * 0.05).astype(np.float32)
+        wheel_vel = (joint_vel[12:] * 0.05).astype(np.float32)
+
+        return np.concatenate(
+            [
+                base_ang_vel,
+                projected_gravity,
+                command.astype(np.float32),
+                joint_pos_rel,
+                joint_vel_leg,
+                wheel_vel,
+                last_actions_raw,
+            ]
+        ).astype(np.float32)
+
+    def send_actions(self, scaled_actions: np.ndarray, default_dof_pos: np.ndarray):
+        act = (scaled_actions + default_dof_pos).astype(np.float32)
+        act = np.clip(act, -100.0, 100.0)
+        act[:12] = self.lpf_legs.filter(act[:12])
+        act[12:] = self.lpf_wheels.filter(act[12:])
+        self.hw.send_control(act, self.kp_leg, self.kd_leg, self.kd_wheel)
+        return act
+
+    def hold_pose(self, sim_target_pose: np.ndarray, kp_scale: float = 1.0):
+        target = np.clip(sim_target_pose.astype(np.float32), -100.0, 100.0)
+        kp_scale = float(np.clip(kp_scale, 0.0, 1.0))
+        self.hw.send_control(target, self.hold_kp_leg * kp_scale, self.hold_kd_leg, self.kd_wheel)
+        return target

05_software/real/sim2real_v2/main.py

@@ -0,0 +1,727 @@
+"""CLI entrypoint for current sim2real deployment."""
+
+import argparse
+import os
+import sys
+import threading
+import time
+from pathlib import Path
+
+import numpy as np
+import yaml
+
+sys.path.insert(0, str(Path(__file__).resolve().parent))
+
+from input_dev.keyboard import KeyboardCommandController
+from interface.real_io import RealIO
+from policy.policy_runner import PolicyRunner, resolve_policy_path
+from safety.runtime_guard import GuardLevel, RuntimeGuard
+from safety.safety_monitor import SafetyLevel, SafetyMonitor
+from startup.pose_initializer import PoseInitFailed, PoseInitializer, STAND_POSE
+from startup.stand_balance import StandBalanceController
+from tools.logger import LogBundle
+from tools.math_utils import get_gravity_orientation
+
+JOINT_LABELS = LogBundle.JOINT_LABELS
+
+
+def make_real_driver_factory():
+    sim2real_root = Path(__file__).resolve().parent
+    workspace_root = sim2real_root.parent.parent
+    for path in (workspace_root,):
+        path_str = str(path)
+        if path_str not in sys.path and path.exists():
+            sys.path.append(path_str)
+
+    from dm_socket.sim2real_factory import driver_factory
+
+    return driver_factory
+
+
+def make_dry_driver_factory():
+    class MockMotor:
+        def __init__(self):
+            class State:
+                position = 0.0
+                velocity = 0.0
+                torque = 0.0
+
+            self.state = State()
+
+    class MockDriver:
+        def __init__(self, port, debug):
+            self.port = port
+            self.motors = {}
+
+        def connect(self): ...
+        def disconnect(self): ...
+        def add_motor(self, name, motor_id, model): self.motors[name] = MockMotor()
+        def enable(self, name): ...
+        def disable(self, name): ...
+        def clear_warnings(self, name): ...
+        def process_messages(self): ...
+        def control_mit(self, *args, **kwargs): ...
+
+    def factory(can1_port, can2_port, debug):
+        return MockDriver(can1_port, debug), MockDriver(can2_port, debug)
+
+    return factory
+
+
+def _sleep_to(next_exec: float) -> float:
+    slack = next_exec - time.perf_counter()
+    if slack > 0:
+        time.sleep(slack)
+        return next_exec + 0.0
+    return time.perf_counter()
+
+
+def build_action_diag(
+    *,
+    joint_pos: np.ndarray,
+    default_pose: np.ndarray,
+    raw: np.ndarray,
+    scaled: np.ndarray,
+    tentative: np.ndarray,
+    cmd: np.ndarray,
+    zero_command: bool,
+    runtime_released: bool,
+    release_alpha: float,
+    safety_details: dict | None = None,
+) -> dict:
+    details = dict(safety_details or {})
+    joint_indices = list(details.get("joint_indices", []))
+    pos_err = tentative - joint_pos
+    leg_offset = tentative[:12] - default_pose[:12]
+    diag = {
+        "joint_indices": joint_indices,
+        "joint_names": [JOINT_LABELS[i] for i in joint_indices if 0 <= i < len(JOINT_LABELS)],
+        "cmd": cmd.tolist(),
+        "zero_command": bool(zero_command),
+        "runtime_released": bool(runtime_released),
+        "release_alpha": float(release_alpha),
+        "max_raw": float(np.max(np.abs(raw))) if raw.size else 0.0,
+        "max_scaled": float(np.max(np.abs(scaled[:12]))) if scaled.size else 0.0,
+        "max_target": float(np.max(np.abs(tentative[:12]))) if tentative.size else 0.0,
+    }
+    if joint_indices:
+        primary = int(joint_indices[0])
+        diag.update(
+            {
+                "primary_joint_index": primary,
+                "primary_joint_name": JOINT_LABELS[primary],
+                "primary_target": float(tentative[primary]),
+                "primary_default": float(default_pose[primary]),
+                "primary_measured": float(joint_pos[primary]),
+                "primary_pos_err": float(pos_err[primary]),
+                "primary_raw": float(raw[primary]),
+                "primary_scaled": float(scaled[primary]),
+            }
+        )
+        if primary < 12:
+            diag["primary_leg_offset"] = float(leg_offset[primary])
+    details.update(diag)
+    return details
+
+
+def policy_release_cfg(cfg: dict) -> dict[str, float]:
+    policy_cfg = cfg.get("policy", {})
+    return {
+        "command_hold_s": max(float(policy_cfg.get("release_command_hold_s", 0.12)), 0.0),
+        "posture_max_err": max(float(policy_cfg.get("release_posture_max_err", 0.35)), 0.0),
+        "target_blend_s": max(float(policy_cfg.get("release_target_blend_s", 0.30)), 1e-3),
+    }
+
+
+def compute_release_metrics(runner: PolicyRunner, state: dict, hold_target: np.ndarray, cmd: np.ndarray) -> dict:
+    joint_pos = np.asarray(state["joint_pos"], dtype=np.float32)
+    default_pose = np.asarray(runner.default_dof_pos, dtype=np.float32)
+    hold_target = np.asarray(hold_target, dtype=np.float32)
+    planar_cmd, yaw_cmd = runner.command_activation_metrics(cmd)
+    return {
+        "planar_cmd": float(planar_cmd),
+        "yaw_cmd": float(yaw_cmd),
+        "max_hold_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))),
+        "max_default_err": float(np.max(np.abs(joint_pos[:12] - default_pose[:12]))),
+        "max_hold_default_gap": float(np.max(np.abs(hold_target[:12] - default_pose[:12]))),
+    }
+
+
+def blend_runtime_target(
+    runner: PolicyRunner,
+    hold_target: np.ndarray,
+    policy_target: np.ndarray,
+    release_alpha: float,
+    target_blend_s: float,
+    control_dt: float,
+) -> np.ndarray:
+    blend = min(1.0, release_alpha * (runner.command_release_s / max(target_blend_s, control_dt)))
+    return ((1.0 - blend) * hold_target + blend * policy_target).astype(np.float32)
+
+
+def compute_target_error_metrics(
+    state: dict,
+    hold_target: np.ndarray,
+    policy_target: np.ndarray,
+) -> dict[str, float]:
+    joint_pos = np.asarray(state["joint_pos"], dtype=np.float32)
+    hold_target = np.asarray(hold_target, dtype=np.float32)
+    policy_target = np.asarray(policy_target, dtype=np.float32)
+    return {
+        "hold_target_max_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))),
+        "policy_target_max_err": float(np.max(np.abs(joint_pos[:12] - policy_target[:12]))),
+        "hold_policy_max_gap": float(np.max(np.abs(hold_target[:12] - policy_target[:12]))),
+    }
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--config", default=str(Path(__file__).parent / "config.yaml"))
+    parser.add_argument("--policy", default=None)
+    parser.add_argument("--dry-run", action="store_true")
+    args = parser.parse_args()
+
+    with open(args.config, "r", encoding="utf-8") as file_obj:
+        cfg = yaml.safe_load(file_obj)
+
+    sim2real_root = Path(__file__).resolve().parent
+    policy_path = resolve_policy_path(args.policy, sim2real_root)
+    if not policy_path.exists():
+        print(f"[Main] policy not found: {policy_path}")
+        sys.exit(1)
+
+    control_dt = 1.0 / float(cfg["control_freq"])
+    driver_factory = make_dry_driver_factory() if args.dry_run else make_real_driver_factory()
+
+    logger = LogBundle(cfg["log_dir"])
+    logger.event(
+        "CONFIG_LOADED",
+        config_path=args.config,
+        policy=str(policy_path),
+        dry_run=args.dry_run,
+        control_freq=cfg["control_freq"],
+        motor_model=cfg["motor_model"],
+    )
+
+    io = RealIO(
+        driver_factory=driver_factory,
+        motor_model=cfg["motor_model"],
+        can1_port=cfg["can1_port"],
+        can2_port=cfg["can2_port"],
+        imu_lib_path=cfg.get("imu_lib_path"),
+        control_dt=control_dt,
+        kp_leg=cfg["controller"]["kp_leg"],
+        kd_leg=cfg["controller"]["kd_leg"],
+        hold_kp_leg=cfg["controller"].get("hold_kp_leg", cfg["controller"]["kp_leg"]),
+        hold_kd_leg=cfg["controller"].get("hold_kd_leg", cfg["controller"]["kd_leg"]),
+        kd_wheel=cfg["controller"]["kd_wheel"],
+        debug=cfg.get("debug", False),
+    )
+    runner = PolicyRunner(
+        policy_path,
+        enable_zero_cmd_suppression=cfg.get("policy", {}).get("enable_zero_cmd_suppression", True),
+        hold_zero_command_pose=cfg.get("policy", {}).get("hold_zero_command_pose", True),
+        command_release_s=cfg.get("policy", {}).get("command_release_s", 0.35),
+        action_scale=np.asarray(
+            cfg.get("policy", {}).get(
+                "action_scale",
+                [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0],
+            ),
+            dtype=np.float32,
+        ),
+        zero_cmd_use_yaw_rate=cfg.get("policy", {}).get("zero_cmd_use_yaw_rate", False),
+        clip_obs=cfg.get("policy", {}).get("clip_obs", 100.0),
+    )
+    require_active_command = cfg.get("policy", {}).get("require_active_command_to_release", True)
+    keyboard = KeyboardCommandController(
+        max_x_vel=cfg["controller"]["max_vx"],
+        max_y_vel=cfg["controller"]["max_vy"],
+        max_yaw_vel=cfg["controller"]["max_yaw_rate"],
+    )
+    safety = SafetyMonitor(
+        max_target_offset=cfg["safety"]["max_target_offset"],
+        max_ang_vel=cfg["safety"]["max_ang_vel"],
+        max_tilt_z=cfg["safety"]["max_tilt_z"],
+        clip_to_brake=cfg["safety"].get("clip_to_brake", 0),
+        hard_target_offset=cfg["safety"].get("hard_target_offset", 1.2),
+    )
+    safety.reset()
+    guard = RuntimeGuard(
+        max_ang_vel=cfg["safety"]["max_ang_vel"],
+        max_tilt_z=cfg["safety"]["max_tilt_z"],
+        imu_age_warn_ms=cfg["safety"].get("imu_age_warn_ms", 60.0),
+        imu_age_stop_ms=cfg["safety"].get("imu_age_stop_ms", 200.0),
+    )
+    initializer = PoseInitializer(
+        io,
+        control_dt=control_dt,
+        transition_time_min=cfg["startup"].get("transition_time_min", 2.0),
+        transition_time_max=cfg["startup"].get("transition_time_max", 6.0),
+        transition_seconds_per_rad=cfg["startup"].get("transition_seconds_per_rad", 1.5),
+        hold_time=cfg["startup"]["hold_time"],
+        settle_pos_threshold=cfg["startup"]["settle_pos_threshold"],
+        settle_vel_threshold=cfg["startup"]["settle_vel_threshold"],
+        timeout_extra=cfg["startup"].get("timeout_extra", 3.0),
+        imu_fresh_wait_s=cfg["startup"].get("imu_fresh_wait_s", 1.0),
+        progress_log_interval=cfg["startup"]["progress_log_interval"],
+        ramp_kp_time=cfg["startup"].get("ramp_kp_time", 1.0),
+        soft_hold_duration=cfg["startup"].get("soft_hold_duration", 1.0),
+        max_dev_warn=cfg["startup"].get("max_dev_warn", 1.5),
+        max_dev_abort=cfg["startup"].get("max_dev_abort", 3.0),
+    )
+    initializer.attach(logger=logger, guard=guard, keyboard=keyboard)
+    stand_balance = StandBalanceController(cfg.get("stand_balance", {}), control_dt=control_dt)
+
+    print("\n[Main] connecting hardware...")
+    keyboard.start()
+    try:
+        io.connect()
+        logger.event("CAN_IMU_CONNECTED", initial_gravity=io.imu.initial_gravity)
+    except Exception as exc:
+        logger.event("HARDWARE_CONNECT_FAILED", error=str(exc))
+        keyboard.stop()
+        logger.close()
+        raise
+
+    try:
+        io.enable_motors()
+        logger.event("MOTORS_ENABLED")
+        time.sleep(0.5)
+
+        target_pose = initializer.transition_to_stand_from_current(target_pose=STAND_POSE) if cfg["startup"]["enabled"] else STAND_POSE.copy()
+
+        if stand_balance.enabled:
+            logger.event("STAND_BALANCE_BEGIN")
+            print("[Main] waiting for stand-balance to settle...")
+            stand_balance.reset()
+            next_exec = time.perf_counter()
+            while True:
+                state = io.read_state()
+                target_pose = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32))
+                io.hold_pose(target_pose, kp_scale=1.0)
+                debug = stand_balance.last_debug
+                if stand_balance.is_stable():
+                    logger.event(
+                        "STAND_BALANCE_STABLE",
+                        roll_deg=float(np.degrees(debug.roll)),
+                        pitch_deg=float(np.degrees(debug.pitch)),
+                        pitch_corr=float(debug.pitch_corr),
+                        pitch_compensation_enabled=bool(debug.pitch_compensation_enabled),
+                    )
+                    break
+                next_exec += control_dt
+                next_exec = _sleep_to(next_exec)
+            logger.event("STAND_BALANCE_END")
+
+        if cfg["startup"]["require_user_confirm"]:
+            print("[Main] standing complete. Press Enter to release policy control...")
+            done = threading.Event()
+
+            def _wait():
+                try:
+                    input()
+                except EOFError:
+                    pass
+                done.set()
+
+            threading.Thread(target=_wait, daemon=True).start()
+            if not initializer.hold_until_user_confirm(target_pose, done):
+                raise PoseInitFailed("WAIT_USER interrupted")
+
+        print("[Main] priming current observation...")
+        logger.event("PRIME_BEGIN")
+        zero_cmd = np.zeros(3, dtype=np.float32)
+        next_exec = time.perf_counter()
+        for index in range(1):
+            if stand_balance.enabled:
+                state = io.read_state()
+                target_pose = stand_balance.compute_target(state, zero_cmd)
+                io.hold_pose(target_pose, kp_scale=1.0)
+            else:
+                io.hold_pose(target_pose, kp_scale=1.0)
+                state = io.read_state()
+            obs = io.get_obs_policy(state, zero_cmd, runner.default_dof_pos, runner.last_actions)
+            if index == 0:
+                runner.reset(prime_obs=obs)
+            logger.state(
+                phase="PRIME",
+                joint_pos=state["joint_pos"],
+                joint_vel=state["joint_vel"],
+                joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)),
+                target_pose=target_pose,
+                raw_action=None,
+                gyro=state["imu_gyro"],
+                accel=state["imu_accel"],
+                quat=state["quat_wxyz"],
+                proj_gravity=state["projected_gravity"],
+                command=zero_cmd,
+                imu_age_ms=float(state["imu_age_ms"]),
+                loop_dt_ms=0.0,
+                kp_scale=1.0,
+            )
+            next_exec += control_dt
+            next_exec = _sleep_to(next_exec)
+        logger.event("PRIME_END")
+
+        print("[Main] entering 50Hz control loop... (space = estop)")
+        logger.event("RUNTIME_BEGIN")
+        next_exec = time.perf_counter()
+        loop_count = 0
+        last_print = next_exec
+        log_every = int(cfg.get("log_every", 1))
+        recent_dt_ms = []
+        runtime_released = not require_active_command
+        release_cfg = policy_release_cfg(cfg)
+        release_active_time = 0.0
+
+        while True:
+            loop_t0 = time.perf_counter()
+            cmd = keyboard.get_command()
+            state = io.read_state()
+            obs = io.get_obs_policy(state, cmd, runner.default_dof_pos, runner.last_actions)
+            zero_command = runner._is_zero_command(cmd, state["imu_gyro"])
+
+            obs_nan = bool(np.any(np.isnan(obs)) or np.any(np.isinf(obs)))
+            if obs_nan:
+                logger.event("OBS_NAN", obs_max=float(np.nanmax(obs)))
+                io.damping_brake()
+                break
+
+            if not runtime_released and zero_command:
+                raw = np.zeros(16, dtype=np.float32)
+                scaled = np.zeros(16, dtype=np.float32)
+                target_hold = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if stand_balance.enabled else runner.default_dof_pos.copy()
+                actual_target = io.hold_pose(target_hold, kp_scale=1.0)
+                policy_target = runner.default_dof_pos.copy()
+                release_metrics = compute_release_metrics(runner, state, target_hold, cmd)
+                target_metrics = compute_target_error_metrics(state, target_hold, policy_target)
+                release_active_time = 0.0
+                safety_decision = SafetyMonitor().check(
+                    target_pose=target_hold,
+                    default_pose=runner.default_dof_pos,
+                    imu_gyro=state["imu_gyro"],
+                    projected_gravity=state["projected_gravity"],
+                    estop_triggered=keyboard.is_estop_triggered(),
+                )
+                guard_decision = guard.check(
+                    imu_gyro=state["imu_gyro"],
+                    projected_gravity=state["projected_gravity"],
+                    imu_age_ms=float(state["imu_age_ms"]),
+                    estop_triggered=keyboard.is_estop_triggered(),
+                    extra_nan_arrays=(target_hold,),
+                )
+            else:
+                target_hold = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if stand_balance.enabled else runner.default_dof_pos.copy()
+                release_metrics = compute_release_metrics(runner, state, target_hold, cmd)
+                if not runtime_released:
+                    release_active_time += control_dt if runner.is_command_active(cmd) else 0.0
+                    active_ready = release_active_time >= release_cfg["command_hold_s"]
+                    posture_ready = release_metrics["max_hold_err"] <= release_cfg["posture_max_err"]
+                    if active_ready and posture_ready:
+                        runtime_released = True
+                        logger.event(
+                            "RUNTIME_COMMAND_RELEASED",
+                            cmd=cmd.tolist(),
+                            active_hold_s=release_active_time,
+                            max_hold_err=release_metrics["max_hold_err"],
+                            max_default_err=release_metrics["max_default_err"],
+                            max_hold_default_gap=release_metrics["max_hold_default_gap"],
+                        )
+                    else:
+                        reasons = []
+                        if not active_ready:
+                            reasons.append(f"cmd_hold<{release_cfg['command_hold_s']:.2f}s")
+                        if not posture_ready:
+                            reasons.append(f"hold_err>{release_cfg['posture_max_err']:.3f}")
+                        logger.event(
+                            "RUNTIME_RELEASE_BLOCKED",
+                            reason=",".join(reasons),
+                            cmd=cmd.tolist(),
+                            active_hold_s=release_active_time,
+                            max_hold_err=release_metrics["max_hold_err"],
+                            max_default_err=release_metrics["max_default_err"],
+                            max_hold_default_gap=release_metrics["max_hold_default_gap"],
+                        )
+                        raw = np.zeros(16, dtype=np.float32)
+                        scaled = np.zeros(16, dtype=np.float32)
+                        actual_target = io.hold_pose(target_hold, kp_scale=1.0)
+                        policy_target = runner.default_dof_pos.copy()
+                        target_metrics = compute_target_error_metrics(state, target_hold, policy_target)
+                        safety_decision = SafetyMonitor().check(
+                            target_pose=target_hold,
+                            default_pose=runner.default_dof_pos,
+                            imu_gyro=state["imu_gyro"],
+                            projected_gravity=state["projected_gravity"],
+                            estop_triggered=keyboard.is_estop_triggered(),
+                        )
+                        guard_decision = guard.check(
+                            imu_gyro=state["imu_gyro"],
+                            projected_gravity=state["projected_gravity"],
+                            imu_age_ms=float(state["imu_age_ms"]),
+                            estop_triggered=keyboard.is_estop_triggered(),
+                            extra_nan_arrays=(target_hold,),
+                        )
+                        loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0
+                        if log_every and (loop_count % log_every == 0):
+                            motor_diag = state.get("motor_stale", {})
+                            logger.state(
+                                phase="RUNTIME",
+                                joint_pos=state["joint_pos"],
+                                joint_vel=state["joint_vel"],
+                                joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)),
+                                target_pose=actual_target,
+                                raw_action=raw,
+                                gyro=state["imu_gyro"],
+                                accel=state["imu_accel"],
+                                quat=state["quat_wxyz"],
+                                proj_gravity=state["projected_gravity"],
+                                command=cmd,
+                                imu_age_ms=float(state["imu_age_ms"]),
+                                loop_dt_ms=loop_dt_ms,
+                                safety_level=int(safety_decision.level),
+                                guard_level=int(guard_decision.level),
+                                holdover=int(motor_diag.get("holdover_this_frame", 0)),
+                                stale_max=int(motor_diag.get("stale_max", 0)),
+                                fresh_count=int(motor_diag.get("fresh_count", 16)),
+                                kp_scale=1.0,
+                                nan_flag=0,
+                                kp_leg_cmd=float(io.kp_leg),
+                                kd_leg_cmd=float(io.kd_leg),
+                                kd_wheel_cmd=float(io.kd_wheel),
+                                runtime_release_alpha=0.0,
+                                runtime_release_hold_s=release_active_time,
+                                runtime_blend_ratio=0.0,
+                                hold_target_max_err=target_metrics["hold_target_max_err"],
+                                policy_target_max_err=target_metrics["policy_target_max_err"],
+                                hold_policy_max_gap=target_metrics["hold_policy_max_gap"],
+                                target_source="runtime_hold",
+                                clip_primary_joint="",
+                                safety_reason=f"release_blocked:{','.join(reasons)}",
+                                guard_reason=guard_decision.reason,
+                            )
+                        next_exec += control_dt
+                        next_exec = _sleep_to(next_exec)
+                        loop_count += 1
+                        continue
+                scaled, raw = runner.step(obs)
+                act_nan = bool(np.any(np.isnan(raw)) or np.any(np.isinf(raw)))
+                if act_nan:
+                    logger.event("ACTION_NAN")
+                    io.damping_brake()
+                    break
+
+                policy_target = (scaled + runner.default_dof_pos).astype(np.float32)
+                tentative = blend_runtime_target(
+                    runner,
+                    target_hold,
+                    policy_target,
+                    float(getattr(runner, "_command_release_alpha", 0.0)),
+                    release_cfg["target_blend_s"],
+                    control_dt,
+                )
+                scaled = tentative - runner.default_dof_pos
+                target_metrics = compute_target_error_metrics(state, target_hold, policy_target)
+                runtime_blend_ratio = min(
+                    1.0,
+                    float(getattr(runner, "_command_release_alpha", 0.0))
+                    * (runner.command_release_s / max(release_cfg["target_blend_s"], control_dt)),
+                )
+                projected_gravity = get_gravity_orientation(state["quat_wxyz"])
+
+                guard_decision = guard.check(
+                    imu_gyro=state["imu_gyro"],
+                    projected_gravity=projected_gravity,
+                    imu_age_ms=float(state["imu_age_ms"]),
+                    estop_triggered=keyboard.is_estop_triggered(),
+                    extra_nan_arrays=(raw, tentative),
+                )
+                if guard_decision.level == GuardLevel.STOP:
+                    logger.event("GUARD_STOP", phase="RUNTIME", reason=guard_decision.reason)
+                    io.damping_brake()
+                    break
+
+                safety_decision = safety.check(
+                    target_pose=tentative,
+                    default_pose=runner.default_dof_pos,
+                    imu_gyro=state["imu_gyro"],
+                    projected_gravity=projected_gravity,
+                    estop_triggered=keyboard.is_estop_triggered(),
+                )
+                if safety_decision.level == SafetyLevel.ESTOP:
+                    logger.event("SAFETY_ESTOP", reason=safety_decision.message)
+                    io.damping_brake()
+                    break
+                if safety_decision.level == SafetyLevel.BRAKE:
+                    safety_diag = build_action_diag(
+                        joint_pos=state["joint_pos"],
+                        default_pose=runner.default_dof_pos,
+                        raw=raw,
+                        scaled=scaled,
+                        tentative=tentative,
+                        cmd=cmd,
+                        zero_command=zero_command,
+                        runtime_released=runtime_released,
+                        release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
+                        safety_details=safety_decision.details,
+                    )
+                    logger.event(
+                        "SAFETY_BRAKE",
+                        reason=safety_decision.message,
+                        details=safety_diag,
+                        primary_joint=safety_diag.get("primary_joint_name"),
+                        primary_offset=safety_diag.get("primary_leg_offset"),
+                        primary_target=safety_diag.get("primary_target"),
+                        primary_measured=safety_diag.get("primary_measured"),
+                        primary_raw=safety_diag.get("primary_raw"),
+                        primary_scaled=safety_diag.get("primary_scaled"),
+                        cmd=cmd.tolist(),
+                        release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
+                    )
+                    io.damping_brake()
+                    break
+                if safety_decision.level == SafetyLevel.CLIP and safety_decision.clipped_target is not None:
+                    scaled = safety_decision.clipped_target - runner.default_dof_pos
+                    safety_diag = build_action_diag(
+                        joint_pos=state["joint_pos"],
+                        default_pose=runner.default_dof_pos,
+                        raw=raw,
+                        scaled=scaled,
+                        tentative=tentative,
+                        cmd=cmd,
+                        zero_command=zero_command,
+                        runtime_released=runtime_released,
+                        release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
+                        safety_details=safety_decision.details,
+                    )
+                    logger.event(
+                        "SAFETY_CLIP",
+                        reason=safety_decision.message,
+                        details=safety_diag,
+                        primary_joint=safety_diag.get("primary_joint_name"),
+                        primary_offset=safety_diag.get("primary_leg_offset"),
+                        primary_target=safety_diag.get("primary_target"),
+                        primary_measured=safety_diag.get("primary_measured"),
+                        primary_raw=safety_diag.get("primary_raw"),
+                        primary_scaled=safety_diag.get("primary_scaled"),
+                        max_raw=float(np.max(np.abs(raw))),
+                        cmd=cmd.tolist(),
+                        release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
+                    )
+
+                actual_target = io.send_actions(scaled, runner.default_dof_pos)
+            loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0
+
+            if log_every and (loop_count % log_every == 0):
+                motor_diag = state.get("motor_stale", {})
+                logger.state(
+                    phase="RUNTIME",
+                    joint_pos=state["joint_pos"],
+                    joint_vel=state["joint_vel"],
+                    joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)),
+                    target_pose=actual_target,
+                    raw_action=raw,
+                    gyro=state["imu_gyro"],
+                    accel=state["imu_accel"],
+                    quat=state["quat_wxyz"],
+                    proj_gravity=projected_gravity,
+                    command=cmd,
+                    imu_age_ms=float(state["imu_age_ms"]),
+                    loop_dt_ms=loop_dt_ms,
+                    safety_level=int(safety_decision.level),
+                    guard_level=int(guard_decision.level),
+                    holdover=int(motor_diag.get("holdover_this_frame", 0)),
+                    stale_max=int(motor_diag.get("stale_max", 0)),
+                    fresh_count=int(motor_diag.get("fresh_count", 16)),
+                    kp_scale=1.0,
+                    nan_flag=int(obs_nan or act_nan),
+                    kp_leg_cmd=float(io.kp_leg),
+                    kd_leg_cmd=float(io.kd_leg),
+                    kd_wheel_cmd=float(io.kd_wheel),
+                    runtime_release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)),
+                    runtime_release_hold_s=release_active_time,
+                    runtime_blend_ratio=runtime_blend_ratio,
+                    hold_target_max_err=target_metrics["hold_target_max_err"],
+                    policy_target_max_err=target_metrics["policy_target_max_err"],
+                    hold_policy_max_gap=target_metrics["hold_policy_max_gap"],
+                    target_source="runtime_blend" if runtime_blend_ratio < 0.999 else "runtime_policy",
+                    clip_primary_joint=str((safety_decision.details or {}).get("primary_joint_name", "")),
+                    clip_primary_target=float((safety_decision.details or {}).get("primary_target", 0.0) or 0.0),
+                    clip_primary_measured=float((safety_decision.details or {}).get("primary_measured", 0.0) or 0.0),
+                    clip_primary_default=float((safety_decision.details or {}).get("primary_default", 0.0) or 0.0),
+                    clip_primary_pos_err=float((safety_decision.details or {}).get("primary_pos_err", 0.0) or 0.0),
+                    clip_primary_raw=float((safety_decision.details or {}).get("primary_raw", 0.0) or 0.0),
+                    clip_primary_scaled=float((safety_decision.details or {}).get("primary_scaled", 0.0) or 0.0),
+                    safety_reason=(
+                        f"{safety_decision.message};zero_cmd={int(zero_command)};"
+                        f"released={int(runtime_released)};alpha={getattr(runner, '_command_release_alpha', 0.0):.2f};"
+                        f"max_raw={float(np.max(np.abs(raw))):.2f};"
+                        f"clip={((safety_decision.details or {}).get('joint_indices', []))}"
+                    ),
+                    guard_reason=guard_decision.reason,
+                )
+
+            next_exec += control_dt
+            slack = next_exec - time.perf_counter()
+            if slack > 0:
+                coarse = slack - 0.002
+                if coarse > 0:
+                    time.sleep(coarse)
+                while time.perf_counter() < next_exec:
+                    pass
+            elif slack < -control_dt:
+                logger.event("LOOP_OVERRUN", over_ms=-slack * 1000.0)
+                next_exec = time.perf_counter()
+
+            recent_dt_ms.append(loop_dt_ms)
+            if len(recent_dt_ms) > 50:
+                recent_dt_ms.pop(0)
+            if len(recent_dt_ms) == 50:
+                median_dt = float(np.median(recent_dt_ms))
+                if median_dt > 22.0:
+                    logger.event("SLOW_LOOP_TREND", median_dt_ms=median_dt)
+                    recent_dt_ms.clear()
+
+            loop_count += 1
+            if time.perf_counter() - last_print > 1.0:
+                print(
+                    f"[Loop] cmd=[{cmd[0]:+.2f},{cmd[1]:+.2f},{cmd[2]:+.2f}] "
+                    f"|raw|={float(np.max(np.abs(raw))):.2f} "
+                    f"zero={int(zero_command)} rel={int(runtime_released)} "
+                    f"alpha={getattr(runner, '_command_release_alpha', 0.0):.2f} "
+                    f"imu_age={state['imu_age_ms']:.1f}ms "
+                    f"holdover={io.hw.holdover_total} "
+                    f"safety={int(safety_decision.level)}"
+                )
+                last_print = time.perf_counter()
+
+    except PoseInitFailed as exc:
+        print(f"[Main] startup aborted: {exc}")
+        logger.event("POSE_INIT_FAILED", error=str(exc))
+    except KeyboardInterrupt:
+        print("\n[Main] Ctrl+C received, stopping...")
+        logger.event("KEYBOARD_INTERRUPT")
+    except Exception as exc:
+        import traceback
+
+        print(f"\n[Main] exception: {exc}")
+        traceback.print_exc()
+        logger.event("UNEXPECTED_ERROR", error=str(exc), traceback=traceback.format_exc())
+    finally:
+        print("[Main] cleaning up...")
+        try:
+            io.damping_brake()
+            time.sleep(0.05)
+            logger.event("DAMPING_BRAKE_APPLIED")
+        except Exception as exc:
+            logger.event("DAMPING_BRAKE_FAILED", error=str(exc))
+        try:
+            io.disconnect()
+            logger.event("HARDWARE_DISCONNECTED")
+        finally:
+            keyboard.stop()
+            logger.close()
+        os._exit(0)
+
+
+if __name__ == "__main__":
+    main()

05_software/real/sim2real_v2/mjcf/scene.xml

@@ -0,0 +1,22 @@
+<mujoco model="wheelleg_scene">
+  <include file="wheelleg.xml"/>
+
+  <option timestep="0.002" gravity="0 0 -9.81" integrator="implicitfast"/>
+
+  <visual>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3"/>
+    <global azimuth="120" 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>
+    <light pos="0 0 3" dir="0 0 -1" directional="true"/>
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" friction="0.8 0.05 0.01"/>
+  </worldbody>
+</mujoco>

05_software/real/sim2real_v2/mjcf/wheelleg.xml

@@ -0,0 +1,157 @@
+<mujoco model="wheelleg">
+  <compiler angle="radian" meshdir="meshes/"/>
+
+  <default>
+    <geom margin="0"/>
+  </default>
+  <asset>
+    <mesh name="base_link" content_type="model/stl" file="base_link.STL"/>
+    <mesh name="fl_hip_abduction_Link" content_type="model/stl" file="fl_hip_abduction_Link.STL"/>
+    <mesh name="fl_hip_pitch_Link" content_type="model/stl" file="fl_hip_pitch_Link.STL"/>
+    <mesh name="fl_knee_Link" content_type="model/stl" file="fl_knee_Link.STL"/>
+    <mesh name="fl_wheel_Link" content_type="model/stl" file="fl_wheel_Link.STL"/>
+    <mesh name="fr_hip_abduction_Link" content_type="model/stl" file="fr_hip_abduction_Link.STL"/>
+    <mesh name="fr_hip_pitch_Link" content_type="model/stl" file="fr_hip_pitch_Link.STL"/>
+    <mesh name="fr_knee_Link" content_type="model/stl" file="fr_knee_Link.STL"/>
+    <mesh name="fr_wheel_Link" content_type="model/stl" file="fr_wheel_Link.STL"/>
+    <mesh name="rl_hip_abduction_Link" content_type="model/stl" file="rl_hip_abduction_Link.STL"/>
+    <mesh name="rl_hip_pitch_Link" content_type="model/stl" file="rl_hip_pitch_Link.STL"/>
+    <mesh name="rl_knee_Link" content_type="model/stl" file="rl_knee_Link.STL"/>
+    <mesh name="rl_wheel_Link" content_type="model/stl" file="rl_wheel_Link.STL"/>
+    <mesh name="rr_hip_abduction_Link" content_type="model/stl" file="rr_hip_abduction_Link.STL"/>
+    <mesh name="rr_hip_pitch_Link" content_type="model/stl" file="rr_hip_pitch_Link.STL"/>
+    <mesh name="rr_knee_Link" content_type="model/stl" file="rr_knee_Link.STL"/>
+    <mesh name="rr_wheel_Link" content_type="model/stl" file="rr_wheel_Link.STL"/>
+  </asset>
+
+  <worldbody>
+    <body name="base_link">
+      <inertial pos="0.1517 0.0002 0.0542" mass="3.5" diaginertia="0.0215 0.0904 0.0985"/>
+      <joint type="free"/>
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="base_link"/>
+      <geom size="0.178 0.1175 0.073" pos="0.1518 0 0.054" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+      <body name="fl_hip_abduction_Link" pos="0.32826 0.066172 0.053981">
+        <inertial pos="0.0488 -0.0026 0.0007" mass="0.5" diaginertia="0.0003 0.0006 0.0005"/>
+        <joint name="fl_hip_abduction_joint" pos="0 0 0" axis="1 0 0" range="-0.436 0.611" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fl_hip_abduction_Link"/>
+        <body name="fl_hip_pitch_Link" pos="0.06389 -0.027344 0.00010727" quat="0.999997 -0.0025023 0 0">
+          <inertial pos="0.0019 0.1119 -0.048" mass="0.935" diaginertia="0.0062 0.0064 0.001"/>
+          <joint name="fl_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.58 2.58" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fl_hip_pitch_Link"/>
+          <geom size="0.046 0.048" pos="0 0.048 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+          <geom size="0.0435 0.0115 0.06" pos="0 0.1155 -0.06" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+          <body name="fl_knee_Link" pos="0 0.1035 -0.25" quat="0.999997 0.0025023 0 0">
+            <inertial pos="0.0002 0.0242 -0.1539" mass="0.651" fullinertia="0.0042 0.0045 0.0005 0 0 0.0002"/>
+            <joint name="fl_knee_joint" pos="0 0 0" axis="0 1 0" range="-2.65 2.65" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fl_knee_Link"/>
+            <geom size="0.0475 0.015" pos="0 0.025 -0.20011" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            <geom size="0.015 0.0125 0.06" pos="0 0.0125 -0.09" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+            <body name="fl_wheel_Link" pos="0 0.014699 -0.20011">
+              <inertial pos="-0.0002 0.0407 -0.0001" mass="0.53" diaginertia="0.0017 0.0032 0.0017"/>
+              <joint name="fl_wheel_joint" pos="0 0 0" axis="0 1 0" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fl_wheel_Link"/>
+              <geom size="0.1 0.015" pos="0 0.04074 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="fr_hip_abduction_Link" pos="0.32826 -0.065853 0.054034">
+        <inertial pos="0.0488 0.0026 0.0008" mass="0.5" diaginertia="0.0003 0.0006 0.0005"/>
+        <joint name="fr_hip_abduction_joint" pos="0 0 0" axis="1 0 0" range="-0.611 0.436" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fr_hip_abduction_Link"/>
+        <body name="fr_hip_pitch_Link" pos="0.06389 0.027311 -0.00036027" quat="0.999976 -0.00686995 0 0">
+          <inertial pos="-0.0019 -0.1119 -0.048" mass="0.935" diaginertia="0.0062 0.0064 0.001"/>
+          <joint name="fr_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.58 2.58" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fr_hip_pitch_Link"/>
+          <geom size="0.046 0.048" pos="0 -0.048 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+          <geom size="0.0435 0.0115 0.06" pos="0 -0.1155 -0.06" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+          <body name="fr_knee_Link" pos="-0.00075079 -0.1035 -0.25" quat="0.999976 0.00686995 0 0">
+            <inertial pos="-0.0002 -0.0242 -0.1539" mass="0.651" fullinertia="0.0042 0.0045 0.0005 0 0 0.0001"/>
+            <joint name="fr_knee_joint" pos="0 0 0" axis="0 1 0" range="-2.65 2.65" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fr_knee_Link"/>
+            <geom size="0.0475 0.015" pos="0 -0.025 -0.1998" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            <geom size="0.015 0.0125 0.06" pos="0 -0.0125 -0.09" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+            <body name="fr_wheel_Link" pos="0 -0.018447 -0.1998">
+              <inertial pos="0.0002 -0.0407 -0.0001" mass="0.53" diaginertia="0.0017 0.0032 0.0017"/>
+              <joint name="fr_wheel_joint" pos="0 0 0" axis="0 1 0" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="fr_wheel_Link"/>
+              <geom size="0.1 0.015" pos="0 -0.040735 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="rl_hip_abduction_Link" pos="-0.024743 0.066141 0.054034">
+        <inertial pos="-0.0488 -0.0026 -0.0008" mass="0.5" diaginertia="0.0003 0.0006 0.0005"/>
+        <joint name="rl_hip_abduction_joint" pos="0 0 0" axis="1 0 0" range="-0.436 0.611" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rl_hip_abduction_Link"/>
+        <body name="rl_hip_pitch_Link" pos="-0.06389 -0.027309 0.00045509">
+          <inertial pos="0.0019 0.1119 -0.048" mass="0.935" diaginertia="0.0062 0.0064 0.001"/>
+          <joint name="rl_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.58 2.58" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rl_hip_pitch_Link"/>
+          <geom size="0.046 0.048" pos="0 0.048 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+          <geom size="0.0435 0.0115 0.06" pos="0 0.1155 -0.06" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+          <body name="rl_knee_Link" pos="0 0.099459 -0.25163">
+            <inertial pos="0.0002 0.0242 -0.1539" mass="0.651" fullinertia="0.0042 0.0045 0.0005 0 0 -0.0003"/>
+            <joint name="rl_knee_joint" pos="0 0 0" axis="0 1 0" range="-2.65 2.65" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rl_knee_Link"/>
+            <geom size="0.0475 0.015" pos="0 0.025 -0.20027" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            <geom size="0.015 0.0125 0.06" pos="0 0.0125 -0.09" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+            <body name="rl_wheel_Link" pos="0 0.012475 -0.20027">
+              <inertial pos="-0.0002 0.0407 -0.0001" mass="0.53" diaginertia="0.0017 0.0032 0.0017"/>
+              <joint name="rl_wheel_joint" pos="0 0 0" axis="0 1 0" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rl_wheel_Link"/>
+              <geom size="0.1 0.015" pos="0 0.040737 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="rr_hip_abduction_Link" pos="-0.024743 -0.065884 0.053981">
+        <inertial pos="-0.0488 0.0026 0.0008" mass="0.5" diaginertia="0.0003 0.0006 0.0005"/>
+        <joint name="rr_hip_abduction_joint" pos="0 0 0" axis="1 0 0" range="-0.611 0.436" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rr_hip_abduction_Link"/>
+        <body name="rr_hip_pitch_Link" pos="-0.06389 0.027341 0.00041625">
+          <inertial pos="-0.002 -0.1111 -0.0498" mass="0.935" diaginertia="0.0062 0.0064 0.001"/>
+          <joint name="rr_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.58 2.58" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rr_hip_pitch_Link"/>
+          <geom size="0.046 0.048" pos="0 -0.048 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+          <geom size="0.0435 0.0115 0.06" pos="0 -0.1155 -0.06" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+          <body name="rr_knee_Link" pos="-0.00075079 -0.099408 -0.25165">
+            <inertial pos="-0.0002 -0.0225 -0.1541" mass="0.651" fullinertia="0.0042 0.0045 0.0005 0 0 -0.0001"/>
+            <joint name="rr_knee_joint" pos="0 0 0" axis="0 1 0" range="-2.65 2.65" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rr_knee_Link"/>
+            <geom size="0.0475 0.015" pos="0 -0.025 -0.20027" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            <geom size="0.015 0.0125 0.06" pos="0 -0.0125 -0.09" type="box" rgba="0.75294 0.75294 0.75294 1"/>
+            <body name="rr_wheel_Link" pos="0 -0.012435 -0.20027">
+              <inertial pos="0.0002 -0.0407 -0.0005" mass="0.53" diaginertia="0.0017 0.0032 0.0017"/>
+              <joint name="rr_wheel_joint" pos="0 0 0" axis="0 1 0" actuatorfrcrange="-17 17" damping="0.01" frictionloss="0.01" armature="0.0042"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.75294 0.75294 0.75294 1" mesh="rr_wheel_Link"/>
+              <geom size="0.1 0.015" pos="0 -0.040737 0" quat="0.707105 0.707108 0 0" type="cylinder" rgba="0.75294 0.75294 0.75294 1"/>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="imu_link" pos="0.1518 0 0.127">
+        <inertial pos="0 0 0" mass="0" diaginertia="0 0 0"/>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <general name="fl_hip_abduction_joint" joint="fl_hip_abduction_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="fl_hip_pitch_joint" joint="fl_hip_pitch_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="fl_knee_joint" joint="fl_knee_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="fl_wheel_joint" joint="fl_wheel_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="0.5"/>
+    <general name="fr_hip_abduction_joint" joint="fr_hip_abduction_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="fr_hip_pitch_joint" joint="fr_hip_pitch_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="fr_knee_joint" joint="fr_knee_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="fr_wheel_joint" joint="fr_wheel_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="0.5"/>
+    <general name="rl_hip_abduction_joint" joint="rl_hip_abduction_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="rl_hip_pitch_joint" joint="rl_hip_pitch_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="rl_knee_joint" joint="rl_knee_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="rl_wheel_joint" joint="rl_wheel_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="0.5"/>
+    <general name="rr_hip_abduction_joint" joint="rr_hip_abduction_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="rr_hip_pitch_joint" joint="rr_hip_pitch_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="rr_knee_joint" joint="rr_knee_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="120" biasprm="0 -8 -8"/>
+    <general name="rr_wheel_joint" joint="rr_wheel_joint" ctrlrange="-17 17" forcerange="-17 17" gainprm="0.5"/>
+  </actuator>
+</mujoco>

05_software/real/sim2real_v2/policy/policy_runner.py

@@ -0,0 +1,243 @@
+from pathlib import Path
+
+import numpy as np
+import torch
+import torch.nn as nn
+
+
+def resolve_policy_path(policy_path: Path | str | None, root: Path | None = None) -> Path:
+    """Prefer ONNX for deployment while keeping the PT file as source/fallback."""
+    root = root or Path(__file__).resolve().parents[1]
+    if policy_path is None:
+        onnx_path = root / "policies" / "model_rough.onnx"
+        pt_path = root / "policies" / "model_rough.pt"
+        return onnx_path if onnx_path.exists() else pt_path
+
+    path = Path(policy_path)
+    if not path.is_absolute():
+        path = root / path
+    if path.suffix.lower() == ".pt":
+        onnx_path = path.with_suffix(".onnx")
+        if onnx_path.exists():
+            return onnx_path
+    return path
+
+
+class PolicyMLP(nn.Module):
+    def __init__(self, obs_dim: int, action_dim: int):
+        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: torch.Tensor) -> torch.Tensor:
+        x = (x - self.obs_mean) / torch.clamp(self.obs_std, min=1e-6)
+        return self.net(x)
+
+
+class OnnxPolicy:
+    def __init__(self, model_path: Path):
+        try:
+            import onnxruntime as ort
+        except ImportError as exc:
+            raise ImportError(
+                "onnxruntime is required for ONNX policy inference. "
+                "Install it on Orin with `python -m pip install onnxruntime`."
+            ) from exc
+
+        opts = ort.SessionOptions()
+        opts.intra_op_num_threads = 1
+        opts.inter_op_num_threads = 1
+        opts.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
+        self.session = ort.InferenceSession(
+            str(model_path),
+            sess_options=opts,
+            providers=["CPUExecutionProvider"],
+        )
+        self.input_name = self.session.get_inputs()[0].name
+        self.output_name = self.session.get_outputs()[0].name
+        input_shape = self.session.get_inputs()[0].shape
+        output_shape = self.session.get_outputs()[0].shape
+        self.expected_obs_dim = int(input_shape[1]) if len(input_shape) >= 2 and isinstance(input_shape[1], int) else 53
+        self.expected_action_dim = int(output_shape[1]) if len(output_shape) >= 2 and isinstance(output_shape[1], int) else 16
+        self.backend = "onnxruntime"
+
+        self.obs_mean = torch.zeros(self.expected_obs_dim)
+        self.obs_std = torch.ones(self.expected_obs_dim)
+
+    def __call__(self, x: torch.Tensor) -> torch.Tensor:
+        obs = x.detach().cpu().numpy().astype(np.float32, copy=False)
+        action = self.session.run([self.output_name], {self.input_name: obs})[0]
+        return torch.from_numpy(np.asarray(action, dtype=np.float32)).to(x.device)
+
+
+def load_policy(model_path: Path, device: torch.device):
+    if model_path.suffix.lower() == ".onnx":
+        return OnnxPolicy(model_path)
+
+    checkpoint = torch.load(model_path, map_location=device, weights_only=False)
+    state_dict = checkpoint["actor_state_dict"]
+
+    input_key = "mlp.0.weight" if "mlp.0.weight" in state_dict else "net.0.weight"
+    output_key = "mlp.6.weight" if "mlp.6.weight" in state_dict else "net.6.weight"
+    obs_dim = int(state_dict[input_key].shape[1])
+    action_dim = int(state_dict[output_key].shape[0])
+
+    model = PolicyMLP(obs_dim=obs_dim, action_dim=action_dim)
+    remapped_state_dict: dict[str, torch.Tensor] = {}
+    for key, value in state_dict.items():
+        if key.startswith("mlp."):
+            remapped_state_dict[key.replace("mlp.", "net.")] = value
+        elif key.startswith("net."):
+            remapped_state_dict[key] = value
+        elif key == "obs_normalizer._mean":
+            remapped_state_dict["obs_mean"] = value.squeeze()
+        elif key == "obs_normalizer._var":
+            remapped_state_dict["obs_std"] = torch.sqrt(value.squeeze() + 1e-5)
+
+    model.load_state_dict(remapped_state_dict, strict=False)
+    model.eval()
+    model.to(device)
+    model.expected_obs_dim = obs_dim
+    model.expected_action_dim = action_dim
+    model.backend = "torch"
+    return model
+
+
+class PolicyRunner:
+    BASE_OBS_DIM = 53
+    DEFAULT_STAND_POSE = np.array(
+        [
+            0.0, 0.9, -1.8,
+            0.0, 0.9, -1.8,
+            0.0, 0.9, -1.8,
+            0.0, 0.9, -1.8,
+            0.0, 0.0, 0.0, 0.0,
+        ],
+        dtype=np.float32,
+    )
+
+    def __init__(
+        self,
+        policy_path: Path,
+        device: torch.device | None = None,
+        enable_zero_cmd_suppression: bool = True,
+        hold_zero_command_pose: bool = True,
+        command_release_s: float = 0.35,
+        action_scale: np.ndarray | None = None,
+        zero_cmd_use_yaw_rate: bool = True,
+        clip_obs: float = 100.0,
+    ):
+        self.device = device or torch.device("cuda" if torch.cuda.is_available() else "cpu")
+        self.policy_path = Path(policy_path)
+        self.enable_zero_cmd_suppression = bool(enable_zero_cmd_suppression)
+        self.hold_zero_command_pose = bool(hold_zero_command_pose)
+        self.command_release_s = max(float(command_release_s), 1e-3)
+        self.clip_obs = max(float(clip_obs), 0.0)
+        self.policy_path = resolve_policy_path(self.policy_path)
+        if not self.policy_path.exists():
+            raise FileNotFoundError(f"policy file not found: {self.policy_path}")
+        print(f"[PolicyRunner] device={self.device}, policy={self.policy_path}")
+        self.policy = load_policy(self.policy_path, self.device)
+        if self.policy.expected_obs_dim != self.BASE_OBS_DIM:
+            raise ValueError(
+                f"Unsupported policy obs dim {self.policy.expected_obs_dim}. "
+                f"Current sim2real only supports {self.BASE_OBS_DIM}-D actor observations."
+            )
+
+        self.default_dof_pos = self.DEFAULT_STAND_POSE.copy()
+        self.last_actions = np.zeros(16, dtype=np.float32)
+
+        self.action_scale = np.asarray(
+            action_scale
+            if action_scale is not None
+            else [
+                0.125, 0.25, 0.25,
+                0.125, 0.25, 0.25,
+                0.125, 0.25, 0.25,
+                0.125, 0.25, 0.25,
+                5.0, 5.0, 5.0, 5.0,
+            ],
+            dtype=np.float32,
+        )
+        if self.action_scale.shape != (16,):
+            raise ValueError(f"action_scale must be shape (16,), got {self.action_scale.shape}")
+
+        self.zero_cmd_lin_thresh = 0.05
+        self.zero_cmd_yaw_thresh = 0.05
+        self.zero_yaw_rate_thresh = 0.10
+        self.zero_cmd_use_yaw_rate = bool(zero_cmd_use_yaw_rate)
+        self._command_release_alpha = 0.0
+        print(
+            f"[PolicyRunner] obs_dim={self.policy.expected_obs_dim}, "
+            f"base_obs_dim={self.BASE_OBS_DIM}, history=1, "
+            f"action_dim={self.policy.expected_action_dim}, "
+            f"backend={getattr(self.policy, 'backend', 'unknown')}, "
+            f"clip_obs={self.clip_obs:.1f}, "
+            f"zero_cmd_suppression={self.enable_zero_cmd_suppression}, "
+            f"hold_zero_command_pose={self.hold_zero_command_pose}"
+        )
+
+    def reset(self, prime_obs: np.ndarray | None = None) -> None:
+        self.last_actions = np.zeros(16, dtype=np.float32)
+        self._command_release_alpha = 0.0
+
+    def _is_zero_command(self, command: np.ndarray, base_ang_vel: np.ndarray) -> bool:
+        cmd_is_zero = (
+            np.linalg.norm(command[:2]) < self.zero_cmd_lin_thresh
+            and abs(command[2]) < self.zero_cmd_yaw_thresh
+        )
+        if not self.zero_cmd_use_yaw_rate:
+            return cmd_is_zero
+        return cmd_is_zero and abs(base_ang_vel[2]) < self.zero_yaw_rate_thresh
+
+    def command_activation_metrics(self, command: np.ndarray) -> tuple[float, float]:
+        command = np.asarray(command, dtype=np.float32)
+        planar = float(np.linalg.norm(command[:2]))
+        yaw = float(abs(command[2]))
+        return planar, yaw
+
+    def is_command_active(self, command: np.ndarray) -> bool:
+        planar, yaw = self.command_activation_metrics(command)
+        return planar >= self.zero_cmd_lin_thresh or yaw >= self.zero_cmd_yaw_thresh
+
+    def step(self, obs: np.ndarray, dt: float = 0.02) -> tuple[np.ndarray, np.ndarray]:
+        obs = np.asarray(obs, dtype=np.float32)
+        expected_obs_dim = int(self.policy.expected_obs_dim)
+        if obs.shape[0] != expected_obs_dim:
+            raise ValueError(
+                f"Observation dim mismatch: got {obs.shape[0]}, expected {expected_obs_dim}."
+            )
+        if self.clip_obs > 0.0:
+            obs = np.clip(obs, -self.clip_obs, self.clip_obs).astype(np.float32, copy=False)
+
+        obs_tensor = torch.tensor(obs, dtype=torch.float32, device=self.device).unsqueeze(0)
+        with torch.no_grad():
+            raw_actions = self.policy(obs_tensor).squeeze(0).cpu().numpy()
+
+        raw_actions = np.clip(raw_actions, -10.0, 10.0).astype(np.float32)
+        command = obs[6:9]
+        base_ang_vel = obs[0:3] / 0.25
+        zero_command = self._is_zero_command(command, base_ang_vel)
+        if zero_command:
+            self._command_release_alpha = 0.0
+            if self.hold_zero_command_pose:
+                raw_actions[:] = 0.0
+            elif self.enable_zero_cmd_suppression:
+                raw_actions[12:16] = 0.0
+                raw_actions[:12] *= 0.5
+        else:
+            self._command_release_alpha = min(1.0, self._command_release_alpha + dt / self.command_release_s)
+            raw_actions *= self._command_release_alpha
+
+        self.last_actions = raw_actions.copy()
+        scaled_actions = raw_actions * self.action_scale
+        return scaled_actions, raw_actions

05_software/real/sim2real_v2/requirements-orin.txt

@@ -0,0 +1,8 @@
+numpy
+PyYAML
+onnxruntime
+pyserial
+
+# Optional:
+# torch         # only needed to export/check .pt policies
+# pynput        # only needed for CLI keyboard control

05_software/real/sim2real_v2/safety/runtime_guard.py

@@ -0,0 +1,78 @@
+"""通用运行期守护：每个控制周期调用一次，无副作用，只做检查。
+
+设计原则：
+- 守护函数本身不下发动作、不打印（除非 verbose），只返回判定
+- 调用方决定收到 GuardStop 时怎么办（damping_brake 或 raise）
+- 起立期 / 等待期 / 主循环都共用同一组检查
+"""
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Optional
+
+import numpy as np
+
+
+class GuardLevel(IntEnum):
+    OK = 0
+    WARN = 1       # 仅记录，不停
+    STOP = 2       # 主调方应立刻 damping_brake + 退出当前阶段
+
+
+@dataclass
+class GuardDecision:
+    level: GuardLevel
+    reason: str  # 触发时人类可读说明，OK 时为空
+
+
+class RuntimeGuard:
+    """启动/起立/主循环共用的安全守护。
+
+    不监控目标位置范围（那是 SafetyMonitor 的职责）。这里只关心
+    机身整体状态：是否倾倒、是否翻滚、是否检测到 NaN、用户是否按急停。
+    """
+
+    def __init__(self,
+                 max_ang_vel: float = 12.0,
+                 max_tilt_z: float = -0.30,
+                 imu_age_warn_ms: float = 60.0,
+                 imu_age_stop_ms: float = 200.0):
+        self.max_ang_vel = max_ang_vel
+        self.max_tilt_z = max_tilt_z
+        self.imu_age_warn_ms = imu_age_warn_ms
+        self.imu_age_stop_ms = imu_age_stop_ms
+
+    def check(self,
+              imu_gyro: np.ndarray,
+              projected_gravity: np.ndarray,
+              imu_age_ms: float,
+              estop_triggered: bool,
+              extra_nan_arrays: tuple = ()) -> GuardDecision:
+        # 1) 用户急停
+        if estop_triggered:
+            return GuardDecision(GuardLevel.STOP, "user E-stop")
+
+        # 2) NaN 检查（任意输入数组中出现 NaN）
+        for arr in (imu_gyro, projected_gravity, *extra_nan_arrays):
+            if arr is None:
+                continue
+            if np.any(np.isnan(arr)) or np.any(np.isinf(arr)):
+                return GuardDecision(GuardLevel.STOP, "NaN/Inf detected in observation/action")
+
+        # 3) IMU 数据陈旧
+        if imu_age_ms > self.imu_age_stop_ms:
+            return GuardDecision(GuardLevel.STOP, f"IMU stale {imu_age_ms:.0f}ms")
+        warned_imu = imu_age_ms > self.imu_age_warn_ms
+
+        # 4) 倾倒
+        if projected_gravity[2] > self.max_tilt_z:
+            return GuardDecision(GuardLevel.STOP,
+                                 f"tilt: g_z={projected_gravity[2]:.3f}")
+
+        # 5) 角速度爆表
+        ang_norm = float(np.linalg.norm(imu_gyro))
+        if ang_norm > self.max_ang_vel:
+            return GuardDecision(GuardLevel.STOP, f"ang_vel overflow: |w|={ang_norm:.2f}")
+
+        if warned_imu:
+            return GuardDecision(GuardLevel.WARN, f"IMU age {imu_age_ms:.0f}ms")
+        return GuardDecision(GuardLevel.OK, "")

05_software/real/sim2real_v2/safety/safety_monitor.py

@@ -0,0 +1,116 @@
+"""三级安全监控（对应方法论 97.11）。
+
+Level 0: 正常
+Level 1: 限幅（位置/速度异常）— 截断目标位置幅值，记录连续触发次数
+Level 2: 刹车（连续限幅 N 次 / IMU 角速度过大 / 倾倒）— 卸载刚度只留阻尼
+Level 3: 急停（用户触发）— 让上层断电
+
+设计原则：监控只判定，不直接关电机；返回 SafetyDecision 由上层决策。
+"""
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Any, Optional
+
+import numpy as np
+
+
+class SafetyLevel(IntEnum):
+    NORMAL = 0
+    CLIP = 1
+    BRAKE = 2
+    ESTOP = 3
+
+
+@dataclass
+class SafetyDecision:
+    level: SafetyLevel
+    message: str
+    clipped_target: Optional[np.ndarray]
+    details: Optional[dict[str, Any]] = None
+
+
+class SafetyMonitor:
+    """安全监控（按 50Hz 控制频率调用）。
+
+    Args:
+        max_target_offset: 单关节相对默认位姿的最大偏离 (rad)
+        max_ang_vel: IMU 角速度模 (rad/s)
+        max_tilt_rad: 机身重力 z 轴投影低于该值认为已严重倾倒
+        clip_to_brake: 连续 clip 多少帧升级为刹车
+    """
+
+    def __init__(self,
+                 max_target_offset: float = 0.6,
+                 max_ang_vel: float = 10.0,
+                 max_tilt_z: float = -0.3,
+                 clip_to_brake: int = 0,
+                 hard_target_offset: float = 1.2):
+        self.max_target_offset = max_target_offset
+        self.max_ang_vel = max_ang_vel
+        self.max_tilt_z = max_tilt_z  # projected_gravity z 应当 ~ -1，明显小于 -0.3 视作倾倒
+        self.clip_to_brake = clip_to_brake
+        self.hard_target_offset = hard_target_offset
+        self.consecutive_clips = 0
+
+    def check(self,
+              target_pose: np.ndarray,
+              default_pose: np.ndarray,
+              imu_gyro: np.ndarray,
+              projected_gravity: np.ndarray,
+              estop_triggered: bool) -> SafetyDecision:
+        if estop_triggered:
+            return SafetyDecision(SafetyLevel.ESTOP, "user E-stop", None, None)
+
+        # 倾倒（projected_gravity[2] 应在 -1 附近，越接近 0 越倾斜）
+        if projected_gravity[2] > self.max_tilt_z:
+            return SafetyDecision(
+                SafetyLevel.BRAKE,
+                f"tilt detected: g_z={projected_gravity[2]:.3f}",
+                None,
+                {"g_z": float(projected_gravity[2])},
+            )
+
+        # 角速度爆表（猛烈翻滚）
+        if np.linalg.norm(imu_gyro) > self.max_ang_vel:
+            return SafetyDecision(
+                SafetyLevel.BRAKE,
+                f"angular velocity overflow: |w|={np.linalg.norm(imu_gyro):.2f}",
+                None,
+                {"ang_vel_norm": float(np.linalg.norm(imu_gyro))},
+            )
+
+        # 目标位置偏离过大 → 截断到允许范围
+        offset_leg = target_pose[:12] - default_pose[:12]
+        clipped_offset = np.clip(offset_leg, -self.max_target_offset, self.max_target_offset)
+        if not np.allclose(offset_leg, clipped_offset):
+            self.consecutive_clips += 1
+            clipped = target_pose.copy()
+            clipped[:12] = default_pose[:12] + clipped_offset
+            exceeded = np.where(np.abs(offset_leg) > self.max_target_offset)[0].tolist()
+            max_offset = float(np.max(np.abs(offset_leg)))
+            details = {
+                "joint_indices": exceeded,
+                "max_leg_offset": max_offset,
+                "consecutive_clips": int(self.consecutive_clips),
+            }
+            if self.hard_target_offset > 0.0 and max_offset > self.hard_target_offset:
+                return SafetyDecision(
+                    SafetyLevel.BRAKE,
+                    f"target leg offset exceeds hard limit: {max_offset:.3f}",
+                    clipped,
+                    details,
+                )
+            if self.clip_to_brake > 0 and self.consecutive_clips >= self.clip_to_brake:
+                return SafetyDecision(
+                    SafetyLevel.BRAKE,
+                    f"clipped {self.consecutive_clips} frames in a row",
+                    clipped,
+                    details,
+                )
+            return SafetyDecision(SafetyLevel.CLIP, "target leg offset out of range", clipped, details)
+
+        self.consecutive_clips = 0
+        return SafetyDecision(SafetyLevel.NORMAL, "", None, None)
+
+    def reset(self):
+        self.consecutive_clips = 0

05_software/real/sim2real_v2/startup/pose_initializer.py

@@ -0,0 +1,351 @@
+"""起立姿态初始化器（实测起点版本）。
+
+设计：
+  - 不再假设机器人的物理起始姿态（不再有 CRAWL_POSE / GROUND_POSE 起点）
+  - enable 后从 io.read_measured_pose() 读 16 关节实测，直接作为插值起点
+  - 余弦插值到 STAND_POSE，transition_time 根据最大偏差自适应
+  - 全程 RuntimeGuard 守护（空格急停/倾倒/翻滚/NaN/IMU 陈旧）
+  - 50Hz 写 LogBundle CSV（phase 字段标识阶段）
+
+Phase 流程：
+  STARTUP_SOFT_HOLD  — 软起步保持实测姿态，kp 从 0.125 渐升到 1.0
+  STARTUP_TRANSITION — 实测起点 → STAND 余弦插值
+  STARTUP_HOLD_AFTER — 站稳后保持 1 秒
+"""
+import time
+from typing import Optional
+
+import numpy as np
+
+from safety.runtime_guard import GuardLevel, RuntimeGuard
+from tools.logger import LogBundle
+from tools.math_utils import get_gravity_orientation
+
+
+# 仅作为目标姿态使用（训练侧 default_dof_pos）
+STAND_POSE = np.array([
+    0.0, 0.9, -1.8,
+    0.0, 0.9, -1.8,
+    0.0, 0.9, -1.8,
+    0.0, 0.9, -1.8,
+    0.0, 0.0, 0.0, 0.0,
+], dtype=np.float32)
+
+
+def _periodic_leg_delta(src: np.ndarray, dst: np.ndarray) -> np.ndarray:
+    """Return shortest signed leg-joint delta from src to dst."""
+    delta = np.asarray(dst[:12], dtype=np.float32) - np.asarray(src[:12], dtype=np.float32)
+    return ((delta + np.pi) % (2.0 * np.pi) - np.pi).astype(np.float32)
+
+
+class PoseInitFailed(RuntimeError):
+    """起立流程触发安全停止。main.py 捕获后立即 damping_brake。"""
+
+
+class PoseInitializer:
+    def __init__(self, real_io, control_dt: float = 0.02,
+                 transition_time_min: float = 2.0,
+                 transition_time_max: float = 6.0,
+                 transition_seconds_per_rad: float = 1.5,
+                 hold_time: float = 1.0,
+                 settle_pos_threshold: float = 0.12,
+                 settle_vel_threshold: float = 0.6,
+                 timeout_extra: float = 3.0,
+                 imu_fresh_wait_s: float = 1.0,
+                 progress_log_interval: float = 0.5,
+                 ramp_kp_time: float = 1.0,
+                 soft_hold_duration: float = 1.0,
+                 max_dev_warn: float = 1.5,
+                 max_dev_abort: float = 3.0):
+        """
+        Args:
+            transition_time_min/max/_per_rad: 自适应公式
+                t = clip(min, max, max_dev * seconds_per_rad)
+            timeout_extra: 起立超时 = transition_time + timeout_extra
+            soft_hold_duration: 起立前先在实测姿态保持几秒，期间 kp ramp-up
+            max_dev_warn: 最大偏差超过此值打警告（仅日志）
+            max_dev_abort: 最大偏差超过此值直接 PoseInitFailed（拒绝起立）
+        """
+        self.io = real_io
+        self.control_dt = control_dt
+        self.transition_time_min = transition_time_min
+        self.transition_time_max = transition_time_max
+        self.transition_seconds_per_rad = transition_seconds_per_rad
+        self.hold_time = hold_time
+        self.settle_pos_threshold = settle_pos_threshold
+        self.settle_vel_threshold = settle_vel_threshold
+        self.timeout_extra = timeout_extra
+        self.imu_fresh_wait_s = max(float(imu_fresh_wait_s), 0.0)
+        self.progress_log_interval = progress_log_interval
+        self.ramp_kp_time = ramp_kp_time
+        self.soft_hold_duration = soft_hold_duration
+        self.max_dev_warn = max_dev_warn
+        self.max_dev_abort = max_dev_abort
+
+        self.logger: Optional[LogBundle] = None
+        self.guard: Optional[RuntimeGuard] = None
+        self.keyboard = None
+
+    def attach(self, logger: LogBundle, guard: RuntimeGuard, keyboard):
+        self.logger = logger
+        self.guard = guard
+        self.keyboard = keyboard
+
+    # ---- 通用每周期工作 ----
+    def _tick(self, phase: str, sim_target: np.ndarray, kp_scale: float, next_exec: float):
+        """读状态 → guard 检查 → 写日志 → 锁帧。返回 (state_dict, next_exec)。
+        若 guard.STOP，立即抛 PoseInitFailed。"""
+        loop_t0 = time.perf_counter()
+
+        state = self.io.read_state()
+        proj_g = get_gravity_orientation(state["quat_wxyz"])
+
+        guard_dec = None
+        if self.guard is not None:
+            estop = bool(self.keyboard and self.keyboard.is_estop_triggered())
+            guard_dec = self.guard.check(
+                imu_gyro=state["imu_gyro"],
+                projected_gravity=proj_g,
+                imu_age_ms=float(state["imu_age_ms"]),
+                estop_triggered=estop,
+                extra_nan_arrays=(sim_target, state["joint_pos"], state["joint_vel"]),
+            )
+
+        if self.logger is not None:
+            motor_diag = state.get("motor_stale", {})
+            self.logger.state(
+                phase=phase,
+                joint_pos=state["joint_pos"],
+                joint_vel=state["joint_vel"],
+                joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)),
+                target_pose=sim_target,
+                raw_action=None,
+                gyro=state["imu_gyro"],
+                accel=state["imu_accel"],
+                quat=state["quat_wxyz"],
+                proj_gravity=proj_g,
+                command=np.zeros(3, dtype=np.float32),
+                imu_age_ms=float(state["imu_age_ms"]),
+                loop_dt_ms=(time.perf_counter() - loop_t0) * 1000.0,
+                safety_level=0,
+                guard_level=int(guard_dec.level) if guard_dec else 0,
+                holdover=int(motor_diag.get("holdover_this_frame", 0)),
+                stale_max=int(motor_diag.get("stale_max", 0)),
+                fresh_count=int(motor_diag.get("fresh_count", 16)),
+                kp_scale=kp_scale,
+                nan_flag=int(np.any(np.isnan(state["joint_pos"]))),
+                kp_leg_cmd=float(getattr(self.io, "hold_kp_leg", self.io.kp_leg) * kp_scale),
+                kd_leg_cmd=float(getattr(self.io, "hold_kd_leg", self.io.kd_leg)),
+                kd_wheel_cmd=float(self.io.kd_wheel),
+                target_source="startup_hold",
+                guard_reason=guard_dec.reason if guard_dec else "",
+            )
+
+        if guard_dec is not None and guard_dec.level == GuardLevel.STOP:
+            if self.logger:
+                self.logger.event("GUARD_STOP", phase=phase, reason=guard_dec.reason)
+            raise PoseInitFailed(f"[{phase}] {guard_dec.reason}")
+
+        next_exec += self.control_dt
+        slack = next_exec - time.perf_counter()
+        if slack > 0:
+            coarse = slack - 0.002
+            if coarse > 0:
+                time.sleep(coarse)
+            micro_slack = next_exec - time.perf_counter()
+            if micro_slack > 0:
+                time.sleep(min(micro_slack, 0.001))
+        else:
+            next_exec = time.perf_counter()
+        return state, next_exec
+
+    # ---- 主入口：从实测姿态起立到 STAND ----
+    def transition_to_stand_from_current(self,
+                                          target_pose: Optional[np.ndarray] = None
+                                          ) -> np.ndarray:
+        """完整起立流程：
+          1. 读实测起点
+          2. 偏差检查（warn / abort）
+          3. SOFT_HOLD：保持实测姿态 + kp ramp-up
+          4. TRANSITION：余弦插值到 target，transition_time 自适应
+          5. HOLD_AFTER：保持 1 秒
+        返回最终 target_pose（供主循环使用）。
+        """
+        if target_pose is None:
+            target_pose = STAND_POSE.copy()
+        target_pose = target_pose.astype(np.float32).copy()
+        target_pose[12:] = 0.0
+
+        # === 1. 读实测起点（要求电机反馈完整）===
+        ok, missing = self.io.wait_feedback_ready(max_attempts=20, poll_interval=0.05)
+        if not ok:
+            msg = f"feedback incomplete: {len(missing)} motors no response: {missing[:4]}"
+            if self.logger:
+                self.logger.event("STARTUP_NO_FEEDBACK",
+                                  missing=[m[2] for m in missing])
+            raise PoseInitFailed(msg)
+
+        last_imu_age = -1.0
+        imu_deadline = time.perf_counter() + self.imu_fresh_wait_s
+        while self.imu_fresh_wait_s > 0.0 and time.perf_counter() < imu_deadline:
+            state = self.io.read_state()
+            last_imu_age = float(state.get("imu_age_ms", 1e9))
+            if last_imu_age <= 60.0:
+                break
+            time.sleep(self.control_dt)
+        else:
+            if self.imu_fresh_wait_s > 0.0 and self.logger:
+                self.logger.event("STARTUP_IMU_STALE_WARN", imu_age_ms=last_imu_age)
+
+        start_pose = self.io.read_measured_pose().astype(np.float32).copy()
+        start_pose[12:] = 0.0   # 轮子起点固定为 0 速度
+
+        # === 2. 偏差检查 ===
+        startup_delta = _periodic_leg_delta(start_pose, target_pose)
+        diff = np.abs(startup_delta)
+        max_dev = float(np.max(diff))
+        max_dev_joint = int(np.argmax(diff))
+        transition_time = float(np.clip(
+            max_dev * self.transition_seconds_per_rad,
+            self.transition_time_min, self.transition_time_max
+        ))
+        timeout = transition_time + self.timeout_extra
+
+        if self.logger:
+            self.logger.event(
+                "STARTUP_PLAN",
+                start_pose_leg=start_pose[:12].tolist(),
+                target_pose_leg=target_pose[:12].tolist(),
+                max_dev=max_dev,
+                max_dev_joint_idx=max_dev_joint,
+                transition_time=transition_time,
+                timeout=timeout,
+            )
+        print(f"[PoseInit] 实测起点最大偏差 {max_dev:.3f} rad (关节 idx={max_dev_joint}); "
+              f"transition_time={transition_time:.2f}s")
+
+        if max_dev > self.max_dev_abort:
+            raise PoseInitFailed(
+                f"实测起点偏差过大 ({max_dev:.2f} rad > abort 阈值 "
+                f"{self.max_dev_abort})；请检查电机是否在合理姿势"
+            )
+        if max_dev > self.max_dev_warn:
+            print(f"[PoseInit] WARNING 偏差 {max_dev:.2f} rad > {self.max_dev_warn}; "
+                  f"起立可能比较剧烈")
+            if self.logger:
+                self.logger.event("STARTUP_LARGE_DEV", max_dev=max_dev)
+
+        # === 3. SOFT_HOLD：实测姿态 + kp ramp-up ===
+        if self.logger:
+            self.logger.event("STARTUP_SOFT_HOLD_BEGIN",
+                              duration=self.soft_hold_duration,
+                              ramp_kp_time=self.ramp_kp_time,
+                              ramp_kp_min=0.125)
+        n = max(1, int(self.soft_hold_duration / max(self.control_dt, 1e-3)))
+        next_exec = time.perf_counter()
+        t0 = next_exec
+        ramp_min = 0.125
+        for i in range(n):
+            elapsed = time.perf_counter() - t0
+            if elapsed < self.ramp_kp_time:
+                kp_scale = ramp_min + (1.0 - ramp_min) * (elapsed / self.ramp_kp_time)
+            else:
+                kp_scale = 1.0
+            self.io.hold_pose(start_pose, kp_scale=kp_scale)
+            _s, next_exec = self._tick("STARTUP_SOFT_HOLD", start_pose, kp_scale, next_exec)
+        if self.logger:
+            self.logger.event("STARTUP_SOFT_HOLD_END")
+
+        # === 4. TRANSITION：余弦插值 ===
+        if self.logger:
+            self.logger.event("STARTUP_TRANSITION_BEGIN",
+                              transition_time=transition_time, timeout=timeout)
+        print(f"[PoseInit] 起立: transition={transition_time:.2f}s, "
+              f"hold={self.hold_time}s, timeout={timeout:.2f}s")
+
+        t0 = time.perf_counter()
+        last_log = t0
+        reached = False
+        hold_start: Optional[float] = None
+        next_exec = t0
+
+        while True:
+            now = time.perf_counter()
+            elapsed = now - t0
+            phase = min(1.0, elapsed / max(transition_time, 1e-3))
+
+            if elapsed > timeout:
+                if self.logger:
+                    self.logger.event("STARTUP_TIMEOUT", elapsed=elapsed)
+                raise PoseInitFailed(
+                    f"transition timeout after {elapsed:.2f}s, target not reached"
+                )
+
+            blend = 0.5 - 0.5 * np.cos(np.pi * phase)
+            blended = start_pose.astype(np.float32).copy()
+            blended[:12] = start_pose[:12] + blend * startup_delta
+            blended[12:] = 0.0
+            self.io.hold_pose(blended, kp_scale=1.0)
+            state, next_exec = self._tick("STARTUP_TRANSITION", blended, 1.0, next_exec)
+
+            joint_pos = state["joint_pos"]
+            joint_vel = state["joint_vel"]
+            pos_err = float(np.max(np.abs(_periodic_leg_delta(joint_pos, target_pose))))
+            vel_err = float(np.max(np.abs(joint_vel[:12])))
+
+            if now - last_log >= self.progress_log_interval:
+                msg = (f"[PoseInit] phase={phase*100:5.1f}% | "
+                       f"max_pos_err={pos_err:.3f} | max_vel={vel_err:.3f}")
+                print(msg)
+                if self.logger:
+                    self.logger.event("STARTUP_PROGRESS",
+                                      phase=phase, pos_err=pos_err, vel_err=vel_err)
+                last_log = now
+
+            if (phase >= 1.0
+                    and pos_err <= self.settle_pos_threshold
+                    and vel_err <= self.settle_vel_threshold):
+                if not reached:
+                    reached = True
+                    hold_start = now
+                    if self.logger:
+                        self.logger.event("STARTUP_REACHED",
+                                          pos_err=pos_err, vel_err=vel_err)
+                    print(f"[PoseInit] 已到位，保持 {self.hold_time:.2f}s")
+                elif hold_start is not None and now - hold_start >= self.hold_time:
+                    break
+            elif phase >= 1.0:
+                reached = False
+                hold_start = None
+
+        # === 5. HOLD_AFTER ===
+        if self.logger:
+            self.logger.event("STARTUP_HOLD_AFTER_BEGIN", duration=self.hold_time)
+        n_hold = max(1, int(self.hold_time / max(self.control_dt, 1e-3)))
+        next_exec = time.perf_counter()
+        for _ in range(n_hold):
+            self.io.hold_pose(target_pose, kp_scale=1.0)
+            _s, next_exec = self._tick("STARTUP_HOLD_AFTER", target_pose, 1.0, next_exec)
+
+        if self.logger:
+            self.logger.event("STARTUP_TRANSITION_END")
+        print("[PoseInit] 默认站姿初始化完成")
+        return target_pose
+
+    # ---- 等用户回车（外部调用，期间持续保持） ----
+    def hold_until_user_confirm(self, target_pose: np.ndarray, evt) -> bool:
+        """阻塞循环到 evt.is_set()，期间持续 PD 保持站姿、跑 guard、写日志。
+        返回 True 正常确认，False 因 guard.STOP 中止。"""
+        if self.logger:
+            self.logger.event("WAIT_USER_BEGIN")
+        next_exec = time.perf_counter()
+        while not evt.is_set():
+            self.io.hold_pose(target_pose, kp_scale=1.0)
+            try:
+                _s, next_exec = self._tick("WAIT_USER", target_pose, 1.0, next_exec)
+            except PoseInitFailed as e:
+                print(f"[PoseInit] WAIT_USER 期间触发停止: {e}")
+                return False
+        if self.logger:
+            self.logger.event("WAIT_USER_END")
+        return True

05_software/real/sim2real_v2/startup/stand_balance.py

@@ -0,0 +1,141 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Any, Dict
+
+import numpy as np
+
+
+@dataclass
+class StandBalanceDebug:
+    roll: float
+    pitch: float
+    roll_rate: float
+    pitch_rate: float
+    hip_base: float
+    knee_base: float
+    roll_corr: float
+    pitch_corr: float
+    pitch_compensation_enabled: bool
+    target: list[float]
+    stable: bool
+
+
+class StandBalanceController:
+    def __init__(self, cfg: Dict[str, Any], control_dt: float):
+        self.enabled = bool(cfg.get("enabled", True))
+        self.control_dt = float(control_dt)
+        self.height = float(cfg.get("height", 0.33))
+        self.kp_roll = float(cfg.get("kp_roll", 0.85))
+        self.pitch_compensation_enabled = bool(cfg.get("pitch_compensation_enabled", True))
+        self.kp_pitch = float(cfg.get("kp_pitch", 0.70))
+        self.kd_roll_rate = float(cfg.get("kd_roll_rate", 0.03))
+        self.kd_pitch_rate = float(cfg.get("kd_pitch_rate", 0.025))
+        self.pitch_deadband = float(np.radians(cfg.get("pitch_deadband_deg", 0.0)))
+        self.pitch_corr_clip = float(cfg.get("pitch_corr_clip", 0.12))
+        self.pitch_corr_filter_alpha = float(np.clip(cfg.get("pitch_corr_filter_alpha", 1.0), 0.0, 1.0))
+        self.pitch_front_sign = float(cfg.get("pitch_front_sign", -1.0))
+        self.lateral_lean_gain = float(cfg.get("lateral_lean_gain", 0.0))
+        self.hip_abduction_clip = float(cfg.get("hip_abduction_clip", 0.45))
+        self.hip_pitch_clip = tuple(cfg.get("hip_pitch_clip", [-1.0, 2.5]))
+        self.knee_clip = tuple(cfg.get("knee_clip", [-2.6, -0.3]))
+        self.stable_roll_deg = float(cfg.get("stable_roll_deg", 6.0))
+        self.stable_pitch_deg = float(cfg.get("stable_pitch_deg", 8.0))
+        self.stable_gyro_deg_s = float(cfg.get("stable_gyro_deg_s", 45.0))
+        self.enter_hold_s = float(cfg.get("enter_hold_s", 1.0))
+
+        self.profile_h = np.asarray(
+            cfg.get("profile_h", [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]),
+            dtype=np.float32,
+        )
+        self.profile_hip = np.asarray(
+            cfg.get("profile_hip", [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]),
+            dtype=np.float32,
+        )
+        self.profile_knee = np.asarray(
+            cfg.get("profile_knee", [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]),
+            dtype=np.float32,
+        )
+        self._stable_time = 0.0
+        self._pitch_corr_filtered = 0.0
+        self._last_debug = StandBalanceDebug(0.0, 0.0, 0.0, 0.0, 0.9, -1.8, 0.0, 0.0, False, [], False)
+
+    @property
+    def last_debug(self) -> StandBalanceDebug:
+        return self._last_debug
+
+    def reset(self) -> None:
+        self._stable_time = 0.0
+        self._pitch_corr_filtered = 0.0
+
+    def _estimate_roll_pitch(self, projected_gravity: np.ndarray) -> tuple[float, float]:
+        gx, gy, gz = [float(v) for v in projected_gravity]
+        roll = float(np.arctan2(-gy, max(1e-6, -gz)))
+        pitch = float(np.arctan2(gx, np.sqrt(max(1e-6, gy * gy + gz * gz))))
+        return roll, pitch
+
+    def _base_leg_pose(self) -> tuple[float, float]:
+        h_clamp = float(np.clip(self.height, float(self.profile_h[0]), float(self.profile_h[-1])))
+        hip = float(np.interp(h_clamp, self.profile_h, self.profile_hip))
+        knee = float(np.interp(h_clamp, self.profile_h, self.profile_knee))
+        return hip, knee
+
+    def compute_target(self, state: Dict[str, Any], command: np.ndarray | None = None) -> np.ndarray:
+        projected_gravity = np.asarray(state["projected_gravity"], dtype=np.float32)
+        imu_gyro = np.asarray(state["imu_gyro"], dtype=np.float32)
+        cmd = np.zeros(3, dtype=np.float32) if command is None else np.asarray(command, dtype=np.float32)
+
+        hip_base, knee_base = self._base_leg_pose()
+        roll, pitch = self._estimate_roll_pitch(projected_gravity)
+        roll_rate = float(imu_gyro[0])
+        pitch_rate = float(imu_gyro[1])
+
+        roll_corr = -self.kp_roll * roll - self.kd_roll_rate * roll_rate
+        if self.pitch_compensation_enabled:
+            pitch_for_ctrl = 0.0 if abs(pitch) < self.pitch_deadband else pitch
+            pitch_corr_raw = -self.kp_pitch * pitch_for_ctrl - self.kd_pitch_rate * pitch_rate
+            pitch_corr_raw = float(np.clip(pitch_corr_raw, -self.pitch_corr_clip, self.pitch_corr_clip))
+            alpha = self.pitch_corr_filter_alpha
+            pitch_corr = (1.0 - alpha) * self._pitch_corr_filtered + alpha * pitch_corr_raw
+            self._pitch_corr_filtered = pitch_corr
+        else:
+            pitch_corr = 0.0
+            self._pitch_corr_filtered = 0.0
+        lateral_lean = self.lateral_lean_gain * float(cmd[1])
+
+        target = np.zeros(16, dtype=np.float32)
+        for leg_idx in range(4):
+            side = 1.0 if leg_idx in (0, 2) else -1.0
+            fore_aft = self.pitch_front_sign if leg_idx in (0, 1) else -self.pitch_front_sign
+            target[leg_idx * 3 + 0] = float(
+                np.clip(side * roll_corr + lateral_lean, -self.hip_abduction_clip, self.hip_abduction_clip)
+            )
+            target[leg_idx * 3 + 1] = float(
+                np.clip(hip_base + fore_aft * pitch_corr, self.hip_pitch_clip[0], self.hip_pitch_clip[1])
+            )
+            target[leg_idx * 3 + 2] = float(np.clip(knee_base, self.knee_clip[0], self.knee_clip[1]))
+        target[12:] = 0.0
+
+        stable = (
+            abs(np.degrees(roll)) <= self.stable_roll_deg
+            and abs(np.degrees(pitch)) <= self.stable_pitch_deg
+            and max(abs(np.degrees(roll_rate)), abs(np.degrees(pitch_rate))) <= self.stable_gyro_deg_s
+        )
+        self._stable_time = self._stable_time + self.control_dt if stable else 0.0
+        self._last_debug = StandBalanceDebug(
+            roll=roll,
+            pitch=pitch,
+            roll_rate=roll_rate,
+            pitch_rate=pitch_rate,
+            hip_base=hip_base,
+            knee_base=knee_base,
+            roll_corr=roll_corr,
+            pitch_corr=pitch_corr,
+            pitch_compensation_enabled=self.pitch_compensation_enabled,
+            target=target.tolist(),
+            stable=stable,
+        )
+        return target
+
+    def is_stable(self) -> bool:
+        return self._stable_time >= self.enter_hold_s

05_software/real/sim2real_v2/tools/alignment_check.py

@@ -0,0 +1,244 @@
+"""Offline deployment alignment check for the current 53-D rough policy."""
+
+import argparse
+from pathlib import Path
+import sys
+
+import numpy as np
+import torch
+import yaml
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+
+from interface.motor_mapping import MotorMapping  # noqa: E402
+from policy.policy_runner import PolicyRunner  # noqa: E402
+
+
+def _load_manifest(manifest_path: Path) -> dict:
+    with open(manifest_path, "r", encoding="utf-8") as f:
+        return yaml.safe_load(f)
+
+
+def _load_config() -> dict:
+    config_path = Path(__file__).resolve().parents[1] / "config.yaml"
+    with open(config_path, "r", encoding="utf-8") as f:
+        return yaml.safe_load(f)
+
+
+def check(policy_path: Path, manifest_path: Path | None = None) -> int:
+    issues: list[tuple[str, str]] = []
+    manifest = _load_manifest(manifest_path) if manifest_path is not None else None
+    config = _load_config()
+
+    expected = (
+        ("fl", "hip_abduction"), ("fl", "hip_pitch"), ("fl", "knee"),
+        ("fr", "hip_abduction"), ("fr", "hip_pitch"), ("fr", "knee"),
+        ("rl", "hip_abduction"), ("rl", "hip_pitch"), ("rl", "knee"),
+        ("rr", "hip_abduction"), ("rr", "hip_pitch"), ("rr", "knee"),
+        ("fl", "wheel"), ("fr", "wheel"), ("rl", "wheel"), ("rr", "wheel"),
+    )
+    if MotorMapping.SIM_JOINT_ORDER != expected:
+        issues.append(("joint_order", "MotorMapping.SIM_JOINT_ORDER mismatch"))
+    else:
+        print("[Check] joint order: PASS")
+    expected_joint_names = [f"{leg}_{joint}" for leg, joint in expected]
+
+    manifest_enable_zero_cmd = True
+    if manifest is not None:
+        manifest_enable_zero_cmd = bool(
+            manifest.get("model", {}).get("enable_zero_cmd_suppression", True)
+        )
+
+    runner = PolicyRunner(
+        policy_path,
+        device=torch.device("cpu"),
+        enable_zero_cmd_suppression=manifest_enable_zero_cmd,
+        clip_obs=float(config.get("policy", {}).get("clip_obs", 100.0)),
+    )
+    obs_mean = np.asarray(runner.policy.obs_mean.detach().cpu().numpy(), dtype=np.float32)
+    obs_std = np.asarray(runner.policy.obs_std.detach().cpu().numpy(), dtype=np.float32)
+    if np.allclose(obs_mean, 0.0) and np.allclose(obs_std, 1.0):
+        print("[Check] obs normalizer: PASS (identity)")
+    else:
+        print(
+            f"[Check] obs normalizer: PASS "
+            f"(mean range=[{obs_mean.min():.3f},{obs_mean.max():.3f}], "
+            f"std range=[{obs_std.min():.3f},{obs_std.max():.3f}])"
+        )
+    if (obs_std < 1e-6).any():
+        issues.append(
+            (
+                "normalizer_zero_std",
+                f"obs_std has near-zero entries: {np.where(obs_std < 1e-6)[0].tolist()}",
+            )
+        )
+
+    raw_zero = np.zeros(runner.BASE_OBS_DIM, dtype=np.float32)
+    runner.reset(prime_obs=raw_zero)
+    _, raw = runner.step(raw_zero)
+    if np.max(np.abs(raw)) > 5.0:
+        issues.append(
+            (
+                "output_range",
+                f"raw action too large under zero obs: {np.max(np.abs(raw)):.3f}",
+            )
+        )
+    else:
+        print(f"[Check] zero-obs output range: PASS (max|raw|={np.max(np.abs(raw)):.3f})")
+
+    expected_default = np.array([0.0, 0.9, -1.8] * 4 + [0.0] * 4, dtype=np.float32)
+    if not np.allclose(runner.default_dof_pos, expected_default):
+        issues.append(("default_pose_mismatch", f"default_dof_pos mismatch: {runner.default_dof_pos}"))
+    else:
+        print("[Check] default_dof_pos: PASS")
+
+    if runner.BASE_OBS_DIM != 53:
+        issues.append(("obs_dim", f"base obs dim {runner.BASE_OBS_DIM} != 53"))
+    else:
+        print("[Check] actor obs dim: PASS (53)")
+
+    if manifest is not None:
+        declared_model = manifest.get("model", {})
+        declared_action = manifest.get("action", {})
+        declared_safety = manifest.get("safety", {})
+        declared_control = manifest.get("control", {})
+
+        if int(declared_model.get("obs_dim", -1)) != runner.policy.expected_obs_dim:
+            issues.append(
+                (
+                    "manifest_obs_dim",
+                    f"manifest obs_dim {declared_model.get('obs_dim')} != policy {runner.policy.expected_obs_dim}",
+                )
+            )
+        else:
+            print("[Check] manifest obs_dim: PASS")
+
+        if int(declared_model.get("action_dim", -1)) != runner.policy.expected_action_dim:
+            issues.append(
+                (
+                    "manifest_action_dim",
+                    f"manifest action_dim {declared_model.get('action_dim')} != policy {runner.policy.expected_action_dim}",
+                )
+            )
+        else:
+            print("[Check] manifest action_dim: PASS")
+
+        declared_clip_obs = float(declared_model.get("clip_obs", -1.0))
+        config_clip_obs = float(config.get("policy", {}).get("clip_obs", -2.0))
+        if declared_clip_obs != runner.clip_obs or config_clip_obs != runner.clip_obs:
+            issues.append(
+                (
+                    "clip_obs",
+                    f"clip_obs mismatch: manifest={declared_clip_obs}, config={config_clip_obs}, runner={runner.clip_obs}",
+                )
+            )
+        else:
+            print("[Check] clip_obs: PASS")
+
+        declared_default = np.asarray(declared_action.get("default_dof_pos", []), dtype=np.float32)
+        if declared_default.shape != runner.default_dof_pos.shape or not np.allclose(
+            declared_default, runner.default_dof_pos
+        ):
+            issues.append(("manifest_default_pose", "manifest default_dof_pos mismatch"))
+        else:
+            print("[Check] manifest default_dof_pos: PASS")
+
+        declared_scale = np.asarray(declared_action.get("scale", []), dtype=np.float32)
+        if declared_scale.shape != runner.action_scale.shape or not np.allclose(
+            declared_scale, runner.action_scale
+        ):
+            issues.append(("manifest_action_scale", "manifest action scale mismatch"))
+        else:
+            print("[Check] manifest action scale: PASS")
+
+        config_scale = np.asarray(config.get("policy", {}).get("action_scale", []), dtype=np.float32)
+        if config_scale.shape != runner.action_scale.shape or not np.allclose(config_scale, runner.action_scale):
+            issues.append(("config_action_scale", "config policy.action_scale mismatch"))
+        else:
+            print("[Check] config action scale: PASS")
+
+        declared_joint_order = list(declared_action.get("joint_order", []))
+        if declared_joint_order != expected_joint_names:
+            issues.append(("manifest_joint_order", "manifest action.joint_order mismatch"))
+        else:
+            print("[Check] manifest joint order: PASS")
+
+        declared_wheel_indices = list(declared_action.get("wheel_indices", []))
+        if declared_wheel_indices != [12, 13, 14, 15]:
+            issues.append(("manifest_wheel_indices", "manifest wheel_indices must be [12,13,14,15]"))
+        else:
+            print("[Check] manifest wheel indices: PASS")
+
+        if float(declared_safety.get("zero_cmd_lin_thresh", -1.0)) != runner.zero_cmd_lin_thresh:
+            issues.append(("manifest_zero_cmd_lin_thresh", "manifest zero_cmd_lin_thresh mismatch"))
+        if float(declared_safety.get("zero_cmd_yaw_thresh", -1.0)) != runner.zero_cmd_yaw_thresh:
+            issues.append(("manifest_zero_cmd_yaw_thresh", "manifest zero_cmd_yaw_thresh mismatch"))
+        if float(declared_safety.get("zero_yaw_rate_thresh", -1.0)) != runner.zero_yaw_rate_thresh:
+            issues.append(("manifest_zero_yaw_rate_thresh", "manifest zero_yaw_rate_thresh mismatch"))
+        if bool(declared_model.get("enable_zero_cmd_suppression", True)) != runner.enable_zero_cmd_suppression:
+            issues.append(("manifest_zero_cmd_switch", "manifest zero-command suppression switch mismatch"))
+        else:
+            print("[Check] manifest zero-command suppression: PASS")
+
+        if int(declared_control.get("control_freq_hz", -1)) != 50:
+            issues.append(("manifest_control_freq", "manifest control_freq_hz must be 50"))
+        else:
+            print("[Check] manifest control freq: PASS")
+
+        if int(config.get("control_freq", -1)) != int(declared_control.get("control_freq_hz", -2)):
+            issues.append(("config_control_freq", "config control_freq != manifest control_freq_hz"))
+        else:
+            print("[Check] config control freq: PASS")
+
+        controller_cfg = config.get("controller", {}) or {}
+        gain_pairs = (
+            ("runtime_kp_leg", "kp_leg"),
+            ("runtime_kd_leg", "kd_leg"),
+            ("hold_kp_leg", "hold_kp_leg"),
+            ("hold_kd_leg", "hold_kd_leg"),
+            ("kd_wheel", "kd_wheel"),
+        )
+        for manifest_key, config_key in gain_pairs:
+            manifest_value = float(declared_control.get(manifest_key, -9999.0))
+            config_value = float(controller_cfg.get(config_key, -9998.0))
+            if not np.isclose(manifest_value, config_value):
+                issues.append(
+                    (
+                        "control_gains",
+                        f"{manifest_key}/{config_key} mismatch: manifest={manifest_value}, config={config_value}",
+                    )
+                )
+        if not any(tag == "control_gains" for tag, _ in issues):
+            print("[Check] control gains: PASS")
+
+        manifest_filter = declared_control.get("command_filter", {}) or {}
+        config_filter = config.get("command_filter", {}) or {}
+        for key in ("enabled", "max_vx_acc", "max_vy_acc", "max_yaw_acc"):
+            if manifest_filter.get(key) != config_filter.get(key):
+                issues.append(("command_filter", f"command_filter.{key} mismatch"))
+        if not any(tag == "command_filter" for tag, _ in issues):
+            print("[Check] command filter config: PASS")
+
+    if issues:
+        print("\n" + "=" * 60)
+        print(f"Alignment check failed: {len(issues)} issue(s)")
+        for tag, msg in issues:
+            print(f"  [{tag}] {msg}")
+        return 1
+
+    print("\n" + "=" * 60)
+    print("All offline alignment checks passed.")
+    return 0
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--policy", type=str, required=True, help="Path to policy .onnx or .pt")
+    parser.add_argument("--manifest", type=str, default=None, help="Optional deployment manifest yaml")
+    args = parser.parse_args()
+    manifest = Path(args.manifest) if args.manifest else None
+    sys.exit(check(Path(args.policy), manifest))
+
+
+if __name__ == "__main__":
+    main()

05_software/real/sim2real_v2/tools/analyze_log.py

@@ -0,0 +1,138 @@
+"""Summarize real-run logs for startup/stand/runtime diagnosis."""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import json
+import math
+from pathlib import Path
+import statistics
+
+LEG_JOINTS = (
+    "fl_hip_abd", "fl_hip_pitch", "fl_knee",
+    "fr_hip_abd", "fr_hip_pitch", "fr_knee",
+    "rl_hip_abd", "rl_hip_pitch", "rl_knee",
+    "rr_hip_abd", "rr_hip_pitch", "rr_knee",
+)
+
+
+def _f(row: dict[str, str], key: str, default: float = 0.0) -> float:
+    try:
+        return float(row.get(key, default))
+    except Exception:
+        return default
+
+
+def _stats(values: list[float]) -> str:
+    if not values:
+        return "--"
+    return (
+        f"mean={statistics.mean(values):.3f} "
+        f"std={statistics.pstdev(values):.3f} "
+        f"min={min(values):.3f} max={max(values):.3f}"
+    )
+
+
+def _pitch_deg(row: dict[str, str]) -> float:
+    gx = _f(row, "pgrav_x")
+    gy = _f(row, "pgrav_y")
+    gz = _f(row, "pgrav_z")
+    return math.degrees(math.atan2(gx, math.sqrt(max(1e-9, gy * gy + gz * gz))))
+
+
+def summarize(log_dir: Path) -> int:
+    state_path = log_dir / "state.csv"
+    events_path = log_dir / "events.jsonl"
+    if not state_path.exists():
+        print(f"[Analyze] missing {state_path}")
+        return 1
+
+    if events_path.exists():
+        print("[Analyze] key events:")
+        for line in events_path.read_text(encoding="utf-8", errors="replace").splitlines():
+            try:
+                ev = json.loads(line)
+            except Exception:
+                continue
+            if ev.get("kind") in {
+                "STARTUP_PLAN",
+                "STARTUP_REACHED",
+                "STAND_BALANCE_STABLE",
+                "RUNTIME_BEGIN",
+                "POLICY_TARGET_STALE",
+                "POLICY_TIMEOUT",
+                "SAFETY_BRAKE",
+                "GUARD_STOP",
+                "POSE_INIT_FAILED",
+            }:
+                detail = {k: v for k, v in ev.items() if k not in ("t", "t_rel")}
+                print(f"  t={ev.get('t_rel', 0):.2f}s {detail}")
+
+    with state_path.open(newline="", encoding="utf-8") as f:
+        rows = list(csv.DictReader(f))
+    print(f"\n[Analyze] state rows: {len(rows)}")
+    global_notes: list[str] = []
+    for phase in sorted({r.get("phase", "") for r in rows}):
+        phase_rows = [r for r in rows if r.get("phase") == phase]
+        if not phase_rows:
+            continue
+        pitch = [_pitch_deg(r) for r in phase_rows]
+        loop = [_f(r, "loop_dt_ms") for r in phase_rows]
+        imu = [_f(r, "imu_age_ms") for r in phase_rows]
+        print(f"\n[Phase] {phase} n={len(phase_rows)} t={phase_rows[0].get('t_rel')}..{phase_rows[-1].get('t_rel')}")
+        print(f"  pitch_deg {_stats(pitch)}")
+        print(f"  loop_ms   {_stats(loop)}")
+        print(f"  imu_age   {_stats(imu)}")
+        if "stand_pitch_corr" in phase_rows[0]:
+            stand_pitch = [_f(r, "stand_pitch_deg") for r in phase_rows]
+            stand_corr = [_f(r, "stand_pitch_corr") for r in phase_rows]
+            stand_enabled = [_f(r, "stand_pitch_comp_enabled") for r in phase_rows]
+            print(f"  stand_pitch_deg  {_stats(stand_pitch)}")
+            print(f"  stand_pitch_corr {_stats(stand_corr)} enabled_mean={statistics.mean(stand_enabled):.3f}")
+        for joint in ("fl_hip_pitch", "fr_hip_pitch", "rl_hip_pitch", "rr_hip_pitch", "fl_knee", "fr_knee", "rl_knee", "rr_knee"):
+            pos_key = f"{joint}_pos"
+            tgt_key = f"{joint}_tgt"
+            tau_key = f"{joint}_tau"
+            if pos_key in phase_rows[0] and tgt_key in phase_rows[0]:
+                err = [_f(r, pos_key) - _f(r, tgt_key) for r in phase_rows]
+                tau = [_f(r, tau_key) for r in phase_rows] if tau_key in phase_rows[0] else []
+                print(f"  {joint}_err {_stats(err)} tau {_stats(tau)}")
+        high_tau = []
+        high_err = []
+        for joint in LEG_JOINTS:
+            tau_key = f"{joint}_tau"
+            pos_key = f"{joint}_pos"
+            tgt_key = f"{joint}_tgt"
+            if tau_key in phase_rows[0]:
+                tau_abs_mean = statistics.mean(abs(_f(r, tau_key)) for r in phase_rows)
+                if tau_abs_mean > 6.0:
+                    high_tau.append((joint, tau_abs_mean))
+            if pos_key in phase_rows[0] and tgt_key in phase_rows[0]:
+                err_abs_mean = statistics.mean(abs(_f(r, pos_key) - _f(r, tgt_key)) for r in phase_rows)
+                if err_abs_mean > 0.08:
+                    high_err.append((joint, err_abs_mean))
+        if high_tau:
+            text = ", ".join(f"{name}:{value:.2f}Nm" for name, value in sorted(high_tau, key=lambda x: -x[1])[:4])
+            print(f"  high_tau_mean {text}")
+            global_notes.append(f"{phase}: high mean torque -> {text}")
+        if high_err:
+            text = ", ".join(f"{name}:{value:.3f}rad" for name, value in sorted(high_err, key=lambda x: -x[1])[:4])
+            print(f"  high_err_mean {text}")
+            global_notes.append(f"{phase}: high tracking error -> {text}")
+    if global_notes:
+        print("\n[Analyze] notes:")
+        for note in global_notes:
+            print(f"  - {note}")
+    return 0
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("log_dir", type=str)
+    args = parser.parse_args()
+    return summarize(Path(args.log_dir))
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

05_software/real/sim2real_v2/tools/calibrate_offsets.py

@@ -0,0 +1,163 @@
+"""零位偏移标定向导。
+
+用途：把机器人摆到 sim2sim/训练侧的 stand 默认姿态（人工摆好），
+跑这个脚本，它会读 16 个电机的当前位置，反算每个电机的 ZERO_OFFSET。
+
+关键公式（与 motor_mapping.py 一致）：
+    real = sign * sim + offset
+当 sim = stand_default 时：
+    offset = real - sign * stand_default
+
+⚠️ 使用前置条件：
+1. 已运行过 motor_driver_direction_test 类的脚本，确认每个电机的 sign 是对的；
+   sign 错的话本工具会算出错误的 offset 看起来很对，但发动作时机器人会反向冲撞
+2. 机器人物理上摆到 stand 姿态：四条腿微弯曲、轮子接地、机身水平
+3. 电机已 enable 并清除告警
+
+输出：把打印出来的 ZERO_OFFSET_MAP 字段直接覆盖 motor_mapping.py 中的对应字典。
+"""
+import argparse
+import sys
+import time
+from pathlib import Path
+
+import numpy as np
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+
+from interface.motor_mapping import MotorMapping  # noqa: E402
+from policy.policy_runner import PolicyRunner  # noqa: E402
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--can1-port", default="/dev/can1")
+    parser.add_argument("--can2-port", default="/dev/can2")
+    parser.add_argument("--motor-model", default="rs-02")
+    parser.add_argument("--samples", type=int, default=100,
+                        help="平均采样帧数（去抖动）")
+    parser.add_argument("--target-pose", default="stand", choices=["stand", "crawl"],
+                        help="标定时机器人摆的物理姿态")
+    parser.add_argument("--no-enable", action="store_true",
+                        help="不主动 enable 电机（仅读取，适合手动转关节标定）")
+    args = parser.parse_args()
+
+    # 真机驱动注入（路径优先级与 main.py 一致：vendored/drivers > /home/rc2/...）
+    sim2real_root = Path(__file__).resolve().parents[1]
+    for path in (sim2real_root / "vendored",
+                 "/home/rc2/work/rcwork/control",
+                 "/home/rc2/work/rcwork"):
+        sp = str(path)
+        if sp not in sys.path and Path(path).exists():
+            sys.path.append(sp)
+    from drivers.motor_driver import RobStrideDriver  # type: ignore
+
+    mapper = MotorMapping()
+    drv1 = RobStrideDriver(args.can1_port, debug=False)
+    drv2 = RobStrideDriver(args.can2_port, debug=False)
+    drv1.connect()
+    drv2.connect()
+
+    for jk in mapper.SIM_JOINT_ORDER:
+        leg, joint = jk
+        bus, mid = mapper.CAN_ID_MAP[jk]
+        name = f"{leg}_{joint}"
+        (drv1 if bus == 1 else drv2).add_motor(name, mid, args.motor_model)
+
+    if not args.no_enable:
+        print("[Calib] Enable 电机以读取状态...（已就位则可加 --no-enable 跳过）")
+        for drv in (drv1, drv2):
+            for name in drv.motors:
+                drv.clear_warnings(name)
+                drv.enable(name)
+        time.sleep(0.5)
+
+    # 选择标定姿态
+    if args.target_pose == "stand":
+        sim_pose = PolicyRunner.DEFAULT_STAND_POSE.copy()  # [0,0.9,-1.8] x4 + zeros
+    else:
+        sim_pose = np.array([
+            0.4, 1.65, -2.55, -0.4, 1.65, -2.55,
+            0.4, 1.65, -2.55, -0.4, 1.65, -2.55,
+            0.0, 0.0, 0.0, 0.0,
+        ], dtype=np.float32)
+
+    print(f"\n[Calib] 请把机器人物理摆成 {args.target_pose.upper()} 姿态：")
+    if args.target_pose == "stand":
+        print("  四条腿髋外展=0, 髋俯仰=0.9rad(~52°), 膝=-1.8rad(~-103°), 轮接地")
+    else:
+        print("  内收外展 ±0.4rad, 髋俯仰=1.65rad, 膝=-2.55rad（深蹲下趴）")
+    print("  轮子可以保持任意角度，offset 强制为 0")
+    print("  按回车开始采样...")
+    try:
+        input()
+    except EOFError:
+        pass
+
+    print(f"\n[Calib] 开始采样 {args.samples} 帧并平均...")
+    pos_acc = np.zeros(16, dtype=np.float64)
+    valid = 0
+    for i in range(args.samples):
+        drv1.process_messages()
+        drv2.process_messages()
+        real_pos = {}
+        for drv_idx, drv in enumerate((drv1, drv2)):
+            bus = drv_idx + 1
+            for name, motor in drv.motors.items():
+                parts = name.split("_", 1)
+                if len(parts) != 2:
+                    continue
+                key = (parts[0], parts[1])
+                if key not in mapper.CAN_ID_MAP:
+                    continue
+                _, mid = mapper.CAN_ID_MAP[key]
+                real_pos[(bus, mid)] = motor.state.position
+        if len(real_pos) == 16:
+            ordered = np.array([real_pos[mapper.CAN_ID_MAP[jk]]
+                                for jk in mapper.SIM_JOINT_ORDER], dtype=np.float64)
+            pos_acc += ordered
+            valid += 1
+        time.sleep(0.02)
+
+    if valid < args.samples * 0.5:
+        print(f"[Calib] 警告: 只收到 {valid}/{args.samples} 帧反馈，标定可能不可靠")
+    real_avg = pos_acc / max(valid, 1)
+
+    # 反算 offset：offset = real - sign * sim
+    sign = mapper._sign
+    offsets = real_avg - sign * sim_pose
+
+    # 轮子 offset 强制 0
+    for i, jk in enumerate(mapper.SIM_JOINT_ORDER):
+        if jk[1] == "wheel":
+            offsets[i] = 0.0
+
+    # 打印结果（按 motor_mapping.py 的字典格式）
+    print("\n" + "=" * 64)
+    print(f"[Calib] 标定完成（{valid} 帧平均）")
+    print("=" * 64)
+    print("把以下字典覆盖 sim2real/interface/motor_mapping.py 中的 ZERO_OFFSET_MAP:")
+    print()
+    print("    ZERO_OFFSET_MAP = {")
+    for i, jk in enumerate(mapper.SIM_JOINT_ORDER):
+        leg, joint = jk
+        cur = offsets[i]
+        old = mapper.ZERO_OFFSET_MAP[jk]
+        delta = cur - old
+        marker = " *" if abs(delta) > 0.01 else ""
+        print(f'        ("{leg}", "{joint:13s}"): {cur:>+8.4f},  '
+              f'# old={old:+.4f} delta={delta:+.4f}{marker}')
+    print("    }")
+    print("\n标记 * 的项与现表偏离 > 0.01 rad，请重点核对该关节的 sign 是否正确。\n")
+
+    # Disable
+    if not args.no_enable:
+        for drv in (drv1, drv2):
+            for name in drv.motors:
+                drv.disable(name)
+    drv1.disconnect()
+    drv2.disconnect()
+
+
+if __name__ == "__main__":
+    main()

05_software/real/sim2real_v2/tools/export_onnx.py

@@ -0,0 +1,80 @@
+"""Export the current PyTorch actor checkpoint to ONNX and verify parity."""
+
+from __future__ import annotations
+
+import argparse
+from pathlib import Path
+import sys
+
+import numpy as np
+import torch
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+
+from policy.policy_runner import load_policy  # noqa: E402
+
+
+def export_onnx(pt_path: Path, onnx_path: Path, opset: int = 14) -> None:
+    device = torch.device("cpu")
+    model = load_policy(pt_path, device)
+    if getattr(model, "backend", "torch") != "torch":
+        raise ValueError(f"export source must be a .pt policy, got {pt_path}")
+
+    obs_dim = int(model.expected_obs_dim)
+    dummy = torch.randn(1, obs_dim, dtype=torch.float32, device=device)
+    onnx_path.parent.mkdir(parents=True, exist_ok=True)
+
+    torch.onnx.export(
+        model,
+        dummy,
+        str(onnx_path),
+        export_params=True,
+        opset_version=opset,
+        do_constant_folding=True,
+        input_names=["obs"],
+        output_names=["action"],
+        dynamic_axes={"obs": {0: "batch_size"}, "action": {0: "batch_size"}},
+    )
+
+    try:
+        import onnxruntime as ort
+    except ImportError:
+        print("[Export] onnxruntime not installed; export done but parity check skipped.")
+        return
+
+    opts = ort.SessionOptions()
+    opts.intra_op_num_threads = 1
+    opts.inter_op_num_threads = 1
+    opts.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
+    session = ort.InferenceSession(str(onnx_path), sess_options=opts, providers=["CPUExecutionProvider"])
+    with torch.no_grad():
+        torch_out = model(dummy).detach().cpu().numpy()
+    ort_out = session.run([session.get_outputs()[0].name], {session.get_inputs()[0].name: dummy.cpu().numpy()})[0]
+    max_diff = float(np.max(np.abs(torch_out - ort_out)))
+    mean_diff = float(np.mean(np.abs(torch_out - ort_out)))
+    print(f"[Export] ONNX parity max_diff={max_diff:.8f}, mean_diff={mean_diff:.8f}")
+    if max_diff > 1e-4:
+        raise RuntimeError(f"ONNX parity check failed: max_diff={max_diff:.8f}")
+
+
+def main() -> int:
+    root = Path(__file__).resolve().parents[1]
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--pt", default=str(root / "policies" / "model_rough.pt"), help="Source .pt checkpoint")
+    parser.add_argument("--onnx", default=None, help="Destination .onnx path; default replaces .pt suffix")
+    parser.add_argument("--opset", type=int, default=14)
+    args = parser.parse_args()
+
+    pt_path = Path(args.pt)
+    onnx_path = Path(args.onnx) if args.onnx else pt_path.with_suffix(".onnx")
+    if not pt_path.exists():
+        print(f"[Export] missing source policy: {pt_path}")
+        return 1
+
+    export_onnx(pt_path, onnx_path, args.opset)
+    print(f"[Export] wrote {onnx_path}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

05_software/real/sim2real_v2/tools/logger.py

@@ -0,0 +1,296 @@
+"""Logging helpers for sim2real runs.
+
+Each session writes:
+- `state.csv`: high-rate state stream
+- `events.jsonl`: event / milestone stream
+"""
+
+import json
+import queue
+import threading
+import time
+from datetime import datetime
+from pathlib import Path
+from typing import Any, Dict, Optional
+
+import numpy as np
+
+
+class LogBundle:
+    """One session directory containing state CSV and event JSONL."""
+
+    JOINT_LABELS = (
+        "fl_hip_abd", "fl_hip_pitch", "fl_knee",
+        "fr_hip_abd", "fr_hip_pitch", "fr_knee",
+        "rl_hip_abd", "rl_hip_pitch", "rl_knee",
+        "rr_hip_abd", "rr_hip_pitch", "rr_knee",
+        "fl_wheel", "fr_wheel", "rl_wheel", "rr_wheel",
+    )
+
+    def __init__(self, log_root: str = "logs"):
+        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+        self.dir = Path(log_root) / timestamp
+        self.dir.mkdir(parents=True, exist_ok=True)
+
+        self.state_path = self.dir / "state.csv"
+        self.events_path = self.dir / "events.jsonl"
+
+        self._state_fp = open(self.state_path, "w", encoding="utf-8")
+        self._events_fp = open(self.events_path, "w", encoding="utf-8")
+        self._t0 = time.time()
+        self._closed = False
+        self._queue: "queue.Queue[tuple]" = queue.Queue(maxsize=20000)
+        self._dropped_state_rows = 0
+        self._writer_thread = threading.Thread(target=self._writer_loop, name="sim2real-log-writer", daemon=True)
+
+        self._write_state_header()
+        self._writer_thread.start()
+        self.event("LOG_START", session_dir=str(self.dir))
+        print(f"[Log] {self.dir}")
+
+    def _write_state_header(self):
+        cols = ["t", "t_rel", "phase"]
+        cols += [f"{joint}_pos" for joint in self.JOINT_LABELS]
+        cols += [f"{joint}_vel" for joint in self.JOINT_LABELS]
+        cols += [f"{joint}_tau" for joint in self.JOINT_LABELS]
+        cols += [f"{joint}_tgt" for joint in self.JOINT_LABELS]
+        cols += [f"{joint}_raw" for joint in self.JOINT_LABELS]
+        cols += ["gyro_x", "gyro_y", "gyro_z"]
+        cols += ["accel_x", "accel_y", "accel_z"]
+        cols += ["quat_w", "quat_x", "quat_y", "quat_z"]
+        cols += ["pgrav_x", "pgrav_y", "pgrav_z"]
+        cols += ["cmd_vx", "cmd_vy", "cmd_yaw"]
+        cols += ["imu_age_ms", "loop_dt_ms"]
+        cols += ["safety_level", "guard_level"]
+        cols += ["holdover", "stale_max", "fresh_count"]
+        cols += ["kp_scale", "nan_flag"]
+        cols += ["kp_leg_cmd", "kd_leg_cmd", "kd_wheel_cmd"]
+        cols += ["runtime_release_alpha", "runtime_release_hold_s", "runtime_blend_ratio"]
+        cols += ["hold_target_max_err", "policy_target_max_err", "hold_policy_max_gap"]
+        cols += [
+            "stand_roll_deg",
+            "stand_pitch_deg",
+            "stand_roll_corr",
+            "stand_pitch_corr",
+            "stand_pitch_comp_enabled",
+        ]
+        cols += ["target_source_code"]
+        cols += [
+            "clip_primary_joint_index",
+            "clip_primary_joint",
+            "clip_primary_target",
+            "clip_primary_measured",
+            "clip_primary_default",
+            "clip_primary_pos_err",
+            "clip_primary_raw",
+            "clip_primary_scaled",
+        ]
+        cols += ["safety_reason", "guard_reason"]
+        self._state_fp.write(",".join(cols) + "\n")
+        self._state_fp.flush()
+
+    def state(
+        self,
+        phase: str,
+        joint_pos: np.ndarray,
+        joint_vel: np.ndarray,
+        joint_torque: np.ndarray,
+        target_pose: np.ndarray,
+        raw_action: Optional[np.ndarray],
+        gyro: np.ndarray,
+        accel: np.ndarray,
+        quat: np.ndarray,
+        proj_gravity: np.ndarray,
+        command: np.ndarray,
+        imu_age_ms: float,
+        loop_dt_ms: float,
+        safety_level: int = 0,
+        guard_level: int = 0,
+        holdover: int = 0,
+        stale_max: int = 0,
+        fresh_count: int = 16,
+        kp_scale: float = 1.0,
+        nan_flag: int = 0,
+        kp_leg_cmd: float = 0.0,
+        kd_leg_cmd: float = 0.0,
+        kd_wheel_cmd: float = 0.0,
+        runtime_release_alpha: float = 0.0,
+        runtime_release_hold_s: float = 0.0,
+        runtime_blend_ratio: float = 0.0,
+        hold_target_max_err: float = 0.0,
+        policy_target_max_err: float = 0.0,
+        hold_policy_max_gap: float = 0.0,
+        stand_roll_deg: float = 0.0,
+        stand_pitch_deg: float = 0.0,
+        stand_roll_corr: float = 0.0,
+        stand_pitch_corr: float = 0.0,
+        stand_pitch_comp_enabled: bool = False,
+        target_source: str = "",
+        clip_primary_joint: str = "",
+        clip_primary_target: float = 0.0,
+        clip_primary_measured: float = 0.0,
+        clip_primary_default: float = 0.0,
+        clip_primary_pos_err: float = 0.0,
+        clip_primary_raw: float = 0.0,
+        clip_primary_scaled: float = 0.0,
+        safety_reason: str = "",
+        guard_reason: str = "",
+    ):
+        if self._closed:
+            return
+        if target_pose is None:
+            target_pose = np.zeros(16, dtype=np.float32)
+        if raw_action is None:
+            raw_action = np.zeros(16, dtype=np.float32)
+
+        now = time.time()
+        numeric_values = []
+        numeric_values += joint_pos.tolist()
+        numeric_values += joint_vel.tolist()
+        numeric_values += joint_torque.tolist()
+        numeric_values += target_pose.tolist()
+        numeric_values += raw_action.tolist()
+        numeric_values += gyro.tolist()
+        numeric_values += accel.tolist()
+        numeric_values += quat.tolist()
+        numeric_values += proj_gravity.tolist()
+        numeric_values += command.tolist()
+        numeric_values += [imu_age_ms, loop_dt_ms]
+        numeric_values += [safety_level, guard_level, holdover, stale_max, fresh_count, kp_scale, nan_flag]
+        numeric_values += [kp_leg_cmd, kd_leg_cmd, kd_wheel_cmd]
+        numeric_values += [runtime_release_alpha, runtime_release_hold_s, runtime_blend_ratio]
+        numeric_values += [hold_target_max_err, policy_target_max_err, hold_policy_max_gap]
+        numeric_values += [
+            stand_roll_deg,
+            stand_pitch_deg,
+            stand_roll_corr,
+            stand_pitch_corr,
+            1.0 if stand_pitch_comp_enabled else 0.0,
+        ]
+        numeric_values += [_target_source_code(target_source)]
+        numeric_values += [_csv_numeric_joint_index(clip_primary_joint)]
+        numeric_values += [
+            clip_primary_target,
+            clip_primary_measured,
+            clip_primary_default,
+            clip_primary_pos_err,
+            clip_primary_raw,
+            clip_primary_scaled,
+        ]
+
+        parts = [f"{now:.6f}", f"{now - self._t0:.6f}", phase]
+        parts += [f"{value:.6f}" for value in numeric_values]
+        parts += [_csv_escape(clip_primary_joint), _csv_escape(safety_reason), _csv_escape(guard_reason)]
+        self._enqueue(("state", ",".join(parts) + "\n"), drop_if_full=True)
+
+    def event(self, kind: str, **fields: Any):
+        if self._closed:
+            return
+        record = {"t": time.time(), "t_rel": time.time() - self._t0, "kind": kind}
+        for key, value in fields.items():
+            if isinstance(value, np.ndarray):
+                record[key] = value.tolist()
+            elif isinstance(value, (np.integer, np.floating)):
+                record[key] = value.item()
+            else:
+                record[key] = value
+        self._enqueue(("event", json.dumps(record, ensure_ascii=False) + "\n", kind != "STATE_TICK"), drop_if_full=False)
+        if kind != "STATE_TICK":
+            print(f"[Event {record['t_rel']:7.2f}s] {kind} {_short_fields(fields)}")
+
+    def flush(self):
+        if not self._closed:
+            self._queue.join()
+            self._state_fp.flush()
+            self._events_fp.flush()
+
+    def close(self):
+        if self._closed:
+            return
+        self.event("LOG_END")
+        self._queue.join()
+        self._closed = True
+        self._enqueue(("close",), drop_if_full=False, allow_after_closed=True)
+        self._writer_thread.join(timeout=2.0)
+        self._state_fp.flush()
+        self._state_fp.close()
+        self._events_fp.flush()
+        self._events_fp.close()
+        print(f"[Log] saved -> {self.dir}")
+
+    def _enqueue(self, item: tuple, drop_if_full: bool, allow_after_closed: bool = False):
+        if self._closed and not allow_after_closed:
+            return
+        try:
+            if drop_if_full:
+                self._queue.put_nowait(item)
+            else:
+                self._queue.put(item, timeout=0.2)
+        except queue.Full:
+            if item and item[0] == "state":
+                self._dropped_state_rows += 1
+
+    def _writer_loop(self):
+        while True:
+            item = self._queue.get()
+            try:
+                kind = item[0]
+                if kind == "close":
+                    return
+                if kind == "state":
+                    self._state_fp.write(item[1])
+                elif kind == "event":
+                    self._events_fp.write(item[1])
+                    if item[2]:
+                        self._events_fp.flush()
+            finally:
+                self._queue.task_done()
+
+
+def _csv_escape(text: str) -> str:
+    if not text:
+        return ""
+    return text.replace(",", ";").replace("\n", " ").replace("\r", " ")
+
+
+def _csv_numeric_joint_index(joint_name: str) -> float:
+    if not joint_name:
+        return -1.0
+    try:
+        return float(LogBundle.JOINT_LABELS.index(joint_name))
+    except ValueError:
+        return -1.0
+
+
+def _target_source_code(target_source: str) -> float:
+    mapping = {
+        "": -1.0,
+        "startup_hold": 0.0,
+        "stand_balance": 1.0,
+        "stand_hold": 1.5,
+        "runtime_hold": 2.0,
+        "runtime_blend": 3.0,
+        "runtime_policy": 4.0,
+    }
+    return mapping.get(target_source, 99.0)
+
+
+def _short_fields(fields: Dict[str, Any]) -> str:
+    parts = []
+    for key, value in fields.items():
+        if isinstance(value, (list, tuple, np.ndarray)):
+            arr = np.asarray(value).ravel()
+            if arr.size > 4:
+                continue
+            try:
+                parts.append(f"{key}=[{','.join(f'{float(x):.2f}' for x in arr)}]")
+            except (TypeError, ValueError):
+                parts.append(f"{key}={list(arr)[:4]}")
+        elif isinstance(value, float):
+            parts.append(f"{key}={value:.3f}")
+        else:
+            parts.append(f"{key}={value}")
+    return " ".join(parts)
+
+
+SimpleLogger = LogBundle

05_software/real/sim2real_v2/tools/math_utils.py

@@ -0,0 +1,111 @@
+"""数学工具 — 与 rc_mjlab/sim2sim/tools/math_utils.py 数值完全一致。"""
+from typing import Optional
+import numpy as np
+
+
+def get_gravity_orientation(quat_wxyz: np.ndarray) -> np.ndarray:
+    qw, qx, qy, qz = quat_wxyz
+    gx = 2.0 * (-qz * qx + qw * qy)
+    gy = -2.0 * (qz * qy + qw * qx)
+    gz = 1.0 - 2.0 * (qw * qw + qz * qz)
+    return np.array([gx, gy, gz], dtype=np.float32)
+
+
+def quat_rotate_inverse(quat_wxyz: np.ndarray, v: np.ndarray) -> np.ndarray:
+    q_w = quat_wxyz[0]
+    q_vec = quat_wxyz[1:]
+    a = v * (2.0 * q_w * q_w - 1.0)
+    b = np.cross(q_vec, v) * q_w * 2.0
+    c = q_vec * np.dot(q_vec, v) * 2.0
+    return a - b + c
+
+
+def quat_from_accel(accel: np.ndarray) -> np.ndarray:
+    """用静止重力方向初始化机身姿态四元数。
+
+    思想：仿真启动时 quat = [1,0,0,0] 隐含"机身完全水平"，但真机摆在地面上
+    pitch/roll 通常各自有几度偏差，会让 projected_gravity 一开始就错。
+    用加速度计读数与 [0,0,-1] 的最短旋转作为初值，可以把首步重力误差
+    降到 IMU 噪声级。
+    """
+    g_meas = accel / (np.linalg.norm(accel) + 1e-9)
+    g_ref = np.array([0.0, 0.0, 1.0], dtype=np.float32)
+    cross = np.cross(g_ref, g_meas)
+    dot = float(np.dot(g_ref, g_meas))
+    if dot < -0.999999:
+        return np.array([0.0, 1.0, 0.0, 0.0], dtype=np.float32)
+    s = float(np.sqrt((1.0 + dot) * 2.0))
+    q = np.array([s * 0.5, cross[0] / s, cross[1] / s, cross[2] / s], dtype=np.float32)
+    return q / (np.linalg.norm(q) + 1e-9)
+
+
+class LowPassFilter:
+    """一阶 IIR 低通，alpha 公式与训练侧 rc_mjlab/src/robot/mdp/lowpass_actions.py
+    `_lowpass_weights` 完全一致：
+
+        alpha = 1 - exp(-2π · cutoff_freq / control_freq)
+              = 1 - exp(-2π · cutoff_freq · dt)
+
+    注意：这与 rc_mjlab/sim2sim/interface/mujoco_io.py 用的近似公式
+    (dt / (dt + 1/(2π·fc))) 数值上不同，在 15Hz 截止时差约 30%。
+    我们以训练侧为准，因为策略是在那个滤波下学的。
+    """
+
+    def __init__(self, cutoff_freq: float, dt: float, dim: int):
+        self.alpha = float(1.0 - np.exp(-2.0 * np.pi * cutoff_freq * dt))
+        self.y_prev = None
+
+    def filter(self, x: np.ndarray) -> np.ndarray:
+        if self.y_prev is None:
+            self.y_prev = x.copy()
+        y = self.alpha * x + (1.0 - self.alpha) * self.y_prev
+        self.y_prev = y.copy()
+        return y
+
+    def reset(self):
+        self.y_prev = None
+
+
+class MahonyFilter:
+    """互补滤波器：高频用陀螺仪积分，低频用加速度计修正。"""
+
+    def __init__(self, kp: float = 2.0, ki: float = 0.0, dt: float = 0.02):
+        self.kp = kp
+        self.ki = ki
+        self.dt = dt
+        self.q = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32)
+        self.e_int = np.zeros(3, dtype=np.float32)
+
+    def reset_with_accel(self, accel: np.ndarray):
+        self.q = quat_from_accel(accel)
+        self.e_int.fill(0.0)
+
+    def update(self, accel: np.ndarray, gyro: np.ndarray, dt: Optional[float] = None) -> np.ndarray:
+        if dt is None:
+            dt = self.dt
+        norm_a = float(np.linalg.norm(accel))
+        if norm_a > 1e-6:
+            a = accel / norm_a
+            q = self.q
+            v = np.array([
+                2.0 * (q[1] * q[3] - q[0] * q[2]),
+                2.0 * (q[0] * q[1] + q[2] * q[3]),
+                q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3],
+            ], dtype=np.float32)
+            e = np.cross(a, v)
+            if self.ki > 0.0:
+                self.e_int += e * dt
+            else:
+                self.e_int.fill(0.0)
+            gyro = gyro + self.kp * e + self.ki * self.e_int
+
+        q = self.q
+        q_dot = 0.5 * np.array([
+            -q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2],
+             q[0] * gyro[0] + q[2] * gyro[2] - q[3] * gyro[1],
+             q[0] * gyro[1] - q[1] * gyro[2] + q[3] * gyro[0],
+             q[0] * gyro[2] + q[1] * gyro[1] - q[2] * gyro[0],
+        ], dtype=np.float32)
+        self.q += q_dot * dt
+        self.q /= (np.linalg.norm(self.q) + 1e-9)
+        return self.q

05_software/real/sim2real_v2/tools/remote_live_test.py

@@ -0,0 +1,61 @@
+from __future__ import annotations
+
+import argparse
+import sys
+import time
+from pathlib import Path
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+
+from input_dev.remote_uart import RemoteCommandSource  # noqa: E402
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--port", required=True)
+    parser.add_argument("--max-vx", type=float, default=0.8)
+    parser.add_argument("--max-vy", type=float, default=0.3)
+    parser.add_argument("--max-yaw", type=float, default=0.5)
+    parser.add_argument("--hz", type=float, default=20.0)
+    parser.add_argument("--baudrate", type=int, default=100000)
+    parser.add_argument("--timeout", type=float, default=0.02)
+    parser.add_argument("--deadzone", type=int, default=50)
+    args = parser.parse_args()
+
+    remote = RemoteCommandSource(
+        port=args.port,
+        baudrate=args.baudrate,
+        timeout=args.timeout,
+        axis_deadzone=args.deadzone,
+        active_threshold=args.deadzone,
+        max_vx=args.max_vx,
+        max_vy=args.max_vy,
+        max_yaw=args.max_yaw,
+    )
+    remote.open()
+    print(f"[remote-test] listening on {args.port}")
+    try:
+        period = 1.0 / max(args.hz, 1.0)
+        while True:
+            remote.poll()
+            status = remote.get_status()
+            print(
+                "cmd=({:+.3f}, {:+.3f}, {:+.3f}) active={} estop={} raw=({}, {}, {}, {})".format(
+                    status["cmd"][0],
+                    status["cmd"][1],
+                    status["cmd"][2],
+                    status["command_active"],
+                    status["estop_requested"],
+                    status["ch1"],
+                    status["ch2"],
+                    status["ch3"],
+                    status["ch4"],
+                )
+            )
+            time.sleep(period)
+    finally:
+        remote.close()
+
+
+if __name__ == "__main__":
+    main()

05_software/real/sim2real_v2/tools/standalone_check.py

@@ -0,0 +1,105 @@
+"""Check whether `sim2real/` is self-contained enough for direct deployment."""
+
+from __future__ import annotations
+
+import importlib
+from pathlib import Path
+import sys
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+
+
+REQUIRED_FILES = [
+    "config.yaml",
+    "deployment_manifest.yaml",
+    "main.py",
+    "policy/policy_runner.py",
+    "interface/real_io.py",
+    "interface/imu_client.py",
+    "interface/motor_driver.py",
+    "vendored/drivers/motor_driver.py",
+    "vendored/drivers/usb_can_adapter.py",
+    "vendored/odin1_imu/odin1_imu.py",
+    "vendored/odin1_imu/build/libodin1_imu_bridge.so",
+    "mjcf/wheelleg.xml",
+]
+
+REQUIRED_IMPORTS = [
+    "numpy",
+    "yaml",
+    "serial",
+]
+
+OPTIONAL_IMPORTS = [
+    ("onnxruntime", "required when deploying the default ONNX policy"),
+    ("torch", "required only for exporting/checking .pt policies"),
+    ("pynput", "only needed for CLI keyboard control"),
+]
+
+
+def check() -> int:
+    root = Path(__file__).resolve().parents[1]
+    issues: list[str] = []
+    warnings: list[str] = []
+
+    print(f"[Check] sim2real root: {root}")
+
+    for rel in REQUIRED_FILES:
+        path = root / rel
+        if path.exists():
+            print(f"[Check] file: PASS {rel}")
+        else:
+            issues.append(f"missing required file: {rel}")
+
+    if (root / "policies" / "model_rough.onnx").exists():
+        print("[Check] file: PASS policies/model_rough.onnx")
+    elif (root / "policies" / "model_rough.pt").exists():
+        warnings.append("policies/model_rough.onnx not found; runtime will fall back to .pt unless exported")
+        print("[Check] file: PASS policies/model_rough.pt fallback")
+    else:
+        issues.append("missing policy file: policies/model_rough.onnx or policies/model_rough.pt")
+
+    for module_name in REQUIRED_IMPORTS:
+        try:
+            importlib.import_module(module_name)
+            print(f"[Check] import: PASS {module_name}")
+        except Exception as exc:
+            issues.append(f"missing python dependency `{module_name}`: {exc}")
+
+    for module_name, note in OPTIONAL_IMPORTS:
+        try:
+            importlib.import_module(module_name)
+            print(f"[Check] optional import: PASS {module_name}")
+        except Exception:
+            warnings.append(f"optional dependency `{module_name}` not found ({note})")
+
+    index_html = (root / "web" / "static" / "index.html").read_text(encoding="utf-8")
+    if "https://unpkg.com/three@" in index_html:
+        warnings.append(
+            "web 3D viewer depends on remote three.js CDN; CLI/web backend are standalone, "
+            "but full offline 3D viewer is not bundled yet"
+        )
+
+    if issues:
+        print("\n" + "=" * 60)
+        print("Standalone deployment check: FAIL")
+        for item in issues:
+            print(f"- {item}")
+    else:
+        print("\n" + "=" * 60)
+        print("Standalone deployment check: PASS")
+
+    if warnings:
+        print("\nWarnings:")
+        for item in warnings:
+            print(f"- {item}")
+
+    return 1 if issues else 0
+
+
+def main():
+    raise SystemExit(check())
+
+
+if __name__ == "__main__":
+    main()

05_software/real/sim2real_v2/tools/test_angle_wrap.py

@@ -0,0 +1,45 @@
+"""Offline checks for RS02 multi-turn angle wrapping in startup/control."""
+
+from __future__ import annotations
+
+from pathlib import Path
+import sys
+
+import numpy as np
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+
+from interface.motor_mapping import MotorMapping  # noqa: E402
+from startup.pose_initializer import STAND_POSE, _periodic_leg_delta  # noqa: E402
+
+
+def main() -> int:
+    mapper = MotorMapping()
+
+    # Real log example: startup saw rl_knee sim angle 4.648rad while the stand
+    # target was -1.8rad. Those are close modulo 2*pi and must not plan a full turn.
+    raw_sim = STAND_POSE.copy()
+    raw_sim[8] = 4.648097991943359
+    raw_real = mapper.sim_to_real(raw_sim)
+    canonical = mapper.real_to_sim({(2, 3): raw_real[(2, 3)]})
+    delta = _periodic_leg_delta(canonical, STAND_POSE)
+    real_target = mapper.sim_to_real(STAND_POSE, current_real_pos={(2, 3): raw_real[(2, 3)]})[(2, 3)]
+    real_move = real_target - raw_real[(2, 3)]
+
+    print(f"[WrapCheck] canonical_sim_idx8={canonical[8]:.6f}")
+    print(f"[WrapCheck] startup_delta_idx8={delta[8]:.6f}")
+    print(f"[WrapCheck] real_move_idx8={real_move:.6f}")
+
+    if abs(float(delta[8])) > 0.25:
+        print("[WrapCheck] FAIL: periodic startup delta is too large")
+        return 1
+    if abs(float(real_move)) > 0.25:
+        print("[WrapCheck] FAIL: real target would command a long-path move")
+        return 1
+
+    print("[WrapCheck] PASS")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

05_software/real/sim2real_v2/tools/test_remote_mapping.py

@@ -0,0 +1,54 @@
+from __future__ import annotations
+
+import sys
+from pathlib import Path
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+
+from input_dev.remote_uart import (  # noqa: E402
+    RemoteCommandMapper,
+    RemoteControlState,
+    RemoteSwitchState,
+    SWITCH_HIGH,
+    SWITCH_MID,
+)
+
+
+def assert_close(actual: float, expected: float, tol: float = 1e-6) -> None:
+    if abs(actual - expected) > tol:
+        raise AssertionError(f"expected {expected}, got {actual}")
+
+
+def test_deadzone() -> None:
+    mapper = RemoteCommandMapper(max_vx=0.8, max_vy=0.3, max_yaw=0.5, active_threshold=50)
+    state = RemoteControlState(ch1=40, ch2=-49, ch4=50, switches=RemoteSwitchState(ch7=SWITCH_MID), frame_ok=True)
+    cmd = mapper.map_command(state)
+    assert_close(float(cmd[0]), 0.0)
+    assert_close(float(cmd[1]), 0.0)
+    assert_close(float(cmd[2]), 0.0)
+    if mapper.is_command_active(state):
+        raise AssertionError("deadzone values should not be active")
+
+
+def test_mapping() -> None:
+    mapper = RemoteCommandMapper(max_vx=0.8, max_vy=0.3, max_yaw=0.5, active_threshold=50)
+    state = RemoteControlState(ch1=330, ch2=-660, ch4=165, switches=RemoteSwitchState(ch7=SWITCH_MID), frame_ok=True)
+    cmd = mapper.map_command(state)
+    assert_close(float(cmd[0]), -0.8)
+    assert_close(float(cmd[1]), 0.075)
+    assert_close(float(cmd[2]), 0.25)
+    if not mapper.is_command_active(state):
+        raise AssertionError("mapped command should be active")
+
+
+def test_soft_estop_flag() -> None:
+    state = RemoteControlState(ch1=0, ch2=0, ch4=0, switches=RemoteSwitchState(ch7=SWITCH_HIGH), frame_ok=True)
+    if not state.estop_requested:
+        raise AssertionError("switch high should request estop")
+
+
+if __name__ == "__main__":
+    test_deadzone()
+    test_mapping()
+    test_soft_estop_flag()
+    print("remote command mapping tests passed")

05_software/real/sim2real_v2/vendored/drivers/__init__.py

@@ -0,0 +1,3 @@
+from drivers.motor_driver import RobStrideDriver, RobStrideMotor, MotorState
+from drivers.motor_params import CommunicationType, ParamIndex, RunMode
+from drivers.usb_can_adapter import DmUsbAdapter

05_software/real/sim2real_v2/vendored/drivers/motor_driver.py

@@ -0,0 +1,371 @@
+import struct
+import time
+import queue
+import numpy as np
+from typing import Dict, Optional, Any, List
+from dataclasses import dataclass
+
+from drivers.usb_can_adapter import DmUsbAdapter
+from drivers.motor_params import (
+    CommunicationType, ParamIndex, ParamType, 
+    MODEL_MIT_POSITION_TABLE, MODEL_MIT_VELOCITY_TABLE, 
+    MODEL_MIT_TORQUE_TABLE, MODEL_MIT_KP_TABLE, MODEL_MIT_KD_TABLE,
+    get_pack_format, PARAM_TABLE
+)
+
+@dataclass
+class MotorState:
+    position: float = 0.0
+    velocity: float = 0.0
+    torque: float = 0.0
+    temperature: float = 0.0
+    current: float = 0.0
+    update_count: int = 0
+
+class RobStrideMotor:
+    def __init__(self, name: str, motor_id: int, model: str):
+        """
+        初始化电机对象。
+        
+        :param name: 电机名称 (例如 "knee")
+        :param motor_id: 电机 ID
+        :param model: 电机型号 (例如 "rs-06")
+        """
+        self.name = name
+        self.id = motor_id
+        self.model = model
+        self.state = MotorState()
+        
+    def update_state(self, pos: float, vel: float, torque: float, temp: float, current: float = 0.0):
+        """
+        更新电机状态。
+        """
+        self.state.position = pos
+        self.state.velocity = vel
+        self.state.torque = torque
+        self.state.temperature = temp
+        self.state.update_count += 1
+        if current != 0.0:
+            self.state.current = current
+
+class RobStrideDriver:
+    def __init__(self, port: str, debug: bool = False):
+        """
+        初始化驱动器。
+        
+        :param port: 串口名称
+        :param debug: 是否开启调试模式
+        """
+        self.adapter = DmUsbAdapter(port, debug=debug)
+        self.motors: Dict[str, RobStrideMotor] = {}
+        self.motors_by_id: Dict[int, RobStrideMotor] = {}
+        self.host_id = 0xFD # 根据文档，主机 ID 默认为 0xFD
+        
+        self.parameter_values = {} # 读取参数缓存: (motor_id, param_index) -> value
+        
+    def connect(self):
+        """连接到底层适配器。"""
+        self.adapter.open()
+        print(f"已连接到 RobStride 驱动器，端口: {self.adapter.serial.port}")
+        # 设置 CAN 波特率为 1000kbps (Index 0)
+        self.adapter.set_can_baudrate(0)
+        
+    def disconnect(self):
+        """断开连接。"""
+        self.adapter.close()
+        print("已断开 RobStride 驱动器连接")
+        
+    def set_can_id(self, current_id: int, new_id: int):
+        """
+        设置电机 CAN ID。
+        
+        :param current_id: 当前电机 ID
+        :param new_id: 新电机 ID
+        """
+        # Type 7: Set CAN ID
+        # Bits 23-16: New ID (Preset ID)
+        # Bits 15-8: Master ID
+        # Bits 7-0: Target ID
+        extra_data = (new_id << 8) | self.host_id
+        self._send_command(CommunicationType.SET_CAN_ID, extra_data, current_id)
+        print(f"已发送 ID 修改指令: {current_id} -> {new_id} (Master: {self.host_id})")
+
+    def scan_motors(self, timeout: float = 0.1) -> List[int]:
+        """
+        快速扫描总线上的电机 (ID 1-127)。
+        
+        :param timeout: 等待响应的超时时间
+        :return: 发现的电机 ID 列表
+        """
+        found_ids = []
+        print("正在快速扫描所有电机 (ID 1-127)...")
+        
+        # 清空缓冲区
+        while self.adapter.read_can_frame():
+            pass
+            
+        # 快速发送查询指令
+        for dev_id in range(1, 128):
+            # 发送获取设备 ID 命令
+            self._send_command(CommunicationType.GET_DEVICE_ID, self.host_id, dev_id)
+            
+        # 等待响应
+        start_time = time.time()
+        while time.time() - start_time < timeout:
+            frame = self.adapter.read_can_frame()
+            if frame:
+                can_id, data, cmd, ide, rtr = frame
+                if not ide: continue
+                
+                # 解析回复
+                # 通信类型 0 (GET_DEVICE_ID/Status)
+                comm_type = (can_id >> 24) & 0x1F
+                
+                if comm_type == CommunicationType.GET_DEVICE_ID: # Type 0
+                    # Type 0 回复格式:
+                    # Bits 23-8: Status info
+                    # Bits 7-0: Motor ID
+                    extra_data = (can_id >> 8) & 0xFFFF
+                    motor_id = extra_data & 0xFF # Device ID
+                    
+                    if motor_id not in found_ids:
+                        print(f"发现电机 ID: {motor_id}")
+                        found_ids.append(motor_id)
+                        
+        return sorted(found_ids)
+
+        
+    def add_motor(self, name: str, motor_id: int, model: str):
+        """
+        添加电机到控制列表。
+        
+        :param name: 电机名称
+        :param motor_id: 电机 ID
+        :param model: 电机型号
+        """
+        motor = RobStrideMotor(name, motor_id, model)
+        self.motors[name] = motor
+        self.motors_by_id[motor_id] = motor
+        
+    def _send_command(self, comm_type: int, extra_data: int, device_id: int, data: bytes = b''):
+        # 构建 29 位扩展 CAN ID
+        # Bits 28-24: 通信类型 (Communication Type)
+        # Bits 23-8:  额外数据 (Extra Data)
+        # Bits 7-0:   设备 ID (Device ID)
+        can_id = (comm_type << 24) | (extra_data << 8) | device_id
+        
+        # 通过适配器发送
+        # RobStride 使用扩展帧
+        self.adapter.send_can_frame(can_id, data, extended=True)
+        
+    def enable(self, motor_name: str):
+        """使能电机。"""
+        motor = self.motors[motor_name]
+        self._send_command(CommunicationType.ENABLE, self.host_id, motor.id)
+        
+    def disable(self, motor_name: str):
+        """失能电机 (Type 4: Stop)。"""
+        motor = self.motors[motor_name]
+        # Data: 全 0
+        data = bytes([0x00]*8)
+        self._send_command(CommunicationType.DISABLE, self.host_id, motor.id, data)
+
+    def clear_warnings(self, motor_name: str):
+        """
+        清除警告/故障 (Type 4: Stop Motor with Byte0=1)。
+        根据文档 Type 4: Byte[0]=1 时清除故障。
+        """
+        motor = self.motors[motor_name]
+        data = bytes([0x01] + [0x00]*7)
+        self._send_command(CommunicationType.DISABLE, self.host_id, motor.id, data)
+        
+    def set_zero_position(self, motor_name: str):
+        """设置电机当前位置为零点。"""
+        motor = self.motors[motor_name]
+        # Type 6: Set Zero Position
+        # Data: Byte0=1
+        data = bytes([0x01] + [0x00]*7)
+        self._send_command(CommunicationType.SET_ZERO_POSITION, self.host_id, motor.id, data)
+
+    def control_mit(self, motor_name: str, 
+                    position: float, velocity: float, 
+                    kp: float, kd: float, torque: float):
+        """
+        发送 MIT 控制指令。
+        
+        :param motor_name: 电机名称
+        :param position: 期望位置 (rad)
+        :param velocity: 期望速度 (rad/s)
+        :param kp: 位置增益
+        :param kd: 速度增益
+        :param torque: 前馈力矩 (Nm)
+        """
+        motor = self.motors[motor_name]
+        model = motor.model
+        
+        # 获取限制值
+        p_limit = MODEL_MIT_POSITION_TABLE.get(model, 12.5)
+        v_limit = MODEL_MIT_VELOCITY_TABLE.get(model, 50.0)
+        t_limit = MODEL_MIT_TORQUE_TABLE.get(model, 60.0)
+        kp_limit = MODEL_MIT_KP_TABLE.get(model, 500.0)
+        kd_limit = MODEL_MIT_KD_TABLE.get(model, 5.0)
+        
+        # 限幅
+        position = np.clip(position, -p_limit, p_limit)
+        velocity = np.clip(velocity, -v_limit, v_limit)
+        kp = np.clip(kp, 0, kp_limit)
+        kd = np.clip(kd, 0, kd_limit)
+        torque = np.clip(torque, -t_limit, t_limit)
+        
+        # 转换为 uint16
+        # Position: [-L, L] -> [0, 65535]
+        p_u16 = int(((position / p_limit) + 1.0) * 32767.0)
+        p_u16 = np.clip(p_u16, 0, 65535)
+        
+        # Velocity: [-L, L] -> [0, 65535]
+        v_u16 = int(((velocity / v_limit) + 1.0) * 32767.0)
+        v_u16 = np.clip(v_u16, 0, 65535)
+        
+        # Kp: [0, L] -> [0, 65535]
+        kp_u16 = int((kp / kp_limit) * 65535.0)
+        kp_u16 = np.clip(kp_u16, 0, 65535)
+        
+        # Kd: [0, L] -> [0, 65535]
+        kd_u16 = int((kd / kd_limit) * 65535.0)
+        kd_u16 = np.clip(kd_u16, 0, 65535)
+        
+        # Torque: [-L, L] -> [0, 65535] (发送在 Extra Data 域)
+        t_u16 = int(((torque / t_limit) + 1.0) * 32767.0)
+        t_u16 = np.clip(t_u16, 0, 65535)
+        
+        # 打包数据 (大端序)
+        data = struct.pack('>HHHH', p_u16, v_u16, kp_u16, kd_u16)
+        
+        # 发送
+        self._send_command(CommunicationType.OPERATION_CONTROL, t_u16, motor.id, data)
+
+    def read_parameter(self, motor_id: int, param_index: int):
+        """
+        发送读取参数指令 (Type 17)。
+        """
+        # Type 17
+        # Data: Index (2B) + 00 00 + 00 00 00 00
+        data = struct.pack('<H', param_index) + b'\x00\x00\x00\x00\x00\x00'
+        self._send_command(CommunicationType.READ_PARAMETER, self.host_id, motor_id, data)
+        
+    def write_parameter(self, motor_id: int, param_index: int, value: Any):
+        """
+        发送写入参数指令 (Type 18)。
+        """
+        param_info = PARAM_TABLE.get(param_index)
+        if not param_info:
+            print(f"未知参数索引: {param_index}")
+            return
+            
+        # motor_params.py format: (name, p_type, size)
+        name, p_type, size = param_info
+        
+        fmt, _ = get_pack_format(p_type)
+        if not fmt:
+            print(f"不支持的参数类型: {p_type}")
+            return
+
+        # 注意：不再进行范围检查，因为 motor_params.py 中没有定义范围
+        
+        # 打包数据
+        val_bytes = struct.pack(fmt, value)
+        # 填充 val_bytes 到 4 字节
+        if len(val_bytes) < 4:
+            val_bytes += b'\x00' * (4 - len(val_bytes))
+            
+        # Index (2B) + 00 00 + Value (4B)
+        data = struct.pack('<H', param_index) + b'\x00\x00' + val_bytes
+            
+        self._send_command(CommunicationType.WRITE_PARAMETER, self.host_id, motor_id, data)
+
+    def save_parameters(self, motor_id: int):
+        """
+        保存参数到 EEPROM (Type 22)。
+        """
+        data = bytes([0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08])
+        self._send_command(CommunicationType.SAVE_PARAMETERS, self.host_id, motor_id, data)
+
+    def process_messages(self, max_messages=50):
+        """
+        从 CAN 总线读取消息并更新电机状态。
+        """
+        count = 0
+        while count < max_messages:
+            frame = self.adapter.read_can_frame()
+            if not frame:
+                break
+                
+            can_id, data, cmd, ide, rtr = frame
+            
+            if not ide: 
+                continue # 跳过标准帧
+                
+            # 解析扩展 ID
+            comm_type = (can_id >> 24) & 0x1F
+            
+            if comm_type == CommunicationType.READ_PARAMETER:
+                # 解析参数读取反馈 (Type 17)
+                extra_data = (can_id >> 8) & 0xFFFF
+                success_flag = (extra_data >> 8) & 0xFF
+                motor_id = extra_data & 0xFF
+                
+                if success_flag == 0: # 0 表示成功
+                    if len(data) >= 8:
+                        param_index = struct.unpack('<H', data[0:2])[0]
+                        raw_value = data[4:8]
+                        
+                        param_info = PARAM_TABLE.get(param_index)
+                        if param_info:
+                            name, p_type, size = param_info
+                            fmt, _ = get_pack_format(p_type)
+                            if fmt:
+                                try:
+                                    # 根据类型大小解包
+                                    val_size = struct.calcsize(fmt)
+                                    val = struct.unpack(fmt, raw_value[:val_size])[0]
+                                    self.parameter_values[(motor_id, param_index)] = val
+                                    
+                                    # 如果是 IQF (电流)，更新电机状态
+                                    if param_index == ParamIndex.IQF:
+                                        if motor_id in self.motors_by_id:
+                                            self.motors_by_id[motor_id].state.current = val
+                                            
+                                except Exception as e:
+                                    print(f"解析参数失败: {e}")
+                else:
+                    print(f"读取参数失败，错误码: {success_flag}")
+
+            elif comm_type == CommunicationType.OPERATION_STATUS:
+                # 处理电机反馈
+                extra_data = (can_id >> 8) & 0xFFFF
+                motor_id = extra_data & 0xFF
+                if motor_id in self.motors_by_id:
+                    motor = self.motors_by_id[motor_id]
+                    self._parse_feedback(motor, data)
+            
+            count += 1
+
+    def _parse_feedback(self, motor: RobStrideMotor, data: bytes):
+        if len(data) < 8:
+            return
+            
+        # 解包大端序数据
+        p_u16, v_u16, t_i16, temp_u16 = struct.unpack('>HHHH', data)
+        
+        model = motor.model
+        p_limit = MODEL_MIT_POSITION_TABLE.get(model, 12.5)
+        v_limit = MODEL_MIT_VELOCITY_TABLE.get(model, 50.0)
+        t_limit = MODEL_MIT_TORQUE_TABLE.get(model, 60.0)
+        
+        # 转换回浮点数
+        pos = (float(p_u16) / 32767.0 - 1.0) * p_limit
+        vel = (float(v_u16) / 32767.0 - 1.0) * v_limit
+        torque = (float(t_i16) / 32767.0 - 1.0) * t_limit
+        temp = float(temp_u16) * 0.1
+        
+        motor.update_state(pos, vel, torque, temp)

05_software/real/sim2real_v2/vendored/drivers/motor_params.py

@@ -0,0 +1,422 @@
+import numpy as np
+import struct
+
+class CommunicationType:
+    """
+    电机通信类型定义 (Bit28~24)
+    参考说明书 4.1 章节
+    
+    通信 ID 结构 (29位扩展帧):
+    | Bit 28-24 | Bit 23-8 | Bit 7-0 |
+    | 通信类型   | 数据区2  | 目标地址 |
+    """
+    GET_DEVICE_ID       = 0   # 获取设备 ID 和 64 位 MCU 唯一标识符 (Type 0)
+    OPERATION_CONTROL   = 1   # 运控模式电机控制指令 (MIT 模式) (Type 1)
+    OPERATION_STATUS    = 2   # 电机反馈数据 (标准反馈帧) (Type 2)
+    ENABLE              = 3   # 电机使能运行 (Type 3)
+    DISABLE             = 4   # 电机停止运行 (可用于清除故障) (Type 4)
+    SET_ZERO_POSITION   = 6   # 设置电机机械零位 (设置当前位置为零点) (Type 6)
+    SET_CAN_ID          = 7   # 设置电机 CAN ID (立即生效，需保存) (Type 7)
+    READ_PARAMETER      = 17  # 单个参数读取 (Type 17, 0x11)
+    WRITE_PARAMETER     = 18  # 单个参数写入 (Type 18, 0x12, 掉电丢失)
+    FAULT_REPORT        = 21  # 故障反馈帧 (Type 21, 0x15)
+    SAVE_PARAMETERS     = 22  # 电机数据保存帧 (保存所有参数到 Flash) (Type 22)
+    SET_BAUDRATE        = 23  # 电机波特率修改帧 (重新上电生效) (Type 23)
+    ACTIVE_REPORT       = 24  # 电机主动上报设置帧 (开启/关闭主动上报) (Type 24)
+    PROTOCOL_SWITCH     = 25  # 电机协议修改帧 (切换 Canopen/MIT/私有协议) (Type 25)
+    READ_VERSION        = 26  # 版本号读取帧 (Type 26)
+
+class RunMode:
+    """
+    电机运行模式 (参数索引 0x7005)
+    参考说明书 4.3 章节
+    """
+    MIT                 = 0   # 运控模式 (默认): 适用于高动态响应控制
+    POS_PP              = 1   # 位置模式 (PP): 梯形加减速位置控制
+    SPEED               = 2   # 速度模式: 闭环速度控制
+    CURRENT             = 3   # 电流模式: 闭环力矩(电流)控制
+    POS_CSP             = 5   # 位置模式 (CSP): 循环同步位置模式 (适用于周期性指令)
+
+class BaudRate:
+    """
+    电机波特率 (通信类型 23)
+    参考说明书 4.1 通信类型 23
+    注意: 修改后需重新上电生效
+    """
+    BAUD_1M             = 1   # 1 Mbps (默认)
+    BAUD_500K           = 2   # 500 Kbps
+    BAUD_250K           = 3   # 250 Kbps
+    BAUD_125K           = 4   # 125 Kbps
+
+class ActiveReportStatus:
+    """
+    电机主动上报状态 (通信类型 24)
+    参考说明书 4.1 通信类型 24
+    """
+    DISABLE             = 0   # 关闭主动上报 (默认)
+    ENABLE              = 1   # 开启主动上报 (默认间隔 10ms, 可通过 EP_SCAN_TIME 修改)
+
+class ProtocolType:
+    """
+    电机协议类型 (通信类型 25)
+    参考说明书 4.2.4 章节
+    注意: 切换协议后需重新上电生效
+    """
+    PRIVATE             = 0   # 私有协议 (默认): 使用 29 位扩展帧
+    CANOPEN             = 1   # CANopen 协议: 符合 CiA 402 标准
+    MIT                 = 2   # MIT 协议 (标准帧): 使用 11 位标准帧
+
+class ParamType:
+    """
+    参数数据类型定义
+    
+    - 私有协议 (Type 17/18) 参数表主要使用 UINT8/UINT16/UINT32/FLOAT
+    - CANopen 对象字典会用到有符号类型 (INTEGER8/16/32)
+    """
+    UINT8               = 0   # 无符号 8 位整数
+    UINT16              = 1   # 无符号 16 位整数
+    UINT32              = 2   # 无符号 32 位整数
+    FLOAT               = 3   # 32 位浮点数 (IEEE 754)
+    INT8                = 4   # 有符号 8 位整数
+    INT16               = 5   # 有符号 16 位整数
+    INT32               = 6   # 有符号 32 位整数
+
+class ErrorCode:
+    """
+    异常状态 fault 值位定义
+    
+    说明书位置:
+    - 章节 6 (Mit) 的“异常状态应答帧”对 fault 值 bit 位做了明确描述
+    - 私有协议 Type 21 故障反馈帧也会携带 fault/warning 值
+    """
+    OVER_TEMP           = 1 << 0  # bit0: 电机过温故障 (默认 >145°C)
+    DRIVE_CHIP          = 1 << 1  # bit1: 驱动芯片故障 (DRV8353 等报告错误)
+    UNDER_VOLTAGE       = 1 << 2  # bit2: 欠压故障 (电压 < 12V)
+    OVER_VOLTAGE        = 1 << 3  # bit3: 过压故障 (电压 > 60V)
+    CURRENT_B_OVER      = 1 << 4  # bit4: B 相电流采样过流
+    CURRENT_C_OVER      = 1 << 5  # bit5: C 相电流采样过流
+    ENCODER_NOT_CALIB   = 1 << 7  # bit7: 编码器未标定
+    HARDWARE_ERR        = 1 << 8  # bit8: 硬件识别故障
+    POS_INIT_ERR        = 1 << 9  # bit9: 位置初始化故障
+    LOAD_BLOCK          = 1 << 14 # bit14: 堵转过载算法保护
+    CURRENT_A_OVER      = 1 << 16 # bit16: A 相电流采样过流
+
+class WarningCode:
+    """
+    预警状态 warning 值位定义 (Type 21 Byte 4-7)
+    """
+    OVER_TEMP_WARNING   = 1 << 0  # bit0: 电机过温预警 (默认 >135°C)
+
+class DriveFault1:
+    """
+    驱动芯片故障码 1 (0x3024) - DRV8353 状态寄存器 1
+    参考说明书 3.3.7 章节
+    """
+    VDS_LC              = 1 << 0  # VDS overcurrent on C low-side (C相下管VDS过流)
+    VDS_HC              = 1 << 1  # VDS overcurrent on C high-side (C相上管VDS过流)
+    VDS_LB              = 1 << 2  # VDS overcurrent on B low-side (B相下管VDS过流)
+    VDS_HB              = 1 << 3  # VDS overcurrent on B high-side (B相上管VDS过流)
+    VDS_LA              = 1 << 4  # VDS overcurrent on A low-side (A相下管VDS过流)
+    VDS_HA              = 1 << 5  # VDS overcurrent on A high-side (A相上管VDS过流)
+    OTSD                = 1 << 6  # Overtemperature shutdown (过温关断)
+    UVLO                = 1 << 7  # Undervoltage lockout (欠压锁定)
+    GDF                 = 1 << 8  # Gate drive fault (栅极驱动故障)
+    VDS_OCP             = 1 << 9  # VDS monitor overcurrent (VDS 监控过流)
+    FAULT               = 1 << 10 # Logic OR of FAULT status (故障状态逻辑或)
+
+class DriveFault2:
+    """
+    驱动芯片故障码 2 (0x3025) - DRV8353 状态寄存器 2
+    参考说明书 3.3.7 章节
+    """
+    VGS_LC              = 1 << 0  # Gate drive fault on C low-side (C相下管栅极故障)
+    VGS_HC              = 1 << 1  # Gate drive fault on C high-side (C相上管栅极故障)
+    VGS_LB              = 1 << 2  # Gate drive fault on B low-side (B相下管栅极故障)
+    VGS_HB              = 1 << 3  # Gate drive fault on B high-side (B相上管栅极故障)
+    VGS_LA              = 1 << 4  # Gate drive fault on A low-side (A相下管栅极故障)
+    VGS_HA              = 1 << 5  # Gate drive fault on A high-side (A相上管栅极故障)
+    GDUV                = 1 << 6  # VCP charge pump / VGLS undervoltage (电荷泵欠压)
+    OTW                 = 1 << 7  # Overtemperature warning (过温预警)
+    SC_OC               = 1 << 8  # Overcurrent on phase C sense amplifier (C相采样过流)
+    SB_OC               = 1 << 9  # Overcurrent on phase B sense amplifier (B相采样过流)
+    SA_OC               = 1 << 10 # Overcurrent on phase A sense amplifier (A相采样过流)
+
+class MotorParams:
+    """
+    电机物理参数限制 (用于 MIT 模式数据压缩)
+    参考说明书 4.1 通信类型 1
+    
+    注意:
+    - P_MIN/MAX: 位置范围 (RS03: -12.57 ~ 12.57 rad)
+    - V_MIN/MAX: 速度范围 (RS03: -20 ~ 20 rad/s)
+    - T_MIN/MAX: 力矩范围 (RS03: -60 ~ 60 Nm)
+    - KP/KD: 刚度和阻尼系数范围
+    """
+    def __init__(self,
+                 p_min: float = -12.57,
+                 p_max: float = 12.57,  # RS03: -12.57 ~ 12.57 rad (约 -4pi ~ 4pi)
+                 v_min: float = -20.0,
+                 v_max: float = 20.0,   # RS03: -20 ~ 20 rad/s
+                 kp_min: float = 0.0,
+                 kp_max: float = 5000.0, # RS03: 0 ~ 5000
+                 kd_min: float = 0.0,
+                 kd_max: float = 100.0,  # RS03: 0 ~ 100
+                 t_min: float = -60.0,
+                 t_max: float = 60.0):  # RS03: -60 ~ 60 Nm
+        self.P_MIN = p_min
+        self.P_MAX = p_max
+        self.V_MIN = v_min
+        self.V_MAX = v_max
+        self.KP_MIN = kp_min
+        self.KP_MAX = kp_max
+        self.KD_MIN = kd_min
+        self.KD_MAX = kd_max
+        self.T_MIN = t_min
+        self.T_MAX = t_max
+
+class ParamIndex:
+    """
+    电机参数索引表 (Index)
+    参考说明书 4.1 可读写单个参数列表
+    """
+    RUN_MODE            = 0x7005  # 运行模式: 0:运控, 1:PP, 2:速度, 3:电流, 5:CSP (W/R)
+    IQ_REF              = 0x7006  # 电流模式 Iq 指令 (-43~43A) (W/R)
+    SPD_REF             = 0x700A  # 转速模式转速指令 (-20~20rad/s) (W/R)
+    LIMIT_TORQUE        = 0x700B  # 转矩限制 (0~60Nm) (W/R)
+    CUR_KP              = 0x7010  # 电流 Kp (默认 0.17) (W/R)
+    CUR_KI              = 0x7011  # 电流 Ki (默认 0.012) (W/R)
+    CUR_FILT_GAIN       = 0x7014  # 电流滤波系数 (0~1.0, 默认 0.1) (W/R)
+    LOC_REF             = 0x7016  # 位置模式角度指令 (rad) (W/R)
+    LIMIT_SPD           = 0x7017  # 位置模式(CSP)速度限制 (0~20rad/s) (W/R)
+    LIMIT_CUR           = 0x7018  # 速度/位置模式电流限制 (0~43A) (W/R)
+    MECH_POS            = 0x7019  # 负载端计圈机械角度 (rad) (Read Only)
+    IQF                 = 0x701A  # Iq 滤波值 (A) (Read Only)
+    MECH_VEL            = 0x701B  # 负载端转速 (rad/s) (Read Only)
+    VBUS                = 0x701C  # 母线电压 (V) (Read Only)
+    LOC_KP              = 0x701E  # 位置环 Kp (默认 60) (W/R)
+    SPD_KP              = 0x701F  # 速度环 Kp (默认 6) (W/R)
+    SPD_KI              = 0x7020  # 速度环 Ki (默认 0.02) (W/R)
+    SPD_FILT_GAIN       = 0x7021  # 速度滤波值 (默认 0.1) (W/R)
+    ACC_RAD             = 0x7022  # 速度模式加速度 (默认 20rad/s^2) (W/R)
+    VEL_MAX             = 0x7024  # 位置模式(PP)速度 (默认 10rad/s) (W/R)
+    ACC_SET             = 0x7025  # 位置模式(PP)加速度 (默认 10rad/s^2) (W/R)
+    EP_SCAN_TIME        = 0x7026  # 主动上报时间 (1=10ms, +1=+5ms) (W)
+    CAN_TIMEOUT         = 0x7028  # CAN 超时阈值 (20000=1s, 0=禁用) (W)
+    ZERO_STA            = 0x7029  # 零点标志位 (0: 0~2pi, 1: -pi~pi) (W)
+    DAMPER              = 0x702A  # 阻尼开关 (1: 取消关机反驱保护) (W/R)
+    ADD_OFFSET          = 0x702B  # 零位偏置 (rad) (W/R)
+
+class CanopenIndex:
+    """
+    CANopen 对象字典常用索引
+    参考说明书第 5 章 (Canopen)
+    """
+    ERROR_CODE                   = 0x603F  # 错误码
+    CONTROLWORD                  = 0x6040  # 控制字
+    STATUSWORD                   = 0x6041  # 状态字
+    MODES_OF_OPERATION           = 0x6060  # 运行模式
+    MODES_OF_OPERATION_DISPLAY   = 0x6061  # 当前运行模式显示
+    POSITION_DEMAND_VALUE        = 0x6062  # 位置指令值
+    POSITION_ACTUAL_VALUE        = 0x6064  # 位置实际值
+    POSITION_WINDOW              = 0x6067  # 位置窗口
+    POSITION_WINDOW_TIME         = 0x6068  # 位置窗口时间
+    VELOCITY_DEMAND_VALUE        = 0x606B  # 速度指令值
+    VELOCITY_ACTUAL_VALUE        = 0x606C  # 速度实际值
+    TARGET_TORQUE                = 0x6071  # 目标力矩 (0.1% 额定力矩)
+    TORQUE_ACTUAL_VALUE          = 0x6077  # 力矩实际值
+    CURRENT_ACTUAL_VALUE         = 0x6078  # 电流实际值
+    DC_LINK_CIRCUIT_VOLTAGE      = 0x6079  # 母线电压
+    TARGET_POSITION              = 0x607A  # 目标位置
+    PROFILE_VELOCITY             = 0x6081  # 轮廓速度
+    PROFILE_ACCELERATION         = 0x6083  # 轮廓加速度
+    TARGET_VELOCITY              = 0x60FF  # 目标速度
+
+class CanopenModeOfOperation:
+    """CANopen 模式 (6060)"""
+    PP       = 1  # Profile Position Mode
+    SPEED    = 3  # Profile Velocity Mode
+    TORQUE   = 4  # Profile Torque Mode
+    CSP      = 5  # Cyclic Synchronous Position Mode
+    HOMING   = 6  # Homing Mode
+
+class CanopenControlword:
+    """CANopen 控制字 (6040) 常用值"""
+    SHUTDOWN           = 0x0006  # Shutdown
+    SWITCH_ON          = 0x0007  # Switch On
+    ENABLE_OPERATION   = 0x000F  # Enable Operation
+    DISABLE_VOLTAGE    = 0x0001  # Disable Voltage
+    QUICK_STOP         = 0x000B  # Quick Stop
+
+# CANopen 协议切换帧 (扩展帧)
+# 说明书 5.10: 29 位 ID 为 0xFFF，数据区 Byte0~6 固定 01~06，Byte7=F_CMD(协议类型)
+CANOPEN_PROTOCOL_SWITCH_EXT_ID = 0xFFF
+
+class MitStdCommandType:
+    """
+    MIT 标准帧指令类型 (对应说明书第 6 章的指令 1~11)
+    
+    标准帧 ID (11位) 结构:
+    | Bit 10-8 | Bit 7-0 |
+    | 模式/指令 | 电机 ID |
+    
+    注意:
+    - 指令 1~9: CAN ID 的 Bit10~8 为 0，通过数据区 Payload 区分功能
+    - 指令 10:  CAN ID 的 Bit10~8 为 1 (位置模式)
+    - 指令 11:  CAN ID 的 Bit10~8 为 2 (速度模式)
+    """
+    ENABLE                      = 1   # 指令 1: 电机使能运行
+    STOP                        = 2   # 指令 2: 电机停止运行
+    DYNAMIC_PARAM               = 3   # 指令 3: MIT 动态参数
+    SET_ZERO                    = 4   # 指令 4: 设置零点 (非位置模式)
+    CLEAR_ERROR_OR_READ_STATUS  = 5   # 指令 5: 清错 / 读取异常状态
+    SET_RUN_MODE                = 6   # 指令 6: 设置运行模式
+    SET_MOTOR_CAN_ID            = 7   # 指令 7: 修改电机 CANID
+    SET_PROTOCOL                = 8   # 指令 8: 修改电机协议 (重新上电生效)
+    SET_MASTER_CAN_ID           = 9   # 指令 9: 修改主机 CANID
+    POS_CONTROL                 = 10  # 指令 10: 位置模式控制指令 (ID Bit10-8=1)
+    SPEED_CONTROL               = 11  # 指令 11: 速度模式控制指令 (ID Bit10-8=2)
+
+def get_mit_can_id_mode(cmd_type: int) -> int:
+    """
+    获取 MIT 标准帧 CAN ID 的 Bit10~8 值
+    
+    :param cmd_type: MitStdCommandType 枚举值
+    :return: 模式位 (0, 1, 或 2)
+    """
+    if cmd_type in (MitStdCommandType.POS_CONTROL,):
+        return 1
+    elif cmd_type in (MitStdCommandType.SPEED_CONTROL,):
+        return 2
+    else:
+        # 指令 1~9 (以及其他潜在指令) 默认为 0
+        return 0
+
+def build_mit_std_id(cmd_type: int, motor_id: int) -> int:
+    """
+    构建 MIT 标准帧 11 位 CAN ID
+    
+    :param cmd_type: MitStdCommandType 枚举值
+    :param motor_id: 电机 ID (0~127)
+    :return: 11 位 CAN ID
+    """
+    mode = get_mit_can_id_mode(cmd_type)
+    return ((mode & 0x07) << 8) | (motor_id & 0xFF)
+
+class MitPayloads:
+    """
+    MIT 协议特殊指令的固定 Payload 定义 (指令 1, 2, 4, 5, 6, 7, 8, 9)
+    部分指令的 Payload 末尾字节需要根据参数动态修改
+    """
+    # 指令 1: FF FF FF FF FF FF FF FC
+    ENABLE = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFC'
+    
+    # 指令 2: FF FF FF FF FF FF FF FD
+    STOP = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFD'
+    
+    # 指令 3: 动态参数 (全 0 或根据参数设置)
+    DYNAMIC_PARAM_ZERO = b'\x00\x00\x00\x00\x00\x00\x00\x00'
+    
+    # 指令 4: FF FF FF FF FF FF FF FE
+    SET_ZERO = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE'
+    
+    # 指令 5: FF FF FF FF FF FF FF FB (清除错误)
+    # 若 F_CMD (Byte6) 为 0xFF 则清除错误，否则为读取异常状态
+    CLEAR_ERROR = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFB'
+    
+    # 指令 6: FF FF FF FF FF FF [Mode] FC
+    # Template, last 2 bytes are [Mode, FC]
+    SET_RUN_MODE_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF'
+    
+    # 指令 7: FF FF FF FF FF FF [NewID] FA
+    SET_MOTOR_CAN_ID_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF'
+    
+    # 指令 8: FF FF FF FF FF FF [Protocol] FD
+    SET_PROTOCOL_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF'
+    
+    # 指令 9: FF FF FF FF FF FF [MasterID] 01
+    SET_MASTER_CAN_ID_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF'
+
+
+# 参数表配置: (参数名, 数据类型, 字节数)
+PARAM_TABLE = {
+    ParamIndex.RUN_MODE:      ("run_mode", ParamType.UINT8, 1),
+    ParamIndex.IQ_REF:        ("iq_ref", ParamType.FLOAT, 4),
+    ParamIndex.SPD_REF:       ("spd_ref", ParamType.FLOAT, 4),
+    ParamIndex.LIMIT_TORQUE:  ("limit_torque", ParamType.FLOAT, 4),
+    ParamIndex.CUR_KP:        ("cur_kp", ParamType.FLOAT, 4),
+    ParamIndex.CUR_KI:        ("cur_ki", ParamType.FLOAT, 4),
+    ParamIndex.CUR_FILT_GAIN: ("cur_filt_gain", ParamType.FLOAT, 4),
+    ParamIndex.LOC_REF:       ("loc_ref", ParamType.FLOAT, 4),
+    ParamIndex.LIMIT_SPD:     ("limit_spd", ParamType.FLOAT, 4),
+    ParamIndex.LIMIT_CUR:     ("limit_cur", ParamType.FLOAT, 4),
+    ParamIndex.MECH_POS:      ("mechPos", ParamType.FLOAT, 4),
+    ParamIndex.IQF:           ("iqf", ParamType.FLOAT, 4),
+    ParamIndex.MECH_VEL:      ("mechVel", ParamType.FLOAT, 4),
+    ParamIndex.VBUS:          ("VBUS", ParamType.FLOAT, 4),
+    ParamIndex.LOC_KP:        ("loc_kp", ParamType.FLOAT, 4),
+    ParamIndex.SPD_KP:        ("spd_kp", ParamType.FLOAT, 4),
+    ParamIndex.SPD_KI:        ("spd_ki", ParamType.FLOAT, 4),
+    ParamIndex.SPD_FILT_GAIN: ("spd_filt_gain", ParamType.FLOAT, 4),
+    ParamIndex.ACC_RAD:       ("acc_rad", ParamType.FLOAT, 4),
+    ParamIndex.VEL_MAX:       ("vel_max", ParamType.FLOAT, 4),
+    ParamIndex.ACC_SET:       ("acc_set", ParamType.FLOAT, 4),
+    ParamIndex.EP_SCAN_TIME:  ("EPScan_time", ParamType.UINT16, 2),
+    ParamIndex.CAN_TIMEOUT:   ("cantimeout", ParamType.UINT32, 4),
+    ParamIndex.ZERO_STA:      ("zero_sta", ParamType.UINT8, 1),
+    ParamIndex.DAMPER:        ("damper", ParamType.UINT8, 1),
+    ParamIndex.ADD_OFFSET:    ("add_offset", ParamType.FLOAT, 4),
+}
+
+MODEL_MIT_POSITION_TABLE = {
+    "rs-00": 4 * np.pi, "rs-01": 4 * np.pi, "rs-02": 4 * np.pi,
+    "rs-03": 4 * np.pi, "rs-04": 4 * np.pi, "rs-05": 4 * np.pi, "rs-06": 4 * np.pi,
+    "el-05": 4 * np.pi,
+}
+
+MODEL_MIT_VELOCITY_TABLE = {
+    "rs-00": 50, "rs-01": 44, "rs-02": 44,
+    "rs-03": 50, "rs-04": 15, "rs-05": 33, "rs-06": 20,
+    "el-05": 50,
+}
+
+MODEL_MIT_TORQUE_TABLE = {
+    "rs-00": 17, "rs-01": 17, "rs-02": 17,
+    "rs-03": 60, "rs-04": 120, "rs-05": 17, "rs-06": 60,
+    "el-05": 6,
+}
+
+MODEL_MIT_KP_TABLE = {
+    "rs-00": 500.0, "rs-01": 500.0, "rs-02": 500.0,
+    "rs-03": 5000.0, "rs-04": 5000.0, "rs-05": 500.0, "rs-06": 5000.0,
+    "el-05": 500.0,
+}
+
+MODEL_MIT_KD_TABLE = {
+    "rs-00": 5.0, "rs-01": 5.0, "rs-02": 5.0,
+    "rs-03": 100.0, "rs-04": 100.0, "rs-05": 5.0, "rs-06": 100.0,
+    "el-05": 5.0,
+}
+
+def get_pack_format(param_type):
+    """
+    获取 struct.pack 的格式字符串和字节大小
+
+    说明:
+    - Type 17/18 参数读写使用小端序
+    - CANopen SDO 数据同样通常按小端序解释 (取决于实现)
+    """
+    if param_type == ParamType.UINT8:
+        return '<B', 1
+    elif param_type == ParamType.UINT16:
+        return '<H', 2
+    elif param_type == ParamType.UINT32:
+        return '<I', 4
+    elif param_type == ParamType.INT8:
+        return '<b', 1
+    elif param_type == ParamType.INT16:
+        return '<h', 2
+    elif param_type == ParamType.INT32:
+        return '<i', 4
+    elif param_type == ParamType.FLOAT:
+        return '<f', 4
+    return None, 0

05_software/real/sim2real_v2/vendored/drivers/usb_can_adapter.py

@@ -0,0 +1,185 @@
+import serial
+import struct
+import time
+from typing import Optional, Tuple
+
+class DmUsbAdapter:
+    """
+    达妙 USB 转 CAN 适配器驱动。
+    处理底层串口通信和帧的封装/解包。
+    """
+    
+    # 帧常量
+    SEND_HEADER = b'\x55\xAA'
+    SEND_FRAME_LEN = 30
+    RECV_HEADER = 0xAA
+    RECV_TAIL = 0x55
+    RECV_FRAME_LEN = 16
+    
+    def __init__(self, port: str, baudrate: int = 921600, timeout: float = 0.01, debug: bool = False):
+        """
+        初始化 USB 转 CAN 适配器。
+        
+        :param port: 串口名称 (例如 "COM3")
+        :param baudrate: 串口波特率 (默认 921600)
+        :param timeout: 读取超时时间 (秒)
+        :param debug: 是否打印调试信息
+        """
+        self.serial = serial.Serial()
+        self.serial.port = port
+        self.serial.baudrate = baudrate
+        self.serial.timeout = timeout
+        self.data_buffer = bytearray()
+        self.debug = debug
+
+    def open(self):
+        """打开串口连接。"""
+        if not self.serial.is_open:
+            try:
+                self.serial.open()
+                if self.debug:
+                    print(f"[DEBUG] 串口 {self.serial.port} 已打开")
+            except Exception as e:
+                print(f"[ERROR] 无法打开串口 {self.serial.port}: {e}")
+                raise
+
+    def close(self):
+        """关闭串口连接。"""
+        if self.serial.is_open:
+            self.serial.close()
+            if self.debug:
+                print(f"[DEBUG] 串口 {self.serial.port} 已关闭")
+
+    def set_can_baudrate(self, index: int = 0):
+        """
+        设置 CAN 波特率。
+        
+        索引对照表:
+        0: 1000 kbps
+        1: 800 kbps
+        2: 666 kbps
+        3: 500 kbps
+        ...
+        63: 
+        
+        :param index: 波特率索引 (默认 0, 即 1000kbps)
+        """
+        # 构建设置波特率指令: 55 05 Index(1byte) AA 55
+        cmd = bytearray([0x55, 0x05, index & 0xFF, 0xAA, 0x55])
+        self.serial.write(cmd)
+        if self.debug:
+            print(f"[DEBUG] 发送设置波特率指令: {cmd.hex()}")
+        time.sleep(0.1) # 等待生效
+
+    def send_can_frame(self, can_id: int, data: bytes, 
+                       extended: bool = True, remote: bool = False, 
+                       feedback: bool = False) -> None:
+        """
+        发送 CAN 帧。
+        
+        :param can_id: CAN 标识符 (标准帧或扩展帧)
+        :param data: 数据负载 (最多 8 字节)
+        :param extended: True 为扩展帧 (29位), False 为标准帧 (11位)
+        :param remote: True 为远程帧, False 为数据帧
+        :param feedback: True 请求设备反馈 (CMD 0x01), False 不反馈 (CMD 0x03)
+        """
+        if len(data) > 8:
+            raise ValueError("CAN 数据不能超过 8 字节")
+            
+        # 填充数据到 8 字节
+        data_padded = data + b'\x00' * (8 - len(data))
+        
+        cmd = 0x01 if feedback else 0x03
+        send_count = 1
+        interval = 10 # 默认 10ms
+        id_type = 1 if extended else 0
+        frame_type = 1 if remote else 0
+        data_len = len(data)
+        
+        # 构建帧 (30 字节)
+        frame = bytearray(30)
+        frame[0] = 0x55
+        frame[1] = 0xAA
+        frame[2] = 0x1E # 长度
+        frame[3] = cmd
+        
+        # 发送次数 (4 字节, 小端序)
+        frame[4:8] = struct.pack('<I', send_count)
+        
+        # 时间间隔 (4 字节, 小端序)
+        frame[8:12] = struct.pack('<I', interval)
+        
+        frame[12] = id_type
+        
+        # CAN ID (4 字节, 小端序)
+        frame[13:17] = struct.pack('<I', can_id)
+        
+        frame[17] = frame_type
+        frame[18] = data_len
+        # 19, 20 为保留位 0
+        
+        frame[21:29] = data_padded
+        frame[29] = 0x00 # CRC (任意值)
+        
+        self.serial.write(frame)
+        
+        if self.debug:
+            print(f"[DEBUG] 发送帧: ID=0x{can_id:08X} Data={data.hex()} Raw={frame.hex()}")
+
+    def read_can_frame(self) -> Optional[Tuple[int, bytes, int, bool, bool]]:
+        """
+        如果缓冲区中有可用数据，读取一帧 CAN 数据。
+        
+        :return: 元组 (can_id, data, cmd, extended, remote) 或者 None (如果没有完整帧)
+        """
+        # 读取可用数据
+        if self.serial.in_waiting:
+            raw_data = self.serial.read(self.serial.in_waiting)
+            self.data_buffer.extend(raw_data)
+            
+        # 检查完整帧 (16 字节)
+        while len(self.data_buffer) >= self.RECV_FRAME_LEN:
+            # 查找帧头
+            try:
+                header_idx = self.data_buffer.index(self.RECV_HEADER)
+            except ValueError:
+                # 没有找到帧头，清空缓冲区（保留最后几个字节以防截断）
+                self.data_buffer = self.data_buffer[-(self.RECV_FRAME_LEN-1):]
+                return None
+                
+            # 检查从帧头开始是否有足够字节
+            if len(self.data_buffer) - header_idx < self.RECV_FRAME_LEN:
+                # 保留从帧头开始的数据
+                self.data_buffer = self.data_buffer[header_idx:]
+                return None
+                
+            # 检查帧尾
+            if self.data_buffer[header_idx + self.RECV_FRAME_LEN - 1] != self.RECV_TAIL:
+                # 无效帧，跳过该帧头继续查找
+                self.data_buffer = self.data_buffer[header_idx + 1:]
+                continue
+                
+            # 提取有效帧
+            frame = self.data_buffer[header_idx : header_idx + self.RECV_FRAME_LEN]
+            self.data_buffer = self.data_buffer[header_idx + self.RECV_FRAME_LEN:]
+            
+            if self.debug:
+                print(f"[DEBUG] 解析帧: {frame.hex()}")
+
+            # 解析帧
+            cmd = frame[1]
+            format_byte = frame[2]
+            
+            data_len = format_byte & 0x3F
+            ide = bool((format_byte >> 6) & 0x01)
+            rtr = bool((format_byte >> 7) & 0x01)
+            
+            can_id = struct.unpack('<I', frame[3:7])[0]
+            data = bytes(frame[7:15])
+            
+            if data_len < 8:
+                data = data[:data_len]
+                
+            return (can_id, data, cmd, ide, rtr)
+            
+        return None

05_software/real/sim2real_v2/vendored/odin1_imu/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.16)
+project(odin1 LANGUAGES C CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+find_package(PkgConfig REQUIRED)
+find_package(OpenSSL REQUIRED)
+pkg_check_modules(LIBUSB REQUIRED libusb-1.0)
+
+add_library(odin1_imu_bridge SHARED
+    src/odin1_imu_bridge.cpp
+)
+
+target_include_directories(odin1_imu_bridge
+    PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}/include
+        ${LIBUSB_INCLUDE_DIRS}
+)
+
+target_link_directories(odin1_imu_bridge
+    PRIVATE
+        ${CMAKE_CURRENT_SOURCE_DIR}/lib
+)
+
+target_link_libraries(odin1_imu_bridge
+    PRIVATE
+        ${CMAKE_CURRENT_SOURCE_DIR}/lib/liblydHostApi_arm.a
+        ${LIBUSB_LIBRARIES}
+        OpenSSL::SSL
+        OpenSSL::Crypto
+        pthread
+        rt
+        dl
+)

05_software/real/sim2real_v2/vendored/odin1_imu/build.sh

@@ -0,0 +1,8 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+BUILD_DIR="${SCRIPT_DIR}/build"
+
+cmake -S "${SCRIPT_DIR}" -B "${BUILD_DIR}" -DCMAKE_BUILD_TYPE=Release
+cmake --build "${BUILD_DIR}" -j"$(nproc)"

05_software/real/sim2real_v2/vendored/odin1_imu/include/lidar_api.h

@@ -0,0 +1,308 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+#ifndef LIDAR_API_H
+#define LIDAR_API_H
+
+/**
+ * @file lidar_api.h
+ * @brief LiDAR device API for controlling and accessing LiDAR sensor data
+ * 
+ * This header provides the public interface for interacting with LiDAR devices.
+ * It includes functions for device management, data streaming control, and
+ * device configuration.
+ *
+ * @copyright Copyright (c) 2025, Manifold Tech Limited, All Rights Reserved
+ * @version 1.0
+ */
+
+#include "lidar_api_type.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Initialize the LiDAR system
+ * 
+ * Must be called before any other lidar function to set up the system resources.
+ * 
+ * @param cb Callback function for device events (connection, disconnection)
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_system_init(lidar_device_callback_t cb);
+
+/**
+ * @brief Deinitialize the LiDAR system
+ * 
+ * Releases all resources allocated by the system. Should be called when
+ * application is shutting down.
+ * 
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_system_deinit(void);
+
+/**
+ * @brief Create a handle for a LiDAR device
+ * 
+ * @param dev_info Information about the LiDAR device to create
+ * @param device Pointer to receive the device handle upon success
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_create_device(lidar_device_info_t *dev_info, device_handle *device);
+
+/**
+ * @brief Destroy a LiDAR device handle
+ * 
+ * Releases resources associated with the device handle. Must be called 
+ * when the device is no longer needed.
+ * 
+ * @param device Handle to the device to destroy
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_destory_device(device_handle device);
+
+/**
+ * @brief Register callback function for receiving LiDAR data streams
+ * 
+ * Sets up a callback function that will be called when new data is available.
+ * 
+ * @param device Handle to the target device
+ * @param cb Callback information containing function pointers for different data types
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_register_stream_callback(device_handle device, lidar_data_callback_info_t cb);
+
+/**
+ * @brief Unregister stream callback for a device
+ * 
+ * Stops the device from calling back when new data is available.
+ * 
+ * @param device Handle to the target device
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_unregister_stream_callback(device_handle device);
+
+/**
+ * @brief Open a LiDAR device for communication
+ * 
+ * Establishes a connection to the physical device.
+ * 
+ * @param device Handle to the device to open
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_open_device(device_handle device);
+
+/**
+ * @brief Close a LiDAR device
+ * 
+ * Closes the connection to the physical device.
+ * 
+ * @param device Handle to the device to close
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_close_device(device_handle device);
+
+/**
+ * @brief Set the operating mode of the LiDAR device
+ * 
+ * @param device Handle to the target device
+ * @param mode Operating mode to set (see mode definitions in lidar_api_type.h)
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_set_mode(device_handle device, int mode);
+
+/**
+ * @brief Start data streaming from the device
+ * 
+ * Begins the flow of data from the device for the specified type.
+ * 
+ * @param device Handle to the target device
+ * @param type Type of data stream to start (see stream type definitions in lidar_api_type.h)
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_start_stream(device_handle device, int type, uint32_t &dtof_subframe_odr);
+
+/**
+ * @brief Stop data streaming from the device
+ * 
+ * Stops the flow of data from the device for the specified type.
+ * 
+ * @param device Handle to the target device
+ * @param type Type of data stream to stop
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_stop_stream(device_handle device, int type);
+
+/**
+ * @brief Activate a specific stream type on the device
+ * 
+ * Enables a specific data stream type in the device configuration.
+ * 
+ * @param device Handle to the target device
+ * @param type Type of data stream to activate
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_activate_stream_type(device_handle device, int type);
+
+/**
+ * @brief Deactivate a specific stream type on the device
+ * 
+ * Disables a specific data stream type in the device configuration.
+ * 
+ * @param device Handle to the target device
+ * @param type Type of data stream to deactivate
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_deactivate_stream_type(device_handle device, int type);
+
+/**
+ * @brief Get calibration file from the device
+ * 
+ * Retrieves the calibration file from the device.
+ * 
+ * @param device Handle to the target device
+ * @param path Path to save the calibration file
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_get_calib_file(device_handle device, const char* path);
+
+/**
+ * @brief Set log verbosity level
+ * 
+ * Controls the amount of log information generated by the LiDAR API.
+ * 
+ * @param level Log level to set (see level definitions in lidar_api_type.h)
+ */
+void lidar_log_set_level(lidar_log_level_e level);
+
+/**
+ * @brief Get the version information of the LiDAR device
+ * 
+ * Retrieves version information including firmware, system, and application versions.
+ * 
+ * @param device Handle to the target device
+ * @param version struct Pointer to receive the version information
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_get_version(device_handle device,lidar_fireware_version_t *version);
+
+/**
+ * @brief Set custom algorithm parameters for the device
+ * 
+ * Sends custom parameter settings to the device.
+ *
+ * @param device Handle to the target device
+ * @param param_name String name of the parameter to set
+ * @param value_data Pointer to the value data to set for the parameter
+ * @param value_length Length of the value data in bytes
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_set_custom_parameter(device_handle device, const char* param_name, const void* value_data, size_t value_length);
+
+/**
+ * @brief Get custom algorithm parameters for the device
+ * 
+ * Get custom parameter settings from the device.
+ *
+ * @param device Handle to the target device
+ * @param param_name String name of the parameter to get
+ * @param value Integer value to get for the parameter
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_get_custom_parameter(device_handle device, const char* param_name, int* value);
+
+/**
+ * @brief Set the map file used for relocalization
+ *
+ * Read & send specified map file to device for relocalization
+ *
+ * @param device Handle to the target device
+ * @param abs_path Absolute path to the map file
+ * @return int 0 on success, otherwise on failure
+ */
+ int lidar_set_relocalization_map(device_handle device, const char* abs_path);
+
+ /**
+ * @brief Get the mapping result file from device
+ *
+ * Read & send specified map file from device to host
+ *
+ * @param device Handle to the target device
+ * @param dest_dir Destination directory to save the map file
+ * @param file_name File name to save the map file
+ * @return int 0 on success, -1 on failure without error code, error code (> 0) otherwise
+ */
+ int lidar_get_mapping_result(device_handle device, const char* dest_dir, const char* file_name);
+
+/**
+ * @brief Set the image mask file for the device
+ *
+ * Read & send specified image mask file to device
+ *
+ * @param device Handle to the target device
+ * @param abs_path Absolute path to the image mask file (e.g., mask.png)
+ * @return int 0 on success, -1 on failure, -2 if file transfer in progress
+ */
+ int lidar_set_image_mask(device_handle device, const char* abs_path);
+
+
+  /**
+ * @brief enable device log
+ *
+ *
+ * @param device Handle to the target device
+ * @param dest_dir Destination directory to save the logs
+ * @return int 0 on success, -1 on failure
+ */
+ int lidar_enable_encrypted_device_log(device_handle device, const char* dest_dir);
+
+
+/**
+ * @brief Set the depth parameters for the device
+ * 
+ * This function must be called before starting data stream.
+ *
+ * @param device Handle to the target device
+ * @param params Pointer to the depth parameters to set
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_set_depth_parameter(device_handle device, const lidar_depth_para_t *params);
+
+/**
+ * @brief Enable or disable IMU smooth sending feature
+ *
+ * When enabled, IMU data will be sent at precise intervals (default 400Hz) 
+ * using a dedicated high-priority thread to reduce jitter and timing variance.
+ * When disabled, IMU data will be sent immediately upon reception.
+ *
+ * @param enable 1 to enable smooth sending, 0 to disable
+ * @return int 0 on success, -1 on failure
+ */
+int lidar_enable_imu_smooth_sending(int enable);
+
+/**
+ * @brief Set IMU smooth sending frequency
+ *
+ * Set the target frequency for IMU smooth sending. Only effective when 
+ * smooth sending is enabled via lidar_enable_imu_smooth_sending().
+ *
+ * @param frequency_hz Target frequency in Hz (1-1000 Hz, recommended 400 Hz)
+ * @return int 0 on success, -1 on failure
+ */
+int lidar_set_imu_smooth_frequency(uint32_t frequency_hz);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LIDAR_API_H

05_software/real/sim2real_v2/vendored/odin1_imu/include/lidar_api_type.h

@@ -0,0 +1,242 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+#ifndef LIDAR_TYPES_H
+#define LIDAR_TYPES_H
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define LIDAR_SERIAL_MAX 64
+#define LIDAR_MODEL_MAX  64
+#define LIDAR_IP_MAX     64
+
+typedef void * device_handle;
+
+typedef enum {
+    LIDAR_LOG_ERROR = 0,
+    LIDAR_LOG_WARN,
+    LIDAR_LOG_INFO,
+    LIDAR_LOG_DEBUG,
+} lidar_log_level_e;
+
+typedef enum {
+    LIDAR_OTA_ALGORITHM,
+    LIDAR_OTA_FIRMWARE,
+    LIDAR_OTA_SCRIPT,
+    LIDAR_OTA_CALIBRATION
+} lidar_ota_type_e;
+
+typedef enum {
+    LIDAR_MODE_RAW,
+    LIDAR_MODE_SLAM,
+} lidar_mode_e;
+
+typedef enum {
+    LIDAR_DT_NONE = 0,
+    LIDAR_DT_RAW_RGB,
+    LIDAR_DT_RAW_IMU,
+    LIDAR_DT_RAW_DTOF,
+    LIDAR_DT_SLAM_CLOUD,
+    LIDAR_DT_SLAM_ODOMETRY,
+    LIDAR_DT_DEV_STATUS,
+    LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ,
+    LIDAR_DT_SLAM_ODOMETRY_TF,
+    LIDAR_DT_SLAM_WIWC,
+    LIDAR_DT_NTP
+} lidar_data_type_e;
+
+typedef struct {
+    int8_t serial[LIDAR_SERIAL_MAX];
+    int8_t model[LIDAR_MODEL_MAX];
+    bool online;
+    uint32_t initial_state;
+} lidar_device_info_t;
+
+typedef struct {
+    float x, y, z;
+    float intensity;
+} lidar_point_t;
+
+
+typedef struct {
+    float intrinsics[9];
+    float extrinsics[16];
+} lidar_calibration_t;
+
+
+#define DEVICE_MAX_CH_NUMBER 4
+
+typedef struct {
+    uint64_t timestamp_ns;
+    int64_t pos[3];
+    int64_t orient[4];
+} ros2_odom_convert_t;
+
+typedef struct {
+    uint64_t timestamp_ns;
+    int64_t pos[3];
+    int64_t orient[4];
+    int64_t linear_velocity[3];
+    int64_t angular_velocity[3];
+    double pose_cov[36];
+    double twist_cov[36];
+} ros_odom_convert_complete_t;
+
+typedef struct {
+    float accel_x;
+    float accel_y;
+    float accel_z;
+    float gyro_x;
+    float gyro_y;
+    float gyro_z;
+    uint64_t stamp;
+    uint64_t sequence;
+} imu_convert_data_t;
+
+ typedef struct {
+    uint32_t length;
+    uint64_t sequence;
+    uint64_t timestamp;
+    uint64_t interval;
+    void* pAddr;
+    uint32_t width;
+    uint32_t height;
+} buffer_List_t;
+
+typedef struct {
+    double delay;
+    double offset;
+} ptp_sync_data_t;
+
+typedef struct capture_Image_List_t {
+    uint32_t imageCount;
+    buffer_List_t imageList[DEVICE_MAX_CH_NUMBER];
+} capture_Image_List_t;
+
+typedef struct {
+    uint32_t type;
+    capture_Image_List_t stream;
+} lidar_data_t;
+
+typedef void (*lidar_device_callback_t)(const lidar_device_info_t* device, bool attach);
+typedef void (*lidar_data_callback_t)(const lidar_data_t *data, void *user_data);
+
+typedef struct {
+    lidar_data_callback_t data_callback;
+    void *user_data;
+} lidar_data_callback_info_t;
+
+typedef struct {
+    int major;
+    int minor;
+    int patch;
+}lidar_version_t;
+
+typedef struct {
+    lidar_version_t kernel_version;
+    lidar_version_t mcu_version;
+    lidar_version_t soc_version;
+    lidar_version_t Daemon_proc_version;
+    lidar_version_t slam_version;
+} lidar_fireware_version_t;
+
+/**
+ * @brief RGB image sensor frame rate
+ * 
+ */
+ typedef struct{
+
+    int configured_odr; /* rgb image sensor configured output data rate */
+    int tx_odr;         /* rgb image sensor tx output data rate */
+
+} lidar_rgb_sensor_status_t;
+
+/**
+ * @brief DTOF Lidar frame rate
+ * 
+ */
+typedef struct{
+
+    int configured_odr; /* dtof lidar sensor configured output data rate */
+    int tx_odr;         /* dtof lidar sensor tx output data rate */
+    int subframe_odr;   /* dtof lidar sensor subframe output data rate */
+    short tx_temp;      /* dtof lidar tx module temp */
+    short rx_temp;      /* dtof lidar rx module temp */
+
+} lidar_dtof_sensor_status_t;
+
+/**
+ * @brief IMU Sensor
+ * 
+ */
+typedef struct{
+
+    int configured_odr; /* imu sensor configured output data rate */
+    int tx_odr;         /* imu sensor tx output data rate */
+
+} lidar_imu_sensor_status_t;
+
+typedef struct{
+
+    int package_temp;   /* soc package temp */
+    int cpu_temp;       /* cpu temp */
+    int center_temp;    /* center temp */
+    int gpu_temp;       /* gpu temp */
+    int npu_temp;       /* npu temp */
+
+} lidar_soc_thermal_t;
+typedef struct
+{
+    double uptime_seconds;
+    lidar_soc_thermal_t soc_thermal; 
+
+    int cpu_use_rate[8];              /* cpu usage rate */
+    int ram_use_rate;                 /* ram usage rate */
+
+    lidar_rgb_sensor_status_t rgb_sensor;
+    lidar_dtof_sensor_status_t dtof_sensor;
+    lidar_imu_sensor_status_t imu_sensor;
+
+    int slam_cloud_tx_odr;          /* slam cloud tx output data rate */
+    int slam_odom_tx_odr;           /* slam odom tx output data rate */
+    int slam_odom_highfreq_tx_odr;  /* slam odom high freq tx output data rate */
+
+} lidar_device_status_t;
+
+typedef enum {
+    LIDAR_DEVICE_NONE = 0,
+    LIDAR_DEVICE_NOT_INITIALIZED,
+    LIDAR_DEVICE_INITIALIZED,
+    LIDAR_DEVICE_STREAMING,
+    LIDAR_DEVICE_STREAM_STOPPED,
+} lidar_device_initial_state_e;
+
+typedef enum {
+    LIDAR_DEPTH_ODR_10HZ = 0,
+    LIDAR_DEPTH_ODR_14_5HZ,
+} lidar_depth_odr_e;
+
+typedef struct {
+    lidar_depth_odr_e odr;
+} lidar_depth_para_t;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

05_software/real/sim2real_v2/vendored/odin1_imu/include/odin1_imu_bridge.h

@@ -0,0 +1,159 @@
+#ifndef ODIN1_IMU_BRIDGE_H
+#define ODIN1_IMU_BRIDGE_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * 输入: 无
+ * 输出: odin1_imu_sample_t
+ * 作用: 描述一帧 IMU 数据, 供 C/C++/Python 共享使用
+ * 注意: 坐标系已从 SDK 重映射为 ROS 标准 (right-handed: x前 y左 z上)
+ *       重映射规则: accel_x←SDK_accel_y, accel_y←-SDK_accel_x, accel_z←SDK_accel_z
+ *                   gyro_x ←SDK_gyro_y,  gyro_y ←-SDK_gyro_x,  gyro_z ←SDK_gyro_z
+ */
+typedef struct odin1_imu_sample_t {
+    float accel_x;  /* m/s², ROS 标准坐标系 */
+    float accel_y;
+    float accel_z;
+    float gyro_x;   /* rad/s, ROS 标准坐标系 */
+    float gyro_y;
+    float gyro_z;
+    uint64_t stamp_ns;
+    uint64_t sequence;
+} odin1_imu_sample_t;
+
+/**
+ * 输入: 无
+ * 输出: odin1_odom_type_e
+ * 作用: 标识里程计数据类型
+ */
+typedef enum {
+    ODIN1_ODOM_STANDARD = 0,   /* 标准里程计, 含位置/姿态/速度/协方差 */
+    ODIN1_ODOM_HIGHFREQ,        /* 高频里程计, 含位置/姿态 */
+    ODIN1_ODOM_TF,              /* TF 变换(重定位后), 全局坐标系下位姿 */
+} odin1_odom_type_e;
+
+/**
+ * 输入: 无
+ * 输出: odin1_odom_sample_t
+ * 作用: 描述一帧里程计数据, 统一容纳三种类型, 供 C/C++/Python 共享使用
+ * 缩放: SDK 原始 int64 值已按 ÷1e6 转换为 double (位置:米, 姿态:四元数, 速度:m/s, rad/s)
+ * 注意: 速度与协方差仅 ODIN1_ODOM_STANDARD 有效, 其他类型为零
+ */
+typedef struct odin1_odom_sample_t {
+    odin1_odom_type_e type;
+    uint64_t stamp_ns;
+    double pos_x;          /* 米, odom 坐标系 */
+    double pos_y;
+    double pos_z;
+    double orient_w;       /* 单位四元数 */
+    double orient_x;
+    double orient_y;
+    double orient_z;
+    double linear_vel_x;   /* m/s (仅 STANDARD 有效) */
+    double linear_vel_y;
+    double linear_vel_z;
+    double angular_vel_x;  /* rad/s (仅 STANDARD 有效) */
+    double angular_vel_y;
+    double angular_vel_z;
+    double pose_cov[36];   /* 位姿协方差 (仅 STANDARD 有效) */
+    double twist_cov[36];  /* 速度协方差 (仅 STANDARD 有效) */
+} odin1_odom_sample_t;
+
+/**
+ * 输入: 无
+ * 输出: const char*
+ * 作用: 返回当前 bridge 的版本字符串
+ */
+const char* odin1_imu_version(void);
+
+/**
+ * 输入: timeout_ms[int]
+ * 输出: int, 0 表示成功, 非 0 表示失败
+ * 作用: 初始化 SDK, 等待设备连接并开始 IMU 数据流
+ */
+int odin1_imu_start(int timeout_ms);
+
+/**
+ * 输入: 无
+ * 输出: 无
+ * 作用: 停止数据流并释放 SDK 资源
+ */
+void odin1_imu_stop(void);
+
+/**
+ * 输入: 无
+ * 输出: int, 1 表示运行中, 0 表示未运行
+ * 作用: 返回当前 bridge 是否处于运行状态
+ */
+int odin1_imu_is_running(void);
+
+/**
+ * 输入: timeout_ms[int]
+ * 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常
+ * 作用: 阻塞等待 IMU 数据到达
+ */
+int odin1_imu_wait_for_data(int timeout_ms);
+
+/**
+ * 输入: out_sample[odin1_imu_sample_t*]
+ * 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常
+ * 作用: 从内部队列中弹出一帧 IMU 数据
+ */
+int odin1_imu_pop_sample(odin1_imu_sample_t* out_sample);
+
+/**
+ * 输入: out_sample[odin1_imu_sample_t*]
+ * 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常
+ * 作用: 获取最近一帧 IMU 数据, 不会从队列中删除
+ */
+int odin1_imu_get_latest(odin1_imu_sample_t* out_sample);
+
+/**
+ * 输入: 无
+ * 输出: const char*
+ * 作用: 返回最近一次错误信息
+ */
+const char* odin1_imu_last_error(void);
+
+// ---------------------------------------------------------------------------
+// Odom (里程计) 接口
+// ---------------------------------------------------------------------------
+
+/**
+ * 输入: timeout_ms[int]
+ * 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常
+ * 作用: 阻塞等待里程计数据到达（任意类型）
+ */
+int odin1_odom_wait_for_data(int timeout_ms);
+
+/**
+ * 输入: out_sample[odin1_odom_sample_t*]
+ * 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常
+ * 作用: 从内部队列中弹出一帧里程计数据
+ */
+int odin1_odom_pop_sample(odin1_odom_sample_t* out_sample);
+
+/**
+ * 输入: out_sample[odin1_odom_sample_t*]
+ * 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常
+ * 作用: 获取最近一帧里程计数据, 不会从队列中删除
+ */
+int odin1_odom_get_latest(odin1_odom_sample_t* out_sample);
+
+/**
+ * 输入: 无
+ * 输出: const char*
+ * 作用: 返回最近一次里程计错误信息
+ */
+const char* odin1_odom_last_error(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

05_software/real/sim2real_v2/vendored/odin1_imu/odin1_imu.py

@@ -0,0 +1,230 @@
+#!/usr/bin/python3
+"""ODIN1 IMU / Odom ctypes 封装."""
+
+from __future__ import annotations
+
+import ctypes
+from enum import IntEnum
+from pathlib import Path
+from typing import Iterator, Optional
+
+
+class Odin1ImuSample(ctypes.Structure):
+    """输入: 无; 输出: Odin1ImuSample; 作用: 映射 C++ bridge 的 IMU 结构体."""
+
+    _fields_ = [
+        ("accel_x", ctypes.c_float),
+        ("accel_y", ctypes.c_float),
+        ("accel_z", ctypes.c_float),
+        ("gyro_x", ctypes.c_float),
+        ("gyro_y", ctypes.c_float),
+        ("gyro_z", ctypes.c_float),
+        ("stamp_ns", ctypes.c_uint64),
+        ("sequence", ctypes.c_uint64),
+    ]
+
+
+class Odin1OdomType(IntEnum):
+    """输入: 无; 输出: Odin1OdomType; 作用: 匹配 C 端 odin1_odom_type_e."""
+
+    STANDARD = 0
+    HIGHFREQ = 1
+    TF = 2
+
+
+class Odin1OdomSample(ctypes.Structure):
+    """输入: 无; 输出: Odin1OdomSample; 作用: 映射 C++ bridge 的里程计结构体.
+       位置: 米, 姿态: 单位四元数, 速度: m/s 和 rad/s, 协方差: 原始 double."""
+
+    _fields_ = [
+        ("type", ctypes.c_int),
+        ("stamp_ns", ctypes.c_uint64),
+        ("pos_x", ctypes.c_double),
+        ("pos_y", ctypes.c_double),
+        ("pos_z", ctypes.c_double),
+        ("orient_w", ctypes.c_double),
+        ("orient_x", ctypes.c_double),
+        ("orient_y", ctypes.c_double),
+        ("orient_z", ctypes.c_double),
+        ("linear_vel_x", ctypes.c_double),
+        ("linear_vel_y", ctypes.c_double),
+        ("linear_vel_z", ctypes.c_double),
+        ("angular_vel_x", ctypes.c_double),
+        ("angular_vel_y", ctypes.c_double),
+        ("angular_vel_z", ctypes.c_double),
+        ("pose_cov", ctypes.c_double * 36),
+        ("twist_cov", ctypes.c_double * 36),
+    ]
+
+
+class Odin1ImuClient:
+    """输入: lib_path[Optional[str|Path]]; 输出: Odin1ImuClient; 作用: 提供 Python 对 ODIN1 IMU bridge 的访问接口."""
+
+    def __init__(self, lib_path: Optional[str | Path] = None) -> None:
+        self._project_root = Path(__file__).resolve().parents[1]
+        resolved_path = Path(lib_path) if lib_path else self._project_root / "build" / "libodin1_imu_bridge.so"
+        self._lib = ctypes.CDLL(str(resolved_path))
+        self._configure_signatures()
+
+    def _configure_signatures(self) -> None:
+        """输入: 无; 输出: 无; 作用: 配置 ctypes 函数签名."""
+
+        self._lib.odin1_imu_version.restype = ctypes.c_char_p
+
+        self._lib.odin1_imu_start.argtypes = [ctypes.c_int]
+        self._lib.odin1_imu_start.restype = ctypes.c_int
+
+        self._lib.odin1_imu_stop.argtypes = []
+        self._lib.odin1_imu_stop.restype = None
+
+        self._lib.odin1_imu_is_running.argtypes = []
+        self._lib.odin1_imu_is_running.restype = ctypes.c_int
+
+        self._lib.odin1_imu_wait_for_data.argtypes = [ctypes.c_int]
+        self._lib.odin1_imu_wait_for_data.restype = ctypes.c_int
+
+        self._lib.odin1_imu_pop_sample.argtypes = [ctypes.POINTER(Odin1ImuSample)]
+        self._lib.odin1_imu_pop_sample.restype = ctypes.c_int
+
+        self._lib.odin1_imu_get_latest.argtypes = [ctypes.POINTER(Odin1ImuSample)]
+        self._lib.odin1_imu_get_latest.restype = ctypes.c_int
+
+        self._lib.odin1_imu_last_error.argtypes = []
+        self._lib.odin1_imu_last_error.restype = ctypes.c_char_p
+
+        # ---- Odom ----
+        self._lib.odin1_odom_wait_for_data.argtypes = [ctypes.c_int]
+        self._lib.odin1_odom_wait_for_data.restype = ctypes.c_int
+
+        self._lib.odin1_odom_pop_sample.argtypes = [ctypes.POINTER(Odin1OdomSample)]
+        self._lib.odin1_odom_pop_sample.restype = ctypes.c_int
+
+        self._lib.odin1_odom_get_latest.argtypes = [ctypes.POINTER(Odin1OdomSample)]
+        self._lib.odin1_odom_get_latest.restype = ctypes.c_int
+
+        self._lib.odin1_odom_last_error.argtypes = []
+        self._lib.odin1_odom_last_error.restype = ctypes.c_char_p
+
+    def version(self) -> str:
+        """输入: 无; 输出: str; 作用: 获取 C++ bridge 版本号."""
+
+        return self._lib.odin1_imu_version().decode("utf-8")
+
+    def last_error(self) -> str:
+        """输入: 无; 输出: str; 作用: 获取最近一次 bridge 错误信息."""
+
+        return self._lib.odin1_imu_last_error().decode("utf-8")
+
+    def start(self, timeout_ms: int = 5000) -> None:
+        """输入: timeout_ms[int]; 输出: 无; 作用: 启动 IMU 数据接收."""
+
+        result = self._lib.odin1_imu_start(timeout_ms)
+        if result != 0:
+            raise RuntimeError(f"启动 ODIN1 IMU 失败: {self.last_error()} (code={result})")
+
+    def stop(self) -> None:
+        """输入: 无; 输出: 无; 作用: 停止 IMU 数据接收."""
+
+        self._lib.odin1_imu_stop()
+
+    def is_running(self) -> bool:
+        """输入: 无; 输出: bool; 作用: 返回 bridge 是否仍在运行."""
+
+        return bool(self._lib.odin1_imu_is_running())
+
+    def wait_for_data(self, timeout_ms: int = 1000) -> bool:
+        """输入: timeout_ms[int]; 输出: bool; 作用: 等待 IMU 数据到达（bridge 停止时返回 False 而非抛异常）."""
+
+        result = self._lib.odin1_imu_wait_for_data(timeout_ms)
+        if result < 0:
+            if not self.is_running():
+                return False
+            raise RuntimeError(f"等待 IMU 数据失败: {self.last_error()} (code={result})")
+        return bool(result)
+
+    def pop_sample(self) -> Optional[Odin1ImuSample]:
+        """输入: 无; 输出: Optional[Odin1ImuSample]; 作用: 从队列中取出一帧 IMU 数据."""
+
+        sample = Odin1ImuSample()
+        result = self._lib.odin1_imu_pop_sample(ctypes.byref(sample))
+        if result < 0:
+            raise RuntimeError(f"读取 IMU 队列失败: {self.last_error()} (code={result})")
+        return sample if result == 1 else None
+
+    def get_latest(self) -> Optional[Odin1ImuSample]:
+        """输入: 无; 输出: Optional[Odin1ImuSample]; 作用: 获取最近一帧 IMU 数据（无缝锁，永不等锁）."""
+
+        sample = Odin1ImuSample()
+        result = self._lib.odin1_imu_get_latest(ctypes.byref(sample))
+        if result < 0:
+            raise RuntimeError(f"读取最新 IMU 数据失败: {self.last_error()} (code={result})")
+        return sample if result == 1 else None
+
+    @property
+    def latest(self) -> Optional[Odin1ImuSample]:
+        """输入: 无; 输出: Optional[Odin1ImuSample]; 作用: get_latest 的属性形式，控制循环中推荐使用."""
+        return self.get_latest()
+
+    def iter_samples(self, timeout_ms: int = 1000) -> Iterator[Odin1ImuSample]:
+        """输入: timeout_ms[int]; 输出: Iterator[Odin1ImuSample]; 作用: 连续迭代输出 IMU 数据."""
+
+        while self.is_running():
+            if not self.wait_for_data(timeout_ms):
+                continue
+            while True:
+                sample = self.pop_sample()
+                if sample is None:
+                    break
+                yield sample
+
+    # ---- Odom (里程计) 方法 ----
+
+    def odom_last_error(self) -> str:
+        """输入: 无; 输出: str; 作用: 获取最近一次里程计错误信息."""
+
+        return self._lib.odin1_odom_last_error().decode("utf-8")
+
+    def odom_wait_for_data(self, timeout_ms: int = 1000) -> bool:
+        """输入: timeout_ms[int]; 输出: bool; 作用: 等待里程计数据到达（bridge 停止时返回 False 而非抛异常）."""
+
+        result = self._lib.odin1_odom_wait_for_data(timeout_ms)
+        if result < 0:
+            if not self.is_running():
+                return False
+            raise RuntimeError(f"等待里程计数据失败: {self.odom_last_error()} (code={result})")
+        return bool(result)
+
+    def odom_pop_sample(self) -> Optional[Odin1OdomSample]:
+        """输入: 无; 输出: Optional[Odin1OdomSample]; 作用: 从队列中取出一帧里程计数据."""
+
+        sample = Odin1OdomSample()
+        result = self._lib.odin1_odom_pop_sample(ctypes.byref(sample))
+        if result < 0:
+            raise RuntimeError(f"读取里程计队列失败: {self.odom_last_error()} (code={result})")
+        return sample if result == 1 else None
+
+    def odom_get_latest(self) -> Optional[Odin1OdomSample]:
+        """输入: 无; 输出: Optional[Odin1OdomSample]; 作用: 获取最近一帧里程计数据（无缝锁）."""
+
+        sample = Odin1OdomSample()
+        result = self._lib.odin1_odom_get_latest(ctypes.byref(sample))
+        if result < 0:
+            raise RuntimeError(f"读取最新里程计数据失败: {self.odom_last_error()} (code={result})")
+        return sample if result == 1 else None
+
+    @property
+    def odom_latest(self) -> Optional[Odin1OdomSample]:
+        """输入: 无; 输出: Optional[Odin1OdomSample]; 作用: odom_get_latest 的属性形式."""
+        return self.odom_get_latest()
+
+    def iter_odom(self, timeout_ms: int = 1000) -> Iterator[Odin1OdomSample]:
+        """输入: timeout_ms[int]; 输出: Iterator[Odin1OdomSample]; 作用: 连续迭代输出里程计数据."""
+
+        while self.is_running():
+            if not self.odom_wait_for_data(timeout_ms):
+                continue
+            while True:
+                sample = self.odom_pop_sample()
+                if sample is None:
+                    break
+                yield sample

05_software/real/sim2real_v2/vendored/odin1_imu/src/odin1_imu_bridge.cpp

@@ -0,0 +1,655 @@
+#include "odin1_imu_bridge.h"
+
+#include "lidar_api.h"
+#include "lidar_api_type.h"
+
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <cstdio>
+#include <cstring>
+#include <deque>
+#include <mutex>
+#include <string>
+#include <thread>
+
+namespace {
+
+constexpr const char* kBridgeVersion = "0.2.0";
+constexpr std::size_t kMaxQueueSize = 1024;
+constexpr std::size_t kMaxOdomQueueSize = 2048;
+constexpr int kDefaultMode = LIDAR_MODE_SLAM;
+
+std::atomic<bool> g_running{false};
+std::atomic<bool> g_sdk_initialized{false};
+std::atomic<bool> g_device_connected{false};
+std::atomic<bool> g_stream_started{false};
+
+device_handle g_device = nullptr;
+
+std::mutex g_state_mutex;
+std::mutex g_queue_mutex;
+std::condition_variable g_queue_cv;
+std::deque<odin1_imu_sample_t> g_queue;
+odin1_imu_sample_t g_latest_sample{};
+bool g_has_latest_sample = false;
+
+// 无缝锁 (seqlock) 专用：让 get_latest 与 SDK 回调完全无争抢
+std::atomic<std::uint64_t> g_latest_seq{0};
+odin1_imu_sample_t g_latest_lockfree{};
+
+std::mutex g_error_mutex;
+std::string g_last_error = "bridge not started";
+
+// ---- Odom (里程计) 专用状态 ----
+std::mutex g_odom_queue_mutex;
+std::condition_variable g_odom_queue_cv;
+std::deque<odin1_odom_sample_t> g_odom_queue;
+bool g_odom_enabled = false;
+
+// 无缝锁 (seqlock) 专用：让 get_latest 与 SDK 回调完全无争抢
+std::atomic<std::uint64_t> g_odom_latest_seq{0};
+odin1_odom_sample_t g_odom_latest_lockfree{};
+
+std::mutex g_odom_error_mutex;
+std::string g_odom_last_error = "odom bridge not started";
+
+/**
+ * 输入: message[const std::string&]
+ * 输出: 无
+ * 作用: 线程安全地记录最近一次错误信息
+ */
+void set_last_error(const std::string& message) {
+    std::lock_guard<std::mutex> lock(g_error_mutex);
+    g_last_error = message;
+}
+
+/**
+ * 输入: message[const std::string&]
+ * 输出: 无
+ * 作用: 线程安全地记录最近一次 odom 错误信息
+ */
+void set_odom_last_error(const std::string& message) {
+    std::lock_guard<std::mutex> lock(g_odom_error_mutex);
+    g_odom_last_error = message;
+}
+
+/**
+ * 输入: 无
+ * 输出: 无
+ * 作用: 清空内部 IMU 队列和最近一帧缓存
+ */
+void clear_queue_locked_state() {
+    std::lock_guard<std::mutex> lock(g_queue_mutex);
+    g_queue.clear();
+    g_latest_sample = {};
+    g_has_latest_sample = false;
+}
+
+/**
+ * 输入: 无
+ * 输出: 无
+ * 作用: 清空内部 Odom 队列
+ */
+void clear_odom_queue_locked_state() {
+    std::lock_guard<std::mutex> lock(g_odom_queue_mutex);
+    g_odom_queue.clear();
+}
+
+/**
+ * 输入: raw_sample[const imu_convert_data_t*]
+ * 输出: odin1_imu_sample_t
+ * 作用: 将 SDK IMU 结构转换为 bridge 对外结构, 并重映射为 ROS 标准坐标系
+ *       坐标系: x前 y左 z上 (right-handed)
+ *       重映射: accel_x←SDK_accel_y, accel_y←-SDK_accel_x (来源: host_sdk_sample.h publishImu)
+ */
+odin1_imu_sample_t convert_sample(const imu_convert_data_t* raw_sample) {
+    odin1_imu_sample_t converted{};
+    if (raw_sample == nullptr) {
+        return converted;
+    }
+
+    // 加速度轴重映射 (SDK → ROS 标准)
+    converted.accel_x =  raw_sample->accel_y;
+    converted.accel_y = -raw_sample->accel_x;
+    converted.accel_z =  raw_sample->accel_z;
+    // 陀螺仪轴重映射
+    converted.gyro_x  =  raw_sample->gyro_y;
+    converted.gyro_y  = -raw_sample->gyro_x;
+    converted.gyro_z  =  raw_sample->gyro_z;
+    converted.stamp_ns = raw_sample->stamp;
+    converted.sequence = raw_sample->sequence;
+    return converted;
+}
+
+/**
+ * 输入: raw[const ros_odom_convert_complete_t*]
+ * 输出: odin1_odom_sample_t
+ * 作用: 将 SDK 标准里程计结构转换为 bridge 对外结构
+ *       缩放因子: ÷1e6 (来源: host_sdk_sample.h publishOdometry)
+ */
+odin1_odom_sample_t convert_odom_standard(const ros_odom_convert_complete_t* raw) {
+    odin1_odom_sample_t out{};
+    if (raw == nullptr) return out;
+    out.type = ODIN1_ODOM_STANDARD;
+    out.stamp_ns = raw->timestamp_ns;
+    out.pos_x   = static_cast<double>(raw->pos[0]) / 1e6;
+    out.pos_y   = static_cast<double>(raw->pos[1]) / 1e6;
+    out.pos_z   = static_cast<double>(raw->pos[2]) / 1e6;
+    out.orient_w = static_cast<double>(raw->orient[0]) / 1e6;
+    out.orient_x = static_cast<double>(raw->orient[1]) / 1e6;
+    out.orient_y = static_cast<double>(raw->orient[2]) / 1e6;
+    out.orient_z = static_cast<double>(raw->orient[3]) / 1e6;
+    out.linear_vel_x  = static_cast<double>(raw->linear_velocity[0]) / 1e6;
+    out.linear_vel_y  = static_cast<double>(raw->linear_velocity[1]) / 1e6;
+    out.linear_vel_z  = static_cast<double>(raw->linear_velocity[2]) / 1e6;
+    out.angular_vel_x = static_cast<double>(raw->angular_velocity[0]) / 1e6;
+    out.angular_vel_y = static_cast<double>(raw->angular_velocity[1]) / 1e6;
+    out.angular_vel_z = static_cast<double>(raw->angular_velocity[2]) / 1e6;
+    for (int i = 0; i < 36; ++i) {
+        out.pose_cov[i]  = raw->pose_cov[i];
+        out.twist_cov[i] = raw->twist_cov[i];
+    }
+    return out;
+}
+
+/**
+ * 输入: raw[const ros2_odom_convert_t*], type[odin1_odom_type_e]
+ * 输出: odin1_odom_sample_t
+ * 作用: 将 SDK 高频/TF 里程计结构转换为 bridge 对外结构
+ *       缩放因子: ÷1e6 (来源: host_sdk_sample.h publishOdometry)
+ */
+odin1_odom_sample_t convert_odom_compact(const ros2_odom_convert_t* raw, odin1_odom_type_e type) {
+    odin1_odom_sample_t out{};
+    if (raw == nullptr) return out;
+    out.type = type;
+    out.stamp_ns = raw->timestamp_ns;
+    out.pos_x   = static_cast<double>(raw->pos[0]) / 1e6;
+    out.pos_y   = static_cast<double>(raw->pos[1]) / 1e6;
+    out.pos_z   = static_cast<double>(raw->pos[2]) / 1e6;
+    out.orient_w = static_cast<double>(raw->orient[0]) / 1e6;
+    out.orient_x = static_cast<double>(raw->orient[1]) / 1e6;
+    out.orient_y = static_cast<double>(raw->orient[2]) / 1e6;
+    out.orient_z = static_cast<double>(raw->orient[3]) / 1e6;
+    return out;
+}
+
+/**
+ * 输入: 无
+ * 输出: 无
+ * 作用: 安全关闭当前设备与 SDK 资源
+ */
+void cleanup_device_and_sdk() {
+    std::lock_guard<std::mutex> lock(g_state_mutex);
+
+    if (g_device != nullptr) {
+        if (g_stream_started.load()) {
+            lidar_deactivate_stream_type(g_device, LIDAR_DT_RAW_IMU);  // SDK接口，来源: include/lidar_api.h
+            if (g_odom_enabled) {
+                lidar_deactivate_stream_type(g_device, LIDAR_DT_SLAM_ODOMETRY);  // SDK接口
+                lidar_deactivate_stream_type(g_device, LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ);  // SDK接口
+                lidar_deactivate_stream_type(g_device, LIDAR_DT_SLAM_ODOMETRY_TF);  // SDK接口
+            }
+            lidar_stop_stream(g_device, kDefaultMode);  // SDK接口，来源: include/lidar_api.h
+            g_stream_started = false;
+        }
+
+        lidar_unregister_stream_callback(g_device);  // SDK接口，来源: include/lidar_api.h
+        lidar_close_device(g_device);  // SDK接口，来源: include/lidar_api.h
+        lidar_destory_device(g_device);  // SDK接口，来源: include/lidar_api.h
+        g_device = nullptr;
+    }
+
+    if (g_sdk_initialized.load()) {
+        lidar_system_deinit();  // SDK接口，来源: include/lidar_api.h
+        g_sdk_initialized = false;
+    }
+
+    g_device_connected = false;
+}
+
+/**
+ * 输入: data[const lidar_data_t*], user_data[void*]
+ * 输出: 无
+ * 作用: 接收 SDK 回调中的 IMU 数据并写入内部缓存队列
+ */
+void lidar_data_callback(const lidar_data_t* data, void* user_data) {
+    (void)user_data;
+
+    if (!g_running.load() || data == nullptr) {
+        return;
+    }
+
+    // ---- IMU 处理 ----
+    if (data->type == LIDAR_DT_RAW_IMU) {
+        if (data->stream.imageList[0].pAddr == nullptr) {
+            set_last_error("sdk imu callback returned null payload");
+            return;
+        }
+
+        const auto* raw_sample =
+            static_cast<const imu_convert_data_t*>(data->stream.imageList[0].pAddr);
+        odin1_imu_sample_t sample = convert_sample(raw_sample);
+
+        {
+            std::lock_guard<std::mutex> lock(g_queue_mutex);
+            if (g_queue.size() >= kMaxQueueSize) {
+                g_queue.pop_front();
+            }
+            g_queue.push_back(sample);
+            g_latest_sample = sample;
+            g_has_latest_sample = true;
+        }
+
+        // 无缝锁写入：Python get_latest 可同时读取，永不等锁
+        {
+            std::uint64_t seq = g_latest_seq.fetch_add(1, std::memory_order_acquire) + 1;
+            g_latest_lockfree = sample;
+            g_latest_seq.store(seq + 1, std::memory_order_release);
+        }
+
+        g_queue_cv.notify_all();
+        return;
+    }
+
+    // ---- Odom 处理 ----
+    if (g_odom_enabled) {
+        odin1_odom_sample_t odom_sample{};
+        bool valid = false;
+
+        switch (data->type) {
+        case LIDAR_DT_SLAM_ODOMETRY: {
+            if (data->stream.imageList[0].pAddr == nullptr) {
+                set_odom_last_error("sdk standard odom callback returned null payload");
+                return;
+            }
+            const auto* raw =
+                static_cast<const ros_odom_convert_complete_t*>(data->stream.imageList[0].pAddr);
+            odom_sample = convert_odom_standard(raw);
+            valid = true;
+            break;
+        }
+        case LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ: {
+            if (data->stream.imageList[0].pAddr == nullptr) {
+                set_odom_last_error("sdk high-freq odom callback returned null payload");
+                return;
+            }
+            const auto* raw =
+                static_cast<const ros2_odom_convert_t*>(data->stream.imageList[0].pAddr);
+            odom_sample = convert_odom_compact(raw, ODIN1_ODOM_HIGHFREQ);
+            valid = true;
+            break;
+        }
+        case LIDAR_DT_SLAM_ODOMETRY_TF: {
+            if (data->stream.imageList[0].pAddr == nullptr) {
+                set_odom_last_error("sdk tf odom callback returned null payload");
+                return;
+            }
+            const auto* raw =
+                static_cast<const ros2_odom_convert_t*>(data->stream.imageList[0].pAddr);
+            odom_sample = convert_odom_compact(raw, ODIN1_ODOM_TF);
+            valid = true;
+            break;
+        }
+        default:
+            return;
+        }
+
+        if (!valid) return;
+
+        {
+            std::lock_guard<std::mutex> lock(g_odom_queue_mutex);
+            if (g_odom_queue.size() >= kMaxOdomQueueSize) {
+                g_odom_queue.pop_front();
+            }
+            g_odom_queue.push_back(odom_sample);
+        }
+
+        // 无缝锁写入
+        {
+            std::uint64_t seq = g_odom_latest_seq.fetch_add(1, std::memory_order_acquire) + 1;
+            g_odom_latest_lockfree = odom_sample;
+            g_odom_latest_seq.store(seq + 1, std::memory_order_release);
+        }
+
+        g_odom_queue_cv.notify_all();
+        return;
+    }
+
+    return;
+}
+
+/**
+ * 输入: device_info[const lidar_device_info_t*], attach[bool]
+ * 输出: 无
+ * 作用: 响应 SDK 设备插拔事件并启动 IMU 数据流
+ */
+void lidar_device_callback(const lidar_device_info_t* device_info, bool attach) {
+    if (!g_running.load()) {
+        return;
+    }
+
+    if (!attach) {
+        g_device_connected = false;
+        g_stream_started = false;
+        return;
+    }
+
+    if (device_info == nullptr) {
+        set_last_error("sdk device callback returned null device info");
+        return;
+    }
+
+    std::lock_guard<std::mutex> lock(g_state_mutex);
+
+    if (g_device != nullptr) {
+        return;
+    }
+
+    device_handle device_handle_local = nullptr;
+    if (lidar_create_device(const_cast<lidar_device_info_t*>(device_info), &device_handle_local) != 0) {  // SDK接口，来源: include/lidar_api.h
+        set_last_error("lidar_create_device failed");
+        return;
+    }
+
+    if (lidar_open_device(device_handle_local) != 0) {  // SDK接口，来源: include/lidar_api.h
+        set_last_error("lidar_open_device failed");
+        lidar_destory_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        return;
+    }
+
+    lidar_data_callback_info_t callback_info{};
+    callback_info.data_callback = lidar_data_callback;
+    callback_info.user_data = nullptr;
+    if (lidar_register_stream_callback(device_handle_local, callback_info) != 0) {  // SDK接口，来源: include/lidar_api.h
+        set_last_error("lidar_register_stream_callback failed");
+        lidar_close_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        lidar_destory_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        return;
+    }
+
+    uint32_t dtof_subframe_odr = 0;
+    if (lidar_start_stream(device_handle_local, kDefaultMode, dtof_subframe_odr) != 0) {  // SDK接口，来源: include/lidar_api.h
+        (void)dtof_subframe_odr;
+        set_last_error("lidar_start_stream failed");
+        lidar_unregister_stream_callback(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        lidar_close_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        lidar_destory_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        return;
+    }
+
+    if (lidar_activate_stream_type(device_handle_local, LIDAR_DT_RAW_IMU) != 0) {  // SDK接口，来源: include/lidar_api.h
+        set_last_error("lidar_activate_stream_type(raw_imu) failed");
+        lidar_stop_stream(device_handle_local, kDefaultMode);  // SDK接口，来源: include/lidar_api.h
+        lidar_unregister_stream_callback(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        lidar_close_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        lidar_destory_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        return;
+    }
+
+    // 激活里程计数据流（STANDARD 类型，SDK 会自动启用 HIGHFREQ 和 TF）
+    // 参考: official ROS driver 只激活 LIDAR_DT_SLAM_ODOMETRY
+    if (lidar_activate_stream_type(device_handle_local, LIDAR_DT_SLAM_ODOMETRY) != 0) {  // SDK接口
+        set_odom_last_error("lidar_activate_stream_type(slam_odometry) failed, odom disabled");
+        g_odom_enabled = false;
+        fprintf(stderr, "[bridge] WARNING: odom stream activation failed, odom disabled\n");
+    } else {
+        g_odom_enabled = true;
+        set_odom_last_error("");
+        clear_odom_queue_locked_state();
+        fprintf(stdout, "[bridge] odom stream activated (STANDARD + HIGHFREQ + TF)\n");
+    }
+
+    g_device = device_handle_local;
+    g_stream_started = true;
+    g_device_connected = true;
+    set_last_error("");
+    g_queue_cv.notify_all();
+}
+
+}  // namespace
+
+extern "C" {
+
+/**
+ * 输入: 无
+ * 输出: const char*
+ * 作用: 返回当前 bridge 的版本字符串
+ */
+const char* odin1_imu_version(void) {
+    return kBridgeVersion;
+}
+
+/**
+ * 输入: timeout_ms[int]
+ * 输出: int, 0 表示成功, 非 0 表示失败
+ * 作用: 初始化 SDK, 等待设备连接并开始 IMU 数据流
+ */
+int odin1_imu_start(int timeout_ms) {
+    if (timeout_ms <= 0) {
+        timeout_ms = 5000;
+    }
+
+    if (g_running.load()) {
+        return 0;
+    }
+
+    clear_queue_locked_state();
+    set_last_error("waiting for odin1 device");
+
+    if (lidar_system_init(lidar_device_callback) != 0) {  // SDK接口，来源: include/lidar_api.h
+        set_last_error("lidar_system_init failed");
+        return -1;
+    }
+
+    g_sdk_initialized = true;
+    g_running = true;
+
+    const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms);
+    while (std::chrono::steady_clock::now() < deadline) {
+        if (g_device_connected.load()) {
+            return 0;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+
+    set_last_error("timeout waiting for odin1 imu stream");
+    odin1_imu_stop();
+    return -2;
+}
+
+/**
+ * 输入: 无
+ * 输出: 无
+ * 作用: 停止数据流并释放 SDK 资源
+ */
+void odin1_imu_stop(void) {
+    g_running = false;
+    g_odom_enabled = false;
+    cleanup_device_and_sdk();
+    clear_queue_locked_state();
+    clear_odom_queue_locked_state();
+    g_queue_cv.notify_all();
+    g_odom_queue_cv.notify_all();
+}
+
+/**
+ * 输入: 无
+ * 输出: int, 1 表示运行中, 0 表示未运行
+ * 作用: 返回当前 bridge 是否处于运行状态
+ */
+int odin1_imu_is_running(void) {
+    return g_running.load() ? 1 : 0;
+}
+
+/**
+ * 输入: timeout_ms[int]
+ * 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常
+ * 作用: 阻塞等待 IMU 数据到达
+ */
+int odin1_imu_wait_for_data(int timeout_ms) {
+    if (!g_running.load()) {
+        return -1;
+    }
+
+    std::unique_lock<std::mutex> lock(g_queue_mutex);
+    const bool ready = g_queue_cv.wait_for(
+        lock,
+        std::chrono::milliseconds(timeout_ms > 0 ? timeout_ms : 1000),
+        [] { return !g_queue.empty() || !g_running.load(); });
+
+    if (!g_running.load()) {
+        return -1;
+    }
+
+    return ready && !g_queue.empty() ? 1 : 0;
+}
+
+/**
+ * 输入: out_sample[odin1_imu_sample_t*]
+ * 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常
+ * 作用: 从内部队列中弹出一帧 IMU 数据
+ */
+int odin1_imu_pop_sample(odin1_imu_sample_t* out_sample) {
+    if (out_sample == nullptr) {
+        set_last_error("odin1_imu_pop_sample received null output pointer");
+        return -1;
+    }
+
+    std::lock_guard<std::mutex> lock(g_queue_mutex);
+    if (g_queue.empty()) {
+        return 0;
+    }
+
+    *out_sample = g_queue.front();
+    g_queue.pop_front();
+    return 1;
+}
+
+/**
+ * 输入: out_sample[odin1_imu_sample_t*]
+ * 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常
+ * 作用: 获取最近一帧 IMU 数据, 不会从队列中删除
+ */
+int odin1_imu_get_latest(odin1_imu_sample_t* out_sample) {
+    if (out_sample == nullptr) {
+        set_last_error("odin1_imu_get_latest received null output pointer");
+        return -1;
+    }
+
+    // 无缝锁读取：与 SDK 回调无锁争抢，延迟最低
+    std::uint64_t before, after;
+    do {
+        before = g_latest_seq.load(std::memory_order_acquire);
+        if (before & 1) {
+            // 写者正在写，自旋等待（纳秒级，200Hz 写入下概率极低）
+            continue;
+        }
+        if (before == 0) {
+            // 还没有任何数据写入
+            return 0;
+        }
+        *out_sample = g_latest_lockfree;
+        after = g_latest_seq.load(std::memory_order_acquire);
+    } while (before != after);
+
+    return 1;
+}
+
+/**
+ * 输入: 无
+ * 输出: const char*
+ * 作用: 返回最近一次错误信息
+ */
+const char* odin1_imu_last_error(void) {
+    std::lock_guard<std::mutex> lock(g_error_mutex);
+    return g_last_error.c_str();
+}
+
+// ---------------------------------------------------------------------------
+// Odom (里程计) 接口实现
+// ---------------------------------------------------------------------------
+
+/**
+ * 输入: timeout_ms[int]
+ * 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常
+ * 作用: 阻塞等待里程计数据到达（任意类型）
+ */
+int odin1_odom_wait_for_data(int timeout_ms) {
+    if (!g_running.load()) {
+        return -1;
+    }
+
+    std::unique_lock<std::mutex> lock(g_odom_queue_mutex);
+    const bool ready = g_odom_queue_cv.wait_for(
+        lock,
+        std::chrono::milliseconds(timeout_ms > 0 ? timeout_ms : 1000),
+        [] { return !g_odom_queue.empty() || !g_running.load(); });
+
+    if (!g_running.load()) {
+        return -1;
+    }
+
+    return ready && !g_odom_queue.empty() ? 1 : 0;
+}
+
+/**
+ * 输入: out_sample[odin1_odom_sample_t*]
+ * 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常
+ * 作用: 从内部队列中弹出一帧里程计数据
+ */
+int odin1_odom_pop_sample(odin1_odom_sample_t* out_sample) {
+    if (out_sample == nullptr) {
+        set_odom_last_error("odin1_odom_pop_sample received null output pointer");
+        return -1;
+    }
+
+    std::lock_guard<std::mutex> lock(g_odom_queue_mutex);
+    if (g_odom_queue.empty()) {
+        return 0;
+    }
+
+    *out_sample = g_odom_queue.front();
+    g_odom_queue.pop_front();
+    return 1;
+}
+
+/**
+ * 输入: out_sample[odin1_odom_sample_t*]
+ * 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常
+ * 作用: 获取最近一帧里程计数据, 不会从队列中删除
+ */
+int odin1_odom_get_latest(odin1_odom_sample_t* out_sample) {
+    if (out_sample == nullptr) {
+        set_odom_last_error("odin1_odom_get_latest received null output pointer");
+        return -1;
+    }
+
+    // 无缝锁读取：与 SDK 回调无锁争抢，延迟最低
+    std::uint64_t before, after;
+    do {
+        before = g_odom_latest_seq.load(std::memory_order_acquire);
+        if (before & 1) {
+            continue;
+        }
+        if (before == 0) {
+            return 0;
+        }
+        *out_sample = g_odom_latest_lockfree;
+        after = g_odom_latest_seq.load(std::memory_order_acquire);
+    } while (before != after);
+
+    return 1;
+}
+
+/**
+ * 输入: 无
+ * 输出: const char*
+ * 作用: 返回最近一次里程计错误信息
+ */
+const char* odin1_odom_last_error(void) {
+    std::lock_guard<std::mutex> lock(g_odom_error_mutex);
+    return g_odom_last_error.c_str();
+}
+
+}  // extern "C"

05_software/real/sim2real_v2/web/server.py

@@ -0,0 +1,361 @@
+"""Minimal HTTP + SSE server for the sim2real web console."""
+
+from __future__ import annotations
+
+import argparse
+import json
+import os
+import queue
+import sys
+import threading
+import time
+import traceback
+from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
+from pathlib import Path
+from urllib.parse import urlparse
+
+import yaml
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+
+from web.session import RobotSession  # noqa: E402
+
+
+SESSION: "RobotSession" = None  # type: ignore
+
+
+def make_real_factory():
+    def outer():
+        sim2real_root = Path(__file__).resolve().parents[1]
+        workspace_root = sim2real_root.parent.parent
+        for path in (workspace_root,):
+            path_str = str(path)
+            if path_str not in sys.path and path.exists():
+                sys.path.append(path_str)
+
+        from dm_socket.sim2real_factory import driver_factory
+
+        return driver_factory
+
+    return outer
+
+
+def make_dry_factory():
+    def outer():
+        class MockMotor:
+            def __init__(self):
+                class State:
+                    position = 0.0
+                    velocity = 0.0
+                    torque = 0.0
+                    update_count = 0
+
+                self.state = State()
+
+        class MockDriver:
+            def __init__(self, port, debug):
+                self.port = port
+                self.motors = {}
+
+            def connect(self):
+                pass
+
+            def disconnect(self):
+                pass
+
+            def add_motor(self, name, motor_id, model):
+                self.motors[name] = MockMotor()
+
+            def enable(self, name):
+                pass
+
+            def disable(self, name):
+                pass
+
+            def clear_warnings(self, name):
+                pass
+
+            def process_messages(self):
+                for motor in self.motors.values():
+                    motor.state.update_count += 1
+
+            def control_mit(self, name, q, dq, kp, kd, tau):
+                if name in self.motors:
+                    self.motors[name].state.position = q
+                    self.motors[name].state.velocity = dq
+                    self.motors[name].state.torque = tau
+
+        def factory(can1_port, can2_port, debug):
+            return MockDriver(can1_port, debug), MockDriver(can2_port, debug)
+
+        return factory
+
+    return outer
+
+
+def _send_json(handler: BaseHTTPRequestHandler, code: int, obj):
+    body = json.dumps(obj, ensure_ascii=False).encode("utf-8")
+    handler.send_response(code)
+    handler.send_header("Content-Type", "application/json; charset=utf-8")
+    handler.send_header("Content-Length", str(len(body)))
+    handler.send_header("Cache-Control", "no-store")
+    handler.end_headers()
+    handler.wfile.write(body)
+
+
+def _send_static(handler: BaseHTTPRequestHandler, path: Path, content_type: str):
+    if not path.exists():
+        handler.send_error(404, str(path))
+        return
+    body = path.read_bytes()
+    handler.send_response(200)
+    handler.send_header("Content-Type", content_type)
+    handler.send_header("Content-Length", str(len(body)))
+    handler.send_header("Cache-Control", "no-store, no-cache, must-revalidate, max-age=0")
+    handler.send_header("Pragma", "no-cache")
+    handler.send_header("Expires", "0")
+    handler.end_headers()
+    handler.wfile.write(body)
+
+
+class Handler(BaseHTTPRequestHandler):
+    server_version = "Sim2RealConsole/1.1"
+
+    def log_message(self, fmt, *args):
+        if "GET /events" in (fmt % args):
+            return
+        super().log_message(fmt, *args)
+
+    def do_GET(self):
+        url = urlparse(self.path)
+        if url.path in ("/", "/index.html"):
+            return _send_static(self, Path(__file__).parent / "static" / "index.html", "text/html; charset=utf-8")
+        if url.path == "/static/app.js":
+            return _send_static(self, Path(__file__).parent / "static" / "app.js", "application/javascript; charset=utf-8")
+        if url.path == "/static/style.css":
+            return _send_static(self, Path(__file__).parent / "static" / "style.css", "text/css; charset=utf-8")
+        if url.path.startswith("/static/viewer/"):
+            viewer_file = url.path.split("/static/viewer/", 1)[1]
+            viewer_path = Path(__file__).parent / "static" / "viewer" / viewer_file
+            content_type = "text/javascript" if not viewer_file.endswith(".css") else "text/css"
+            return _send_static(self, viewer_path, content_type)
+        if url.path.startswith("/meshes/"):
+            mesh_name = url.path.split("/meshes/", 1)[1]
+            mesh_path = Path(__file__).resolve().parents[1] / "mjcf" / "meshes" / mesh_name
+            if not mesh_path.exists():
+                return self.send_error(404, f"mesh not found: {mesh_name}")
+            self.send_response(200)
+            self.send_header("Content-Type", "application/octet-stream")
+            self.send_header("Content-Length", str(mesh_path.stat().st_size))
+            self.send_header("Cache-Control", "max-age=3600")
+            self.end_headers()
+            with open(mesh_path, "rb") as file_obj:
+                while True:
+                    chunk = file_obj.read(64 * 1024)
+                    if not chunk:
+                        break
+                    self.wfile.write(chunk)
+            return
+        if url.path.startswith("/mjcf/"):
+            mjcf_name = url.path.split("/mjcf/", 1)[1]
+            mjcf_path = Path(__file__).resolve().parents[1] / "mjcf" / mjcf_name
+            if not mjcf_path.exists():
+                return self.send_error(404, f"mjcf not found: {mjcf_name}")
+            self.send_response(200)
+            self.send_header("Content-Type", "application/xml; charset=utf-8")
+            self.send_header("Content-Length", str(mjcf_path.stat().st_size))
+            self.end_headers()
+            self.wfile.write(mjcf_path.read_bytes())
+            return
+        if url.path == "/api/status":
+            return _send_json(self, 200, SESSION.get_status())
+        if url.path == "/api/debug":
+            return _send_json(self, 200, SESSION.get_debug_snapshot())
+        if url.path == "/api/logs":
+            return _send_json(self, 200, {"sessions": SESSION.list_logs()})
+        if url.path.startswith("/api/logs/"):
+            parts = url.path.split("/")
+            if len(parts) >= 5:
+                session_id = parts[3]
+                filename = parts[4]
+                file_path = Path(SESSION.cfg.get("log_dir", "logs")) / session_id / filename
+                if file_path.exists() and filename in ("state.csv", "events.jsonl"):
+                    self.send_response(200)
+                    self.send_header(
+                        "Content-Type",
+                        "text/csv" if filename.endswith("csv") else "application/json",
+                    )
+                    self.send_header("Content-Disposition", f'attachment; filename="{session_id}_{filename}"')
+                    self.send_header("Content-Length", str(file_path.stat().st_size))
+                    self.end_headers()
+                    with open(file_path, "rb") as file_obj:
+                        while True:
+                            chunk = file_obj.read(64 * 1024)
+                            if not chunk:
+                                break
+                            self.wfile.write(chunk)
+                    return
+            return self.send_error(404)
+        if url.path == "/events":
+            return self._handle_sse()
+        return self.send_error(404, self.path)
+
+    def do_POST(self):
+        url = urlparse(self.path)
+        try:
+            length = int(self.headers.get("Content-Length", "0"))
+            body = self.rfile.read(length) if length else b""
+            data = json.loads(body) if body else {}
+        except Exception as exc:
+            SESSION.note_api_error()
+            return _send_json(self, 400, {"error": f"bad body: {exc}"})
+
+        try:
+            result = self._handle_post(url.path, data)
+        except Exception as exc:
+            SESSION.note_api_error()
+            return _send_json(
+                self,
+                500,
+                {
+                    "error": f"{type(exc).__name__}: {exc}",
+                    "traceback": traceback.format_exc(),
+                },
+            )
+        if result is None:
+            return self.send_error(404)
+        return _send_json(self, 200, {"ok": True, **(result if isinstance(result, dict) else {})})
+
+    def _handle_post(self, path: str, data: dict):
+        if path == "/api/connect":
+            return {"queued": SESSION.connect(dry_run=bool(data.get("dry_run", False)))}
+        if path == "/api/disconnect":
+            return {"queued": SESSION.disconnect()}
+        if path == "/api/enable":
+            return {"queued": SESSION.enable_motors()}
+        if path == "/api/disable":
+            return {"queued": SESSION.disable_motors()}
+        if path == "/api/test_motor":
+            return {
+                "queued": SESSION.test_motor(
+                    leg=data["leg"],
+                    joint=data["joint"],
+                    delta_rad=float(data.get("delta_rad", 0.1)),
+                    kp=float(data.get("kp", 5.0)),
+                    kd=float(data.get("kd", 1.0)),
+                    duration_s=float(data.get("duration_s", 1.0)),
+                )
+            }
+        if path == "/api/calibrate_offsets":
+            return {
+                "queued": SESSION.calibrate_offsets(
+                    target_pose_name=data.get("target_pose", "stand"),
+                    samples=int(data.get("samples", 100)),
+                )
+            }
+        if path == "/api/startup":
+            return {"queued": SESSION.startup()}
+        if path == "/api/runtime/start":
+            return {"queued": SESSION.runtime_start(policy_path=data.get("policy_path"))}
+        if path == "/api/runtime/stop":
+            return {"queued": SESSION.runtime_stop()}
+        if path == "/api/cmd":
+            SESSION.set_command(
+                vx=float(data.get("vx", 0.0)),
+                vy=float(data.get("vy", 0.0)),
+                yaw=float(data.get("yaw", 0.0)),
+            )
+            return {}
+        if path == "/api/remote_takeover":
+            return {"ok": SESSION.set_remote_takeover(enabled=bool(data.get("enabled", False)))}
+        if path == "/api/estop":
+            SESSION.estop()
+            return {}
+        if path == "/api/reset_estop":
+            SESSION.reset_estop()
+            return {}
+        return None
+
+    def _handle_sse(self):
+        self.send_response(200)
+        self.send_header("Content-Type", "text/event-stream")
+        self.send_header("Cache-Control", "no-cache")
+        self.send_header("Connection", "keep-alive")
+        self.send_header("Access-Control-Allow-Origin", "*")
+        self.end_headers()
+
+        event_queue: "queue.Queue" = queue.Queue(maxsize=1024)
+        SESSION.add_listener(event_queue)
+        try:
+            initial = {"kind": "STATUS_FULL", **SESSION.get_status()}
+            self.wfile.write(f"data: {json.dumps(initial, ensure_ascii=False)}\n\n".encode())
+            self.wfile.flush()
+            last_keepalive = time.time()
+            while True:
+                try:
+                    event = event_queue.get(timeout=1.0)
+                    self.wfile.write(f"data: {json.dumps(event, ensure_ascii=False)}\n\n".encode())
+                    self.wfile.flush()
+                except queue.Empty:
+                    if time.time() - last_keepalive > 15:
+                        self.wfile.write(b": keepalive\n\n")
+                        self.wfile.flush()
+                        last_keepalive = time.time()
+        except (BrokenPipeError, ConnectionResetError):
+            pass
+        finally:
+            SESSION.remove_listener(event_queue)
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--port", type=int, default=8080)
+    parser.add_argument("--host", default="0.0.0.0")
+    parser.add_argument("--config", default=str(Path(__file__).resolve().parents[1] / "config.yaml"))
+    parser.add_argument("--dry-run", action="store_true")
+    args = parser.parse_args()
+
+    cfg_path = Path(args.config)
+    with open(cfg_path, "r", encoding="utf-8") as file_obj:
+        cfg = yaml.safe_load(file_obj)
+
+    global SESSION
+    SESSION = RobotSession(
+        cfg=cfg,
+        cfg_path=cfg_path,
+        driver_factory_real=make_real_factory(),
+        driver_factory_dry=make_dry_factory(),
+    )
+
+    def _pulse():
+        while True:
+            try:
+                SESSION._broadcast({"kind": "PULSE", **SESSION.get_status()})
+            except Exception:
+                pass
+            time.sleep(1.0)
+
+    threading.Thread(target=_pulse, daemon=True).start()
+
+    httpd = ThreadingHTTPServer((args.host, args.port), Handler)
+    print(f"\n[Web] sim2real console -> http://{args.host}:{args.port}\n")
+    try:
+        httpd.serve_forever()
+    except KeyboardInterrupt:
+        print("\n[Web] Ctrl+C received, shutting down...")
+    finally:
+        if SESSION is not None and SESSION.status.stage == "RUNTIME":
+            try:
+                SESSION.runtime_stop()
+            except Exception:
+                pass
+        try:
+            SESSION._do_disconnect()
+        except Exception:
+            pass
+        httpd.server_close()
+        os._exit(0)
+
+
+if __name__ == "__main__":
+    main()

05_software/real/sim2real_v2/web/static/app.js

@@ -0,0 +1,570 @@
+const SIM_JOINT_ORDER = [
+  ["fl", "hip_abduction"], ["fl", "hip_pitch"], ["fl", "knee"],
+  ["fr", "hip_abduction"], ["fr", "hip_pitch"], ["fr", "knee"],
+  ["rl", "hip_abduction"], ["rl", "hip_pitch"], ["rl", "knee"],
+  ["rr", "hip_abduction"], ["rr", "hip_pitch"], ["rr", "knee"],
+  ["fl", "wheel"], ["fr", "wheel"], ["rl", "wheel"], ["rr", "wheel"],
+];
+
+const $ = (id) => document.getElementById(id);
+const PLOTS = {};
+let CURRENT_STATUS = null;
+let SSE_CONN = null;
+let SSE_RECONNECT_TIMER = null;
+let LAST_RENDER_TS = 0;
+
+async function api(path, body = null) {
+  const options = { method: body ? "POST" : "GET" };
+  if (body) {
+    options.headers = { "Content-Type": "application/json" };
+    options.body = JSON.stringify(body);
+  }
+  const response = await fetch(path, options);
+  const payload = await response.json().catch(() => ({}));
+  if (!response.ok) {
+    throw new Error(payload.error || `HTTP ${response.status}`);
+  }
+  return payload;
+}
+
+function safeText(value, fallback = "--") {
+  return value === undefined || value === null || Number.isNaN(value) ? fallback : value;
+}
+
+function appendEvent(ev) {
+  const el = $("events-log");
+  if (!el) return;
+  const item = document.createElement("div");
+  let cls = "ev-name";
+  if (/ERROR|STOP|NAN/.test(ev.kind || "")) cls = "ev-stop";
+  else if (/FAULT|BRAKE/.test(ev.kind || "")) cls = "ev-fault";
+  else if (/DONE|CONNECTED|ENABLED|PRIMED/.test(ev.kind || "")) cls = "ev-ok";
+  const t = ev.t ? new Date(ev.t * 1000).toLocaleTimeString() : new Date().toLocaleTimeString();
+  const detail = Object.entries(ev)
+    .filter(([k]) => !["t", "kind"].includes(k))
+    .slice(0, 6)
+    .map(([k, v]) => `${k}=${typeof v === "number" ? v.toFixed(3) : JSON.stringify(v).slice(0, 80)}`)
+    .join(" ");
+  item.innerHTML = `<span class="ev-t">${t}</span> <span class="${cls}">${ev.kind}</span> <span style="color:#8e8e93">${detail}</span>`;
+  el.appendChild(item);
+  while (el.children.length > 300) el.removeChild(el.firstChild);
+  el.scrollTop = el.scrollHeight;
+}
+
+function setStage(stage, detail) {
+  const el = $("stage");
+  if (!el) return;
+  el.textContent = stage + (detail ? ` · ${detail}` : "");
+  el.className = "stage " + stage;
+}
+
+function setButtonEnabled(id, enabled) {
+  const el = $(id);
+  if (!el) return;
+  el.disabled = !enabled;
+}
+
+function updateButtons(status) {
+  if (!status) return;
+  const stage = status.stage || "DISCONNECTED";
+  const busy = !!status.busy;
+  const runtime = stage === "RUNTIME";
+  const connected = stage !== "DISCONNECTED" && stage !== "CONNECTING";
+  const enabled = ["ENABLED", "STARTING_UP", "STAND_HOLD", "RUNTIME"].includes(stage);
+  const canStartup = stage === "ENABLED";
+  const canRuntimeStart = stage === "STAND_HOLD";
+  const canRuntimeStop = runtime;
+  const remoteAllowed = !!status.remote_takeover_allowed;
+  const remoteActive = !!status.remote_takeover_active;
+  const runtimeUiBusy = busy && !runtime;
+
+  setButtonEnabled("btn-connect", !runtimeUiBusy && stage === "DISCONNECTED");
+  setButtonEnabled("btn-disconnect", !runtimeUiBusy && connected);
+  setButtonEnabled("btn-enable", !runtimeUiBusy && ["CONNECTED", "FAULTED"].includes(stage));
+  setButtonEnabled("btn-disable", !runtimeUiBusy && enabled);
+  setButtonEnabled("btn-startup", !runtimeUiBusy && canStartup);
+  setButtonEnabled("btn-runtime-start", !runtimeUiBusy && canRuntimeStart);
+  setButtonEnabled("btn-runtime-stop", canRuntimeStop);
+  setButtonEnabled("btn-reset-estop", !runtimeUiBusy && stage === "ESTOPPED");
+  setButtonEnabled("btn-estop", connected);
+  setButtonEnabled("btn-remote-release", runtime && remoteAllowed && !remoteActive);
+  setButtonEnabled("btn-remote-reclaim", runtime && remoteActive);
+}
+
+function renderState(state) {
+  const el = $("state-summary");
+  if (!el) return;
+  if (!state) {
+    el.innerHTML = '<div class="state-item"><span class="k">STATUS</span><span class="v">NO DATA</span></div>';
+    return;
+  }
+  const metric = (k, v, cls = "") =>
+    `<div class="state-item"><span class="k">${k}</span><span class="v ${cls}">${v}</span></div>`;
+  const safetyText = ["NORMAL", "CLIP", "BRAKE", "ESTOP"][state.safety_level || 0];
+  const guardText = ["NORMAL", "WARN", "STOP"][state.guard_level || 0] || "NORMAL";
+  const imuCls = (state.imu_age_ms || 0) > 60 ? "bad" : (state.imu_age_ms || 0) > 30 ? "warn" : "";
+  const dtCls = (state.loop_dt_ms || 0) > 25 ? "bad" : (state.loop_dt_ms || 0) > 22 ? "warn" : "";
+  const gravityZ = state.proj_gravity?.[2] ?? -1;
+  const gravityCls = gravityZ > -0.5 ? "warn" : "";
+  const rawMax = Math.max(...(state.raw || [0]).map((x) => Math.abs(x || 0)));
+  const odom = state.odom || null;
+  const odomLocal = odom?.local_pos || odom?.pos || [0, 0, 0];
+  const odomText = odom ? `${odom.type || "ODOM"} ${(odom.age_ms || 0).toFixed(0)}ms ${odomLocal[0].toFixed(2)},${odomLocal[1].toFixed(2)}` : "no odom";
+  const cmd = state.cmd || [0, 0, 0];
+  const rawCmd = state.raw_cmd || cmd;
+  const stand = state.stand_balance || CURRENT_STATUS?.diagnostics?.stand_balance || {};
+  const targetInfo = state.latest_target || CURRENT_STATUS?.diagnostics?.latest_target || {};
+  const trackingErr = Math.max(
+    ...(state.joint_pos || []).slice(0, 12).map((pos, i) => Math.abs(pos - ((state.target || [])[i] || 0))),
+    0,
+  );
+  const inputModeEl = $("input-mode");
+  if (inputModeEl) inputModeEl.textContent = CURRENT_STATUS?.input_mode || "WEB";
+  const remoteAllowedEl = $("remote-allowed");
+  if (remoteAllowedEl) remoteAllowedEl.textContent = CURRENT_STATUS?.remote_takeover_allowed ? "YES" : "NO";
+  const remoteEstopEl = $("remote-estop");
+  if (remoteEstopEl) remoteEstopEl.textContent = CURRENT_STATUS?.remote_soft_estop ? "ON" : "OFF";
+  const remotePortEl = $("remote-port");
+  if (remotePortEl) remotePortEl.textContent = CURRENT_STATUS?.remote_status?.port || "--";
+  el.innerHTML = [
+    metric("phase", safeText(state.phase, "?")),
+    metric("cmd", `${cmd.map((x) => (x || 0).toFixed(2)).join(",")}`),
+    metric("raw cmd", `${rawCmd.map((x) => (x || 0).toFixed(2)).join(",")}`),
+    metric("imu_age", `${(state.imu_age_ms || 0).toFixed(1)} ms`, imuCls),
+    metric("loop_dt", `${(state.loop_dt_ms || 0).toFixed(1)} ms`, dtCls),
+    metric("safety", safetyText, state.safety_level >= 2 ? "bad" : state.safety_level === 1 ? "warn" : ""),
+    metric("guard", guardText, state.guard_level >= 2 ? "bad" : state.guard_level === 1 ? "warn" : ""),
+    metric("holdover", String(state.holdover_total || 0)),
+    metric("raw max", rawMax.toFixed(2)),
+    metric("grav_z", gravityZ.toFixed(3), gravityCls),
+    metric("odom", odomText, odom && (odom.age_ms || 0) < 200 && !odom.jump_detected ? "" : "warn"),
+    metric("track_err", trackingErr.toFixed(3), trackingErr > 0.5 ? "bad" : trackingErr > 0.2 ? "warn" : ""),
+    metric("stand", stand.enabled ? `p ${((stand.pitch_deg) || 0).toFixed(1)} r ${((stand.roll_deg) || 0).toFixed(1)}` : "off", stand.enabled && !stand.pitch_compensation_enabled ? "warn" : ""),
+    metric("pitch_corr", `${((stand.pitch_corr) || 0).toFixed(3)} ${stand.pitch_compensation_enabled ? "on" : "off"}`),
+    metric("target_src", targetInfo.source ? `${targetInfo.source} ${(targetInfo.age_ms || 0).toFixed(0)}ms` : "--", targetInfo.age_ms > 100 ? "warn" : ""),
+  ].join("");
+}
+
+function renderDiagnostics(diag, state) {
+  if (!diag) return;
+  const setValue = (id, text, cls = "") => {
+    const el = $(id);
+    if (!el) return;
+    el.textContent = text;
+    el.className = "diag-value " + cls;
+  };
+  setValue("diag-norm", "Aligned", "success");
+  setValue("diag-latency", `${(state?.loop_dt_ms || 0).toFixed(1)} ms`, (state?.loop_dt_ms || 0) > 25 ? "danger" : (state?.loop_dt_ms || 0) > 22 ? "warning" : "success");
+  const trackErr = Math.max(
+    ...(state?.joint_pos || []).slice(0, 12).map((pos, i) => Math.abs(pos - ((state?.target || [])[i] || 0))),
+    0,
+  );
+  setValue("diag-track-err", `${trackErr.toFixed(3)} rad`, trackErr > 0.5 ? "danger" : trackErr > 0.2 ? "warning" : "success");
+  setValue("diag-runtime", diag.runtime_active ? "ACTIVE" : "IDLE", diag.runtime_active ? "success" : "warning");
+  setValue("diag-runtime-age", diag.last_runtime_age_s == null ? "--" : `${diag.last_runtime_age_s.toFixed(2)} s`, diag.last_runtime_age_s != null && diag.last_runtime_age_s > 1.0 ? "danger" : "success");
+  setValue("diag-poll-age", diag.last_poll_age_s == null ? "--" : `${diag.last_poll_age_s.toFixed(2)} s`, diag.last_poll_age_s != null && diag.last_poll_age_s > 1.0 ? "warning" : "success");
+  setValue("diag-cmd-age", diag.last_command_age_s == null ? "--" : `${diag.last_command_age_s.toFixed(2)} s`);
+  setValue("diag-poll-errors", String(diag.poll_error_count || 0), (diag.poll_error_count || 0) > 0 ? "danger" : "success");
+  setValue("diag-api-errors", String(diag.api_error_count || 0), (diag.api_error_count || 0) > 0 ? "warning" : "success");
+  setValue("diag-overruns", `${diag.runtime_overrun_count || 0} / max ${(diag.runtime_overrun_max_ms || 0).toFixed(1)} ms`, (diag.runtime_overrun_count || 0) > 0 ? "warning" : "success");
+  setValue("diag-policy-stale", `${diag.runtime_policy_stale_count || 0} / max ${(diag.runtime_policy_stale_max_ms || 0).toFixed(1)} ms`, (diag.runtime_policy_stale_count || 0) > 0 ? "warning" : "success");
+  const profile = state?.loop_profile || diag.last_loop_profile || {};
+  const slowest = Object.entries(profile)
+    .filter(([k]) => k !== "total_ms")
+    .sort((a, b) => (b[1] || 0) - (a[1] || 0))[0];
+  const totalMs = profile.total_ms ?? state?.loop_dt_ms ?? 0;
+  setValue("diag-loop-profile", slowest ? `${totalMs.toFixed(1)} ms, slow ${slowest[0]}=${(slowest[1] || 0).toFixed(1)}` : "--");
+  const targetInfo = state?.latest_target || diag.latest_target || {};
+  if (targetInfo.source) {
+    setValue("diag-target", `${targetInfo.source} age ${(targetInfo.age_ms || 0).toFixed(0)}ms d ${(targetInfo.delta_max || 0).toFixed(3)}`, (targetInfo.age_ms || 0) > 100 ? "warning" : "success");
+  } else {
+    setValue("diag-target", "--");
+  }
+  const stand = state?.stand_balance || diag.stand_balance || {};
+  if (stand.enabled) {
+    setValue(
+      "diag-stand",
+      `pitch ${((stand.pitch_deg) || 0).toFixed(2)}deg corr ${((stand.pitch_corr) || 0).toFixed(3)} ${stand.pitch_compensation_enabled ? "pitch-on" : "pitch-off"}`,
+      stand.pitch_compensation_enabled ? "warning" : "success",
+    );
+  } else {
+    setValue("diag-stand", "disabled", "warning");
+  }
+  const obsAbs = state?.obs_abs_max;
+  const rawAbs = state?.raw_abs_max;
+  const scaledAbs = state?.scaled_abs_max;
+  const signalText = obsAbs == null ? "--" : `obs ${obsAbs.toFixed(2)} raw ${(rawAbs || 0).toFixed(2)} scaled ${(scaledAbs || 0).toFixed(2)}`;
+  setValue("diag-signal", signalText, obsAbs > 80 || rawAbs > 9.5 ? "warning" : "success");
+  const freshCount = state?.motor_fresh_count;
+  const byCount = state?.motor_fresh_by_update_count;
+  const byValue = state?.motor_fresh_by_value_change;
+  setValue("diag-motor-fresh", freshCount == null ? "--" : `${freshCount}/16 (cnt ${byCount || 0}, val ${byValue || 0})`, freshCount === 16 ? "success" : freshCount >= 12 ? "warning" : "danger");
+  const odom = state?.odom;
+  if (odom) {
+    const p = odom.local_pos || odom.pos || [0, 0, 0];
+    const yaw = odom.local_yaw == null ? 0 : odom.local_yaw;
+    const jump = odom.jump_detected ? " JUMP" : "";
+    setValue("diag-odom", `${odom.type || "ODOM"} ${((odom.age_ms || 0)).toFixed(0)}ms x=${p[0].toFixed(2)} y=${p[1].toFixed(2)} yaw=${yaw.toFixed(2)}${jump}`, (odom.age_ms || 0) > 500 || odom.jump_detected ? "warning" : "success");
+  } else {
+    setValue("diag-odom", "not available", "warning");
+  }
+  setValue("diag-suppression", String(diag.zero_cmd_suppression), diag.zero_cmd_suppression ? "warning" : "success");
+  const pathEl = $("diag-policy");
+  if (pathEl) pathEl.textContent = diag.policy_path || "--";
+}
+
+function renderFault(status) {
+  const faultBox = $("fault-box");
+  const faultText = $("fault-text");
+  const traceText = $("traceback-text");
+  if (!faultBox || !faultText || !traceText) return;
+  if (!status.fault_reason && !status.last_error) {
+    faultBox.classList.add("hidden");
+    faultText.textContent = "";
+    traceText.textContent = "";
+    return;
+  }
+  faultBox.classList.remove("hidden");
+  faultText.textContent = status.fault_reason || status.last_error || "";
+  traceText.textContent = status.last_traceback || "";
+}
+
+function applyStatus(status) {
+  if (!status) return;
+  CURRENT_STATUS = { ...(CURRENT_STATUS || {}), ...status };
+  const merged = CURRENT_STATUS;
+  if (merged.stage) setStage(merged.stage, merged.detail || "");
+  if (merged.busy !== undefined && $("busy")) $("busy").textContent = merged.busy ? " [BUSY]" : "";
+  if (merged.log_dir && $("logdir")) $("logdir").textContent = merged.log_dir;
+  updateButtons(merged);
+  renderFault(merged);
+  if (merged.last_state !== undefined) {
+    const now = performance.now();
+    if (now - LAST_RENDER_TS > 80) {
+      renderState(merged.last_state);
+      renderDiagnostics(merged.diagnostics || {}, merged.last_state);
+      if (window.viewer3d && window.viewer3d._isLoaded && merged.last_state.joint_pos) {
+        window.viewer3d.updateJoints(merged.last_state.joint_pos);
+      }
+      updateMotorsGrid(merged.last_state);
+      addPlotData(merged.last_state);
+      LAST_RENDER_TS = now;
+    }
+  }
+}
+
+async function refreshDebug() {
+  try {
+    const debug = await api("/api/debug");
+    if (debug.status) {
+      applyStatus(debug.status);
+    }
+    renderDiagnostics(debug.status?.diagnostics || {}, debug.status?.last_state || null);
+    renderFault(debug.status || {});
+    const diagJson = $("debug-json");
+    if (diagJson) diagJson.textContent = JSON.stringify(debug.status?.diagnostics || {}, null, 2);
+  } catch (err) {
+    appendEvent({ kind: "DEBUG_FETCH_ERROR", error: err.message });
+  }
+}
+
+function connectSSE() {
+  if (SSE_CONN) {
+    SSE_CONN.close();
+    SSE_CONN = null;
+  }
+  if (SSE_RECONNECT_TIMER) {
+    clearTimeout(SSE_RECONNECT_TIMER);
+    SSE_RECONNECT_TIMER = null;
+  }
+  const es = new EventSource("/events");
+  SSE_CONN = es;
+  es.onmessage = (event) => {
+    const ev = JSON.parse(event.data);
+    if (ev.kind === "STATUS_FULL" || ev.kind === "PULSE" || ev.kind === "STATUS") {
+      applyStatus(ev);
+      if (ev.fault_reason) appendEvent({ t: ev.t, kind: "FAULT_REASON", reason: ev.fault_reason });
+    } else {
+      appendEvent(ev);
+    }
+  };
+  es.onerror = () => {
+    if (SSE_CONN) {
+      SSE_CONN.close();
+      SSE_CONN = null;
+    }
+    if (!SSE_RECONNECT_TIMER) {
+      SSE_RECONNECT_TIMER = setTimeout(() => {
+        SSE_RECONNECT_TIMER = null;
+        connectSSE();
+      }, 1500);
+    }
+  };
+}
+
+window.jog = async (leg, joint, dir) => {
+  const delta = parseFloat($("jt-delta").value) * dir;
+  const kp = parseFloat($("jt-kp").value);
+  const kd = parseFloat($("jt-kd").value);
+  const duration = parseFloat($("jt-dur").value);
+  try {
+    await api("/api/test_motor", { leg, joint, delta_rad: delta, kp, kd, duration_s: duration });
+    appendEvent({ kind: "JOG_SENT", leg, joint, delta });
+  } catch (err) {
+    appendEvent({ kind: "JOG_ERROR", error: err.message, leg, joint });
+  }
+};
+
+function initMotorsGrid() {
+  const grid = $("motors-grid");
+  if (!grid) return;
+  const abbr = { hip_abduction: "H_ABD", hip_pitch: "H_PIT", knee: "KNEE", wheel: "WHEEL" };
+  grid.innerHTML = SIM_JOINT_ORDER.map(([leg, joint], i) => `
+    <div class="motor-row" id="mi-${i}">
+      <span class="m-status" id="ms-${i}" title="offline">●</span>
+      <span class="name" title="${leg}_${joint}">${leg.toUpperCase()}_${abbr[joint]}</span>
+      <span class="val pos">0.00</span>
+      <span class="val vel">0.00</span>
+      <span class="val tau">0.00</span>
+      <span class="val temp">0°C</span>
+      <span class="val fault">OK</span>
+      <div class="m-jog">
+        <button class="btn-jog" onclick="window.jog('${leg}','${joint}',-1)">-</button>
+        <button class="btn-jog" onclick="window.jog('${leg}','${joint}',1)">+</button>
+      </div>
+    </div>
+  `).join("");
+}
+
+function updateMotorsGrid(state) {
+  if (!state || !state.joint_pos) return;
+  const positions = state.joint_pos;
+  const velocities = state.joint_vel || [];
+  const torques = state.joint_torque || [];
+  const stale = state.per_motor_stale || [];
+  const temps = state.motor_temperatures || [];
+  const faults = state.motor_fault_codes || [];
+  for (let i = 0; i < 16; i += 1) {
+    const row = $("mi-" + i);
+    if (!row) continue;
+    const dot = $("ms-" + i);
+    if (dot) {
+      const count = stale[i] ?? 99;
+      if (count <= 0) {
+        dot.style.color = "#4ade80";
+        dot.title = "online";
+      } else if (count < 5) {
+        dot.style.color = "#facc15";
+        dot.title = `stale(${count})`;
+      } else {
+        dot.style.color = "#ef4444";
+        dot.title = `offline(${count})`;
+      }
+    }
+    row.children[2].textContent = (positions[i] || 0).toFixed(2);
+    row.children[3].textContent = (velocities[i] || 0).toFixed(2);
+    const tau = torques[i] || 0;
+    row.children[4].textContent = tau.toFixed(2);
+    row.children[4].style.color = Math.abs(tau) > 16.0 ? "var(--color-danger)" : "";
+    row.children[4].style.fontWeight = Math.abs(tau) > 16.0 ? "bold" : "";
+
+    const temp = temps[i] ?? 0.0;
+    row.children[5].textContent = temp.toFixed(0) + "°C";
+    row.children[5].style.color = temp > 60.0 ? "#ff453a" : temp > 45.0 ? "#ffd60a" : "";
+    row.children[5].style.fontWeight = temp > 45.0 ? "bold" : "";
+
+    const fault = faults[i] ?? 0;
+    if (fault === 0) {
+      row.children[6].textContent = "OK";
+      row.children[6].style.color = "#30d158";
+      row.children[6].style.fontWeight = "";
+    } else {
+      row.children[6].textContent = "E" + fault.toString(16).toUpperCase();
+      row.children[6].style.color = "#ff453a";
+      row.children[6].style.fontWeight = "bold";
+    }
+  }
+}
+
+function initPlots() {
+  const colors12 = ["#ff453a", "#ff9f0a", "#ffd60a", "#32ade6", "#0a84ff", "#5e5ce6", "#ff375f", "#bf5af2", "#30d158", "#66d4cf", "#8e8e93", "#c7c7cc"];
+  const specs = [
+    { id: "plot-pos", title: "Leg Pos (12)", nCh: 12, colors: colors12 },
+    { id: "plot-vel", title: "Wheel Vel (4)", nCh: 4, colors: ["#ff453a", "#32ade6", "#30d158", "#ffd60a"] },
+    { id: "plot-imu", title: "IMU (gyro+gz)", nCh: 4, colors: ["#ff453a", "#30d158", "#0a84ff", "#ffd60a"] },
+    { id: "plot-diag", title: "Diag (dt+age)", nCh: 2, colors: ["#ff453a", "#30d158"] },
+  ];
+  const maxPts = 150;
+  specs.forEach((spec) => {
+    const canvas = $(spec.id);
+    if (!canvas) return;
+    canvas.width = canvas.parentElement.clientWidth;
+    canvas.height = 80;
+    PLOTS[spec.id] = {
+      ctx: canvas.getContext("2d"),
+      title: spec.title,
+      nCh: spec.nCh,
+      colors: spec.colors,
+      data: Array.from({ length: spec.nCh }, () => new Array(maxPts).fill(0)),
+      yMin: Array(spec.nCh).fill(Infinity),
+      yMax: Array(spec.nCh).fill(-Infinity),
+      maxPts,
+    };
+  });
+}
+
+function addPlotData(state) {
+  if (!state) return;
+  const channels = [
+    ["plot-pos", (state.joint_pos || []).slice(0, 12)],
+    ["plot-vel", (state.joint_vel || []).slice(12, 16)],
+    ["plot-imu", [...(state.gyro || [0, 0, 0]), (state.proj_gravity || [0, 0, -1])[2]]],
+    ["plot-diag", [state.loop_dt_ms || 0, state.imu_age_ms || 0]],
+  ];
+  channels.forEach(([id, values]) => {
+    const plot = PLOTS[id];
+    if (!plot) return;
+    for (let i = 0; i < plot.nCh && i < values.length; i += 1) {
+      const data = plot.data[i];
+      data.push(values[i]);
+      if (data.length > plot.maxPts) data.shift();
+      if (values[i] < plot.yMin[i]) plot.yMin[i] = values[i];
+      if (values[i] > plot.yMax[i]) plot.yMax[i] = values[i];
+    }
+    drawPlot(plot);
+  });
+}
+
+function drawPlot(plot) {
+  const { ctx, data, colors, yMin, yMax, title, maxPts } = plot;
+  const canvas = ctx.canvas;
+  const width = canvas.width;
+  const height = canvas.height;
+  ctx.clearRect(0, 0, width, height);
+  ctx.fillStyle = "rgba(255,255,255,0.5)";
+  ctx.font = "10px monospace";
+  ctx.fillText(title, 4, 12);
+  const margin = { l: 30, r: 4, t: 16, b: 4 };
+  const plotW = width - margin.l - margin.r;
+  const plotH = height - margin.t - margin.b;
+  if (plotW <= 0 || plotH <= 0) return;
+  for (let i = 0; i < data.length; i += 1) {
+    if (yMin[i] === Infinity) {
+      yMin[i] = -1;
+      yMax[i] = 1;
+    }
+    const curMin = Math.min(...data[i]);
+    const curMax = Math.max(...data[i]);
+    yMin[i] = yMin[i] * 0.99 + curMin * 0.01;
+    yMax[i] = yMax[i] * 0.99 + curMax * 0.01;
+  }
+  const globalMin = Math.min(...yMin);
+  const globalMax = Math.max(...yMax);
+  const range = globalMax - globalMin || 1;
+  data.forEach((series, i) => {
+    if (series.length < 2) return;
+    ctx.strokeStyle = colors[i] || "#8e8e93";
+    ctx.lineWidth = 1.0;
+    ctx.beginPath();
+    series.forEach((value, j) => {
+      const x = margin.l + (j / maxPts) * plotW;
+      const y = margin.t + plotH - ((value - globalMin) / range) * plotH;
+      if (j === 0) ctx.moveTo(x, y);
+      else ctx.lineTo(x, y);
+    });
+    ctx.stroke();
+  });
+  ctx.fillStyle = "rgba(255,255,255,0.4)";
+  ctx.font = "9px monospace";
+  ctx.fillText(globalMax.toFixed(1), 2, margin.t + 8);
+  ctx.fillText(globalMin.toFixed(1), 2, margin.t + plotH - 2);
+}
+
+async function refreshLogs() {
+  try {
+    const result = await api("/api/logs");
+    const tbody = document.querySelector("#logs-table tbody");
+    if (!tbody) return;
+    tbody.innerHTML = result.sessions.map((s) => `
+      <tr>
+        <td style="font-family:monospace">${s.id.slice(-8)}</td>
+        <td>${s.state_csv ? `<a href="/api/logs/${s.id}/state.csv" download>CSV</a>` : "—"}</td>
+        <td>${s.events_jsonl ? `<a href="/api/logs/${s.id}/events.jsonl" download>JSONL</a>` : "—"}</td>
+        <td>${s.size_kb} KB</td>
+      </tr>
+    `).join("");
+  } catch (err) {
+    appendEvent({ kind: "LOG_REFRESH_ERROR", error: err.message });
+  }
+}
+
+let cmdTimer = null;
+function sendCmd() {
+  if (cmdTimer) return;
+  cmdTimer = setTimeout(() => {
+    cmdTimer = null;
+    api("/api/cmd", {
+      vx: parseFloat($("cmd-vx").value),
+      vy: parseFloat($("cmd-vy").value),
+      yaw: parseFloat($("cmd-yaw").value),
+    }).catch((err) => appendEvent({ kind: "CMD_ERROR", error: err.message }));
+  }, 50);
+}
+
+function bind() {
+  $("btn-connect").onclick = () => api("/api/connect", { dry_run: $("dry-run").checked }).catch((err) => appendEvent({ kind: "CONNECT_ERROR", error: err.message }));
+  $("btn-disconnect").onclick = () => api("/api/disconnect", {}).catch((err) => appendEvent({ kind: "DISCONNECT_ERROR", error: err.message }));
+  $("btn-enable").onclick = () => api("/api/enable", {}).catch((err) => appendEvent({ kind: "ENABLE_ERROR", error: err.message }));
+  $("btn-disable").onclick = () => api("/api/disable", {}).catch((err) => appendEvent({ kind: "DISABLE_ERROR", error: err.message }));
+  $("btn-startup").onclick = () => api("/api/startup", {}).catch((err) => appendEvent({ kind: "STARTUP_ERROR", error: err.message }));
+  $("btn-runtime-start").onclick = () => api("/api/runtime/start", { policy_path: $("policy-path").value || null }).catch((err) => appendEvent({ kind: "RUNTIME_START_ERROR", error: err.message }));
+  $("btn-runtime-stop").onclick = () => api("/api/runtime/stop", {}).catch((err) => appendEvent({ kind: "RUNTIME_STOP_ERROR", error: err.message }));
+  $("btn-remote-release").onclick = () => api("/api/remote_takeover", { enabled: true }).catch((err) => appendEvent({ kind: "REMOTE_TAKEOVER_ENABLE_ERROR", error: err.message }));
+  $("btn-remote-reclaim").onclick = () => api("/api/remote_takeover", { enabled: false }).catch((err) => appendEvent({ kind: "REMOTE_TAKEOVER_DISABLE_ERROR", error: err.message }));
+  $("btn-estop").onclick = () => api("/api/estop", {}).catch((err) => appendEvent({ kind: "ESTOP_ERROR", error: err.message }));
+  $("btn-reset-estop").onclick = () => api("/api/reset_estop", {}).catch((err) => appendEvent({ kind: "RESET_ESTOP_ERROR", error: err.message }));
+  $("btn-refresh-debug").onclick = () => refreshDebug();
+
+  ["vx", "vy", "yaw"].forEach((key) => {
+    const el = $("cmd-" + key);
+    el.oninput = () => {
+      $("cmd-" + key + "-v").textContent = parseFloat(el.value).toFixed(2);
+      sendCmd();
+    };
+  });
+  $("btn-cmd-zero").onclick = () => {
+    ["vx", "vy", "yaw"].forEach((key) => {
+      const el = $("cmd-" + key);
+      el.value = 0;
+      $("cmd-" + key + "-v").textContent = "0.00";
+    });
+    sendCmd();
+  };
+
+  const jtSlider = $("jt-delta");
+  jtSlider.oninput = () => { $("jt-delta-v").textContent = parseFloat(jtSlider.value).toFixed(2); };
+
+  $("btn-show-logs").onclick = () => {
+    refreshLogs();
+    $("logs-modal").classList.remove("hidden");
+  };
+  $("btn-close-logs").onclick = () => $("logs-modal").classList.add("hidden");
+}
+
+window.addEventListener("DOMContentLoaded", () => {
+  initMotorsGrid();
+  bind();
+  initPlots();
+  connectSSE();
+  refreshLogs();
+  refreshDebug();
+  updateButtons({ stage: "DISCONNECTED", busy: false });
+  setInterval(refreshLogs, 10000);
+  setInterval(refreshDebug, 5000);
+});
+
+window.addEventListener("resize", () => {
+  Object.values(PLOTS).forEach((plot) => {
+    plot.ctx.canvas.width = plot.ctx.canvas.parentElement.clientWidth;
+  });
+});

05_software/real/sim2real_v2/web/static/index.html

@@ -0,0 +1,208 @@
+<!doctype html>
+<html lang="zh-CN" data-theme="dark">
+<head>
+  <meta charset="utf-8">
+  <meta name="viewport" content="width=device-width,initial-scale=1">
+  <title>sim2real 控制台</title>
+  <link rel="stylesheet" href="/static/style.css">
+  <script type="importmap">
+  {
+    "imports": {
+      "three": "https://unpkg.com/three@0.160.0/build/three.module.js",
+      "three/examples/jsm/controls/OrbitControls.js": "https://unpkg.com/three@0.160.0/examples/jsm/controls/OrbitControls.js",
+      "three/examples/jsm/loaders/STLLoader.js": "https://unpkg.com/three@0.160.0/examples/jsm/loaders/STLLoader.js"
+    }
+  }
+  </script>
+</head>
+<body>
+  <div id="canvas-container">
+    <canvas id="viewer-canvas"></canvas>
+    <div id="viewer-status" class="viewer-overlay">加载中...</div>
+  </div>
+
+  <header class="glass-panel top-bar">
+    <div class="top-bar-left">
+      <h1>sim2real</h1>
+      <span class="stage" id="stage">DISCONNECTED</span>
+      <span id="busy" class="busy-indicator"></span>
+      <span id="logdir" class="logdir-indicator"></span>
+    </div>
+    <div class="top-bar-center">
+      <label class="toggle-switch">
+        <input type="checkbox" id="dry-run">
+        <span class="slider"></span>
+        <span class="label">Dry-run</span>
+      </label>
+      <button class="btn btn-primary" id="btn-connect">连接硬件</button>
+      <button class="btn btn-secondary" id="btn-disconnect">断开连接</button>
+      <div class="divider"></div>
+      <button class="btn btn-success" id="btn-enable">使能电机</button>
+      <button class="btn btn-warning" id="btn-disable">失能电机</button>
+    </div>
+    <div class="top-bar-right">
+      <button id="btn-reset-camera" class="btn btn-secondary btn-icon" title="重置视角">⟳</button>
+      <button id="btn-estop" class="btn btn-danger">急停</button>
+      <button id="btn-reset-estop" class="btn btn-secondary">解除急停</button>
+    </div>
+  </header>
+
+  <div class="glass-panel side-panel left-panel">
+    <div class="panel-section">
+      <h2 class="panel-title">控制流程</h2>
+      <div class="btn-group-vertical">
+        <button class="btn btn-action" id="btn-startup">一键起立</button>
+        <div class="runtime-group">
+          <input type="text" id="policy-path" class="glass-input" placeholder="策略路径，留空则使用默认 rough">
+          <div class="btn-row">
+            <button class="btn btn-success flex-1" id="btn-runtime-start">启动策略</button>
+            <button class="btn btn-danger flex-1" id="btn-runtime-stop">停止策略</button>
+          </div>
+          <div class="btn-row mt-2">
+            <button class="btn btn-secondary flex-1" id="btn-remote-release">放开遥控</button>
+            <button class="btn btn-secondary flex-1" id="btn-remote-reclaim">收回遥控</button>
+          </div>
+        </div>
+      </div>
+    </div>
+
+    <div class="panel-section state-section">
+      <h2 class="panel-title">实时状态</h2>
+      <div id="state-summary" class="state-grid"></div>
+    </div>
+
+    <div class="panel-section flex-1">
+      <div class="panel-title-row">
+        <h2 class="panel-title">Motors / Jog Test</h2>
+        <span class="hint" style="font-size:10px; color:var(--text-tertiary)">POS | VEL | TAU | TEMP | FAULT</span>
+      </div>
+      <div class="control-row mt-2 mb-2">
+        <span class="label">Kp</span><input type="number" id="jt-kp" class="glass-input mini" value="5">
+        <span class="label">Kd</span><input type="number" id="jt-kd" class="glass-input mini" value="1">
+        <span class="label">Time</span><input type="number" id="jt-dur" class="glass-input mini" value="1.0">
+        <span class="label">Δ(rad)</span><input type="number" id="jt-delta" class="glass-input mini" value="0.1" step="0.05">
+        <span id="jt-delta-v" class="slider-val">0.10</span>
+      </div>
+      <div id="motors-grid" class="motors-grid-list"></div>
+    </div>
+  </div>
+
+  <div class="glass-panel side-panel right-panel">
+    <div class="panel-section">
+      <div class="panel-title-row">
+        <h2 class="panel-title">Diagnostics</h2>
+        <button class="btn btn-secondary" id="btn-refresh-debug">刷新</button>
+      </div>
+      <div class="diag-row"><span class="diag-label">Obs Normalization</span><span class="diag-value success" id="diag-norm">Aligned</span></div>
+      <div class="diag-row"><span class="diag-label">Control Latency</span><span class="diag-value" id="diag-latency">-- ms</span></div>
+      <div class="diag-row"><span class="diag-label">Tracking Error</span><span class="diag-value" id="diag-track-err">-- rad</span></div>
+      <div class="diag-row"><span class="diag-label">Runtime</span><span class="diag-value" id="diag-runtime">--</span></div>
+      <div class="diag-row"><span class="diag-label">Runtime Age</span><span class="diag-value" id="diag-runtime-age">--</span></div>
+      <div class="diag-row"><span class="diag-label">Poll Age</span><span class="diag-value" id="diag-poll-age">--</span></div>
+      <div class="diag-row"><span class="diag-label">Cmd Age</span><span class="diag-value" id="diag-cmd-age">--</span></div>
+      <div class="diag-row"><span class="diag-label">Poll Errors</span><span class="diag-value" id="diag-poll-errors">0</span></div>
+      <div class="diag-row"><span class="diag-label">API Errors</span><span class="diag-value" id="diag-api-errors">0</span></div>
+      <div class="diag-row"><span class="diag-label">Loop Overruns</span><span class="diag-value" id="diag-overruns">0</span></div>
+      <div class="diag-row"><span class="diag-label">Policy Stale</span><span class="diag-value" id="diag-policy-stale">0</span></div>
+      <div class="diag-row"><span class="diag-label">Loop Profile</span><span class="diag-value" id="diag-loop-profile">--</span></div>
+      <div class="diag-row"><span class="diag-label">Latest Target</span><span class="diag-value" id="diag-target">--</span></div>
+      <div class="diag-row"><span class="diag-label">Stand Balance</span><span class="diag-value" id="diag-stand">--</span></div>
+      <div class="diag-row"><span class="diag-label">Obs / Action</span><span class="diag-value" id="diag-signal">--</span></div>
+      <div class="diag-row"><span class="diag-label">Motor Fresh</span><span class="diag-value" id="diag-motor-fresh">--</span></div>
+      <div class="diag-row"><span class="diag-label">Odin Odom</span><span class="diag-value" id="diag-odom">--</span></div>
+      <div class="diag-row"><span class="diag-label">Zero-Cmd Suppression</span><span class="diag-value" id="diag-suppression">--</span></div>
+      <div class="diag-row"><span class="diag-label">Policy</span><span class="diag-value" id="diag-policy">--</span></div>
+    </div>
+
+    <div id="fault-box" class="panel-section hidden">
+      <h2 class="panel-title">Fault</h2>
+      <div id="fault-text" class="diag-value danger"></div>
+      <pre id="traceback-text" style="white-space:pre-wrap; font-size:11px; max-height:160px; overflow:auto;"></pre>
+    </div>
+
+    <div class="panel-section">
+      <h2 class="panel-title">Command</h2>
+      <div class="state-grid compact-grid">
+        <div class="state-item"><span class="k">输入源</span><span class="v" id="input-mode">WEB</span></div>
+        <div class="state-item"><span class="k">遥控可接管</span><span class="v" id="remote-allowed">NO</span></div>
+        <div class="state-item"><span class="k">遥控软急停</span><span class="v" id="remote-estop">OFF</span></div>
+        <div class="state-item"><span class="k">遥控端口</span><span class="v" id="remote-port">--</span></div>
+      </div>
+      <div class="slider-group">
+        <div class="slider-row">
+          <span class="slider-label">vx</span>
+          <input type="range" id="cmd-vx" class="glass-slider" min="-1" max="1" step="0.05" value="0">
+          <span class="slider-val" id="cmd-vx-v">0.00</span>
+        </div>
+        <div class="slider-row">
+          <span class="slider-label">vy</span>
+          <input type="range" id="cmd-vy" class="glass-slider" min="-0.5" max="0.5" step="0.05" value="0">
+          <span class="slider-val" id="cmd-vy-v">0.00</span>
+        </div>
+        <div class="slider-row">
+          <span class="slider-label">yaw</span>
+          <input type="range" id="cmd-yaw" class="glass-slider" min="-1" max="1" step="0.05" value="0">
+          <span class="slider-val" id="cmd-yaw-v">0.00</span>
+        </div>
+        <button class="btn btn-secondary full-width mt-2" id="btn-cmd-zero">速度归零</button>
+      </div>
+    </div>
+
+    <div class="panel-section log-section flex-1">
+      <h2 class="panel-title">事件流</h2>
+      <div id="events-log" class="log"></div>
+    </div>
+
+    <div class="panel-section">
+      <h2 class="panel-title">Debug JSON</h2>
+      <pre id="debug-json" style="white-space:pre-wrap; font-size:11px; max-height:160px; overflow:auto;"></pre>
+    </div>
+
+    <div class="panel-section plots-section">
+      <h2 class="panel-title">实时曲线</h2>
+      <div class="plots-container" style="max-height: 200px;">
+        <canvas id="plot-pos"></canvas>
+        <canvas id="plot-vel"></canvas>
+        <canvas id="plot-imu"></canvas>
+        <canvas id="plot-diag"></canvas>
+      </div>
+    </div>
+  </div>
+
+  <div id="logs-modal" class="glass-modal hidden">
+    <div class="glass-panel modal-content">
+      <div class="modal-header">
+        <h2 class="panel-title">日志下载</h2>
+        <button class="btn-close" id="btn-close-logs">×</button>
+      </div>
+      <div class="modal-body">
+        <table id="logs-table">
+          <thead><tr><th>会话 ID</th><th>state.csv</th><th>events.jsonl</th><th>大小</th></tr></thead>
+          <tbody></tbody>
+        </table>
+      </div>
+    </div>
+  </div>
+
+  <button id="btn-show-logs" class="btn btn-secondary floating-btn" title="查看日志文件">🗂</button>
+  <span id="viewer-joint-count" class="viewer-count-indicator"></span>
+
+  <script type="module">
+    import { RobotViewer3D } from '/static/viewer/RobotViewer3D.js';
+    window.RobotViewer3D = RobotViewer3D;
+    const canvas = document.getElementById('viewer-canvas');
+    window.viewer3d = new RobotViewer3D(canvas, { meshBaseUrl: '/meshes/' });
+    try {
+      await window.viewer3d.load();
+      document.getElementById('viewer-status').textContent = '';
+      document.getElementById('viewer-joint-count').textContent = window.viewer3d.jointMap.size + ' joints';
+    } catch (error) {
+      document.getElementById('viewer-status').textContent = '3D 加载失败: ' + error.message;
+      console.error(error);
+    }
+    document.getElementById('btn-reset-camera').onclick = () => window.viewer3d.resetCamera();
+    window.addEventListener('resize', () => window.viewer3d.resize());
+  </script>
+  <script src="/static/app.js"></script>
+</body>
+</html>

05_software/real/sim2real_v2/web/static/style.css

@@ -0,0 +1,399 @@
+/* Apple Glass Design System for sim2real */
+:root {
+  --bg-primary: #000000;
+  --glass-bg: rgba(20, 20, 22, 0.65);
+  --glass-border: rgba(255, 255, 255, 0.12);
+  --glass-shadow: 0 8px 32px rgba(0, 0, 0, 0.25);
+  --text-primary: #ffffff;
+  --text-secondary: #ebebf5;
+  --text-tertiary: #8e8e93;
+  --accent: #0a84ff;
+  --accent-hover: #409cff;
+  --success: #30d158;
+  --warning: #ffd60a;
+  --danger: #ff453a;
+  --blur-amount: 24px;
+  --saturation: 180%;
+  --spring: cubic-bezier(0.4, 0, 0.2, 1);
+  --panel-radius: 16px;
+  --font-family: -apple-system, BlinkMacSystemFont, 'SF Pro Display', 'PingFang SC', sans-serif;
+}
+
+[data-theme="light"] {
+  --bg-primary: #f5f5f7;
+  --glass-bg: rgba(245, 245, 245, 0.75);
+  --glass-border: rgba(0, 0, 0, 0.15);
+  --glass-shadow: 0 8px 32px rgba(0, 0, 0, 0.12);
+  --text-primary: #1d1d1f;
+  --text-secondary: #424245;
+  --text-tertiary: #86868b;
+}
+
+* {
+  box-sizing: border-box;
+  margin: 0;
+  padding: 0;
+}
+
+body {
+  font-family: var(--font-family);
+  overflow: hidden;
+  background: var(--bg-primary);
+  color: var(--text-primary);
+  -webkit-font-smoothing: antialiased;
+  transition: background 0.3s var(--spring);
+}
+
+/* 3D Canvas Background */
+#canvas-container {
+  position: fixed;
+  top: 0; left: 0; right: 0; bottom: 0;
+  z-index: 0;
+  background: radial-gradient(circle at center, #1a1a24 0%, #000000 100%);
+}
+#viewer-canvas {
+  width: 100%;
+  height: 100%;
+  display: block;
+  cursor: grab;
+}
+#viewer-canvas:active {
+  cursor: grabbing;
+}
+.viewer-overlay {
+  position: absolute;
+  top: 50%; left: 50%;
+  transform: translate(-50%, -50%);
+  color: var(--text-tertiary);
+  font-size: 14px;
+  pointer-events: none;
+}
+
+.viewer-count-indicator {
+  position: fixed;
+  bottom: 20px;
+  right: 20px;
+  font-size: 11px;
+  color: var(--text-tertiary);
+  z-index: 10;
+  font-family: monospace;
+}
+
+/* Glass Panels */
+.glass-panel {
+  background: var(--glass-bg);
+  backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
+  -webkit-backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
+  border: 0.5px solid var(--glass-border);
+  box-shadow: var(--glass-shadow);
+  z-index: 50;
+}
+
+/* Top Bar */
+.top-bar {
+  position: fixed;
+  top: 16px;
+  left: 50%;
+  transform: translateX(-50%);
+  display: flex;
+  align-items: center;
+  justify-content: space-between;
+  padding: 8px 16px;
+  border-radius: 24px;
+  width: 96%;
+  max-width: 1400px;
+  gap: 16px;
+}
+
+.top-bar-left, .top-bar-center, .top-bar-right {
+  display: flex;
+  align-items: center;
+  gap: 12px;
+}
+.top-bar-center {
+  flex: 1;
+  justify-content: center;
+}
+
+.top-bar h1 {
+  font-size: 16px;
+  font-weight: 600;
+  margin: 0;
+  background: -webkit-linear-gradient(45deg, #fff, #8e8e93);
+  -webkit-background-clip: text;
+  -webkit-text-fill-color: transparent;
+}
+
+.divider {
+  width: 1px;
+  height: 24px;
+  background: var(--glass-border);
+  margin: 0 4px;
+}
+
+/* Side Panels */
+.side-panel {
+  position: fixed;
+  top: 80px;
+  bottom: 20px;
+  width: 340px;
+  border-radius: var(--panel-radius);
+  display: flex;
+  flex-direction: column;
+  overflow: hidden;
+}
+
+.left-panel { left: 2%; }
+.right-panel { right: 2%; }
+
+.panel-section {
+  padding: 16px;
+  border-bottom: 0.5px solid var(--glass-border);
+  display: flex;
+  flex-direction: column;
+}
+.panel-section:last-child {
+  border-bottom: none;
+}
+.flex-1 { flex: 1; min-height: 0; }
+
+.panel-title {
+  font-size: 12px;
+  font-weight: 700;
+  color: var(--text-tertiary);
+  text-transform: uppercase;
+  letter-spacing: 0.5px;
+  margin-bottom: 12px;
+}
+.panel-title-row {
+  display: flex; justify-content: space-between; align-items: center;
+}
+
+/* Typography & Badges */
+.stage {
+  padding: 4px 10px;
+  border-radius: 12px;
+  font-size: 11px;
+  font-weight: 700;
+  text-transform: uppercase;
+  letter-spacing: 0.5px;
+  background: rgba(255,255,255,0.1);
+  color: var(--text-secondary);
+}
+.stage.DISCONNECTED { background: rgba(142,142,147,0.3); }
+.stage.CONNECTED { background: rgba(10,132,255,0.3); color: #82c4ff; }
+.stage.ENABLED { background: rgba(48,209,88,0.3); color: #8deda7; }
+.stage.FAULTED { background: rgba(255,69,58,0.3); color: #ff8b86; }
+.stage.ESTOPPED { background: rgba(255,69,58,0.5); color: #ff8b86; box-shadow: 0 0 8px rgba(255,69,58,0.4); }
+
+/* Buttons */
+.btn {
+  background: rgba(255, 255, 255, 0.08);
+  border: 1px solid rgba(255, 255, 255, 0.1);
+  border-radius: 8px;
+  color: var(--text-primary);
+  font-size: 12px;
+  font-weight: 500;
+  padding: 6px 12px;
+  cursor: pointer;
+  transition: all 0.2s var(--spring);
+  font-family: inherit;
+  display: inline-flex;
+  align-items: center;
+  justify-content: center;
+}
+.btn:hover:not(:disabled) {
+  background: rgba(255, 255, 255, 0.15);
+  transform: translateY(-1px);
+}
+.btn:active:not(:disabled) {
+  transform: translateY(1px);
+}
+.btn:disabled {
+  opacity: 0.5;
+  cursor: not-allowed;
+}
+.btn-primary { background: var(--accent); border-color: var(--accent); color: white; }
+.btn-primary:hover:not(:disabled) { background: var(--accent-hover); }
+.btn-success { background: rgba(48,209,88,0.8); border-color: transparent; color: white; }
+.btn-warning { background: rgba(255,214,10,0.8); border-color: transparent; color: black; }
+.btn-danger { background: rgba(255,69,58,0.8); border-color: transparent; color: white; }
+.btn-icon { width: 28px; height: 28px; padding: 0; border-radius: 50%; }
+.full-width { width: 100%; }
+.mt-2 { margin-top: 8px; }
+
+.btn-group-vertical {
+  display: flex; flex-direction: column; gap: 8px;
+}
+.btn-row {
+  display: flex; gap: 8px;
+}
+
+/* Inputs */
+.glass-input, .glass-select {
+  background: rgba(0,0,0,0.2);
+  border: 1px solid var(--glass-border);
+  border-radius: 6px;
+  padding: 6px 10px;
+  color: var(--text-primary);
+  font-size: 12px;
+  font-family: inherit;
+  outline: none;
+  transition: border-color 0.2s;
+}
+.glass-input:focus, .glass-select:focus {
+  border-color: var(--accent);
+}
+.glass-input.small { width: 60px; }
+.glass-input.mini { width: 45px; padding: 4px 6px; }
+
+.control-row {
+  display: flex; align-items: center; gap: 8px; margin-bottom: 8px;
+}
+.label { font-size: 11px; color: var(--text-tertiary); }
+
+.compact-grid {
+  margin-bottom: 10px;
+}
+
+.toggle-switch {
+  display: flex; align-items: center; gap: 8px; cursor: pointer;
+}
+.toggle-switch input { display: none; }
+.toggle-switch .slider {
+  position: relative; width: 32px; height: 18px;
+  background: rgba(255,255,255,0.2); border-radius: 18px;
+  transition: 0.3s;
+}
+.toggle-switch .slider::before {
+  content: ""; position: absolute;
+  width: 14px; height: 14px; border-radius: 50%;
+  background: white; top: 2px; left: 2px; transition: 0.3s;
+}
+.toggle-switch input:checked + .slider { background: var(--accent); }
+.toggle-switch input:checked + .slider::before { transform: translateX(14px); }
+.toggle-switch .label { font-size: 12px; color: var(--text-secondary); }
+
+/* Range Sliders */
+.slider-row {
+  display: flex; align-items: center; gap: 8px; margin-bottom: 8px;
+}
+.slider-label {
+  font-size: 12px; width: 30px; color: var(--text-secondary); font-family: monospace;
+}
+.slider-val {
+  font-size: 12px; width: 36px; text-align: right; color: var(--accent); font-family: monospace;
+}
+.glass-slider {
+  flex: 1; -webkit-appearance: none; height: 4px; border-radius: 2px;
+  background: rgba(255,255,255,0.2); outline: none;
+}
+.glass-slider::-webkit-slider-thumb {
+  -webkit-appearance: none; width: 14px; height: 14px;
+  border-radius: 50%; background: white; cursor: pointer;
+  box-shadow: 0 2px 4px rgba(0,0,0,0.5);
+}
+.glass-slider:active::-webkit-slider-thumb { transform: scale(1.2); }
+
+/* Motors List (Jog & Status) */
+.motors-grid-list {
+  display: flex; flex-direction: column; gap: 2px; overflow-y: auto; padding-right: 4px;
+}
+.motor-row {
+  display: flex; align-items: center; justify-content: space-between;
+  padding: 2px 6px; background: rgba(0,0,0,0.25); border-radius: 6px;
+  border: 1px solid rgba(255,255,255,0.03);
+}
+.motor-row .name { font-size: 11px; color: var(--text-secondary); width: 65px; font-weight: 500; font-family: monospace; }
+.motor-row .m-status { font-size: 8px; color: #ef4444; flex-shrink: 0; width: 12px; text-align: center; transition: color 0.3s; }
+.motor-row .val { font-size: 10px; font-family: monospace; text-align: right; width: 35px; }
+.motor-row .val.pos { color: #0a84ff; }
+.motor-row .val.vel { color: #30d158; }
+.motor-row .val.tau { color: #ff9f0a; }
+.motor-row .val.temp { color: var(--text-secondary); width: 32px; }
+.motor-row .val.fault { width: 38px; text-align: center; font-weight: 500; font-family: monospace; }
+
+.m-jog { display: flex; gap: 2px; }
+.btn-jog { 
+  background: rgba(255,255,255,0.1); border: none; border-radius: 4px; 
+  color: white; font-family: monospace; font-size: 11px; padding: 2px 6px;
+  cursor: pointer; min-width: 24px; text-align: center;
+}
+.btn-jog:hover { background: rgba(255,255,255,0.25); }
+
+/* State Grid */
+.state-grid {
+  display: grid; grid-template-columns: 1fr 1fr; gap: 6px;
+  overflow-y: auto;
+}
+.state-item {
+  display: flex; justify-content: space-between; align-items: center;
+  padding: 4px 6px; background: rgba(0,0,0,0.2); border-radius: 4px;
+}
+.state-item .k { font-size: 10px; color: var(--text-tertiary); text-transform: uppercase; }
+.state-item .v { font-size: 11px; font-family: monospace; color: var(--text-primary); }
+.state-item .v.warn { color: var(--warning); }
+.state-item .v.bad { color: var(--danger); }
+
+/* ==== Plots & Logs ==== */
+.log-section { flex: 1; display: flex; flex-direction: column; min-height: 150px; }
+.log {
+  flex: 1; background: rgba(0,0,0,0.4); border-radius: 6px; padding: 8px;
+  font-family: monospace; font-size: 11px; overflow-y: auto; color: var(--text-secondary);
+  border: 1px solid rgba(255,255,255,0.05);
+}
+.log div { margin-bottom: 2px; line-height: 1.3; }
+.plots-section { margin-top: auto; }
+.plots-container {
+  display: flex; flex-direction: column; gap: 4px; overflow-y: auto; padding-right: 4px;
+}
+.plots-container canvas {
+  width: 100% !important; height: 50px !important; background: rgba(0,0,0,0.2); border-radius: 4px;
+}
+
+/* ==== Diagnostics ==== */
+.diag-row {
+  display: flex; justify-content: space-between; align-items: center;
+  padding: 4px 6px; background: rgba(0,0,0,0.2); border-radius: 4px;
+  margin-bottom: 4px; font-family: monospace; font-size: 12px;
+}
+.diag-label { color: var(--text-secondary); }
+.diag-value { color: var(--text-primary); font-weight: bold; }
+.diag-value.success { color: var(--color-success); }
+.diag-value.warning { color: var(--color-warning); }
+.diag-value.danger { color: var(--color-danger); }
+.plots-container::-webkit-scrollbar { width: 4px; }
+.plots-container::-webkit-scrollbar-thumb { background: rgba(255,255,255,0.2); border-radius: 2px; }
+
+/* Modal & Floating BTN */
+.floating-btn {
+  position: fixed; bottom: 20px; left: 20px; width: 40px; height: 40px;
+  border-radius: 50%; font-size: 18px; z-index: 100;
+  box-shadow: var(--glass-shadow);
+}
+.glass-modal {
+  position: fixed; top: 0; left: 0; right: 0; bottom: 0;
+  background: rgba(0,0,0,0.5); backdrop-filter: blur(4px);
+  display: flex; align-items: center; justify-content: center;
+  z-index: 1000; transition: opacity 0.3s;
+}
+.glass-modal.hidden { opacity: 0; pointer-events: none; }
+.modal-content {
+  width: 80%; max-width: 600px; max-height: 80vh;
+  border-radius: var(--panel-radius); display: flex; flex-direction: column;
+}
+.modal-header {
+  padding: 16px; border-bottom: 0.5px solid var(--glass-border);
+  display: flex; justify-content: space-between; align-items: center;
+}
+.btn-close {
+  background: transparent; border: none; color: var(--text-tertiary);
+  font-size: 20px; cursor: pointer;
+}
+.btn-close:hover { color: var(--text-primary); }
+.modal-body { padding: 16px; overflow-y: auto; }
+table { width: 100%; border-collapse: collapse; font-size: 12px; }
+table th { color: var(--text-tertiary); text-align: left; padding: 8px; border-bottom: 1px solid var(--glass-border); }
+table td { padding: 8px; border-bottom: 1px solid rgba(255,255,255,0.05); }
+table a { color: var(--accent); text-decoration: none; }
+table a:hover { text-decoration: underline; }

05_software/real/sim2real_v2/web/static/viewer/MeshLoader.js

@@ -0,0 +1,106 @@
+/**
+ * Adapted MeshLoader for sim2real web console.
+ * Supports both fileMap-based loading (original robot_viewer API) and URL-based
+ * fetching from the sim2real HTTP server at /meshes/<name>.STL.
+ *
+ * Uses importmap-resolved Three.js via CDN (no bundler).
+ */
+import * as THREE from 'three';
+import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
+
+const _stlLoader = new STLLoader();
+
+let loadersCache = null;
+async function getLoaders() {
+    if (!loadersCache) {
+        loadersCache = { STLLoader: _stlLoader };
+    }
+    return loadersCache;
+}
+
+function normalizePath(path) {
+    if (!path) return '';
+    return path.replace(/\\/g, '/').replace(/^\/+/, '').replace(/\/+/g, '/');
+}
+
+/**
+ * Load mesh from URL (sim2real server) or fileMap (robot_viewer compatibility).
+ * @param {string} meshPath - e.g. "fl_hip_abduction_Link.STL"
+ * @param {Map|null} fileMap - optional File map (compat with MJCFAdapter)
+ * @param {string|null} meshBaseUrl - e.g. "/meshes/" for URL-based loading
+ * @returns {Promise<THREE.BufferGeometry|THREE.Group|null>}
+ */
+export async function loadMeshFile(meshPath, fileMap = null, meshBaseUrl = null) {
+    const fileName = normalizePath(meshPath).split('/').pop();
+
+    // Strategy 1: try fileMap (robot_viewer compatibility)
+    if (fileMap) {
+        for (const [key, file] of fileMap.entries()) {
+            if (typeof key === 'string' && key.toLowerCase().endsWith(fileName.toLowerCase())) {
+                try {
+                    const url = URL.createObjectURL(file);
+                    const geom = await new Promise((resolve, reject) => {
+                        _stlLoader.load(url, resolve, undefined, reject);
+                    });
+                    URL.revokeObjectURL(url);
+                    console.log('[MeshLoader] loaded from fileMap:', fileName);
+                    return geom;
+                } catch (e) {
+                    URL.revokeObjectURL(url);
+                    console.warn('[MeshLoader] fileMap load failed:', fileName, e);
+                }
+            }
+        }
+    }
+
+    // Strategy 2: try URL-based loading from sim2real server
+    const baseUrl = meshBaseUrl || '/meshes/';
+    const url = baseUrl + fileName;
+    try {
+        console.log('[MeshLoader] fetching:', url);
+        const resp = await fetch(url);
+        if (!resp.ok) {
+            console.warn('[MeshLoader] 404:', url);
+            return null;
+        }
+        const arrayBuf = await resp.arrayBuffer();
+        const blobUrl = URL.createObjectURL(new Blob([arrayBuf]));
+        const geom = await new Promise((resolve, reject) => {
+            _stlLoader.load(blobUrl, resolve, undefined, reject);
+        });
+        URL.revokeObjectURL(blobUrl);
+        console.log('[MeshLoader] loaded from URL:', fileName);
+        return geom;
+    } catch (e) {
+        console.warn('[MeshLoader] URL load failed:', url, e);
+    }
+    return null;
+}
+
+export function ensureMeshHasPhongMaterial(meshObject) {
+    meshObject.traverse((child) => {
+        if (child.isMesh && child.material) {
+            const materials = Array.isArray(child.material) ? child.material : [child.material];
+            materials.forEach((mat, i) => {
+                if (!mat) return;
+                if (mat.type === 'MeshBasicMaterial' || mat.type === 'MeshLambertMaterial') {
+                    const nm = new THREE.MeshPhongMaterial({
+                        color: mat.color, map: mat.map,
+                        transparent: mat.transparent, opacity: mat.opacity, side: mat.side,
+                        shininess: 50, specular: new THREE.Color(0.3, 0.3, 0.3),
+                    });
+                    if (nm.map) nm.map.colorSpace = THREE.SRGBColorSpace;
+                    materials[i] = nm;
+                } else if (mat.isMeshPhongMaterial || mat.isMeshStandardMaterial) {
+                    if (mat.shininess === undefined || mat.shininess < 50) mat.shininess = 50;
+                    if (!mat.specular) mat.specular = new THREE.Color(0.3, 0.3, 0.3);
+                    mat.needsUpdate = true;
+                }
+            });
+            if (Array.isArray(child.material)) child.material = materials;
+            else if (materials.length === 1) child.material = materials[0];
+        }
+    });
+}
+
+export { getLoaders };

05_software/real/sim2real_v2/web/static/viewer/RobotViewer3D.js

@@ -0,0 +1,181 @@
+/**
+ * RobotViewer3D — sim2real 3D 可视化（基于 robot_viewer 的 MJCFAdapter + Three.js）
+ *
+ * 加载 wheelleg.xml → MJCFAdapter.parse → Three.js 场景树
+ * 建立 jointName → THREE.Object3D 映射，通过 updateJoints(pos16) 实时更新。
+ * 支持 OrbitControls 旋转/缩放/平移。
+ *
+ * 用法：
+ *   const viewer = new RobotViewer3D(canvasElement);
+ *   await viewer.load('/mjcf/wheelleg.xml');
+ *   viewer.updateJoints(jointPositions16);
+ */
+import * as THREE from 'three';
+import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
+import { MJCFAdapter } from './MJCFAdapter.js';
+import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
+
+// 16 关节的标准顺序（与 motor_mapping.py:SIM_JOINT_ORDER 对齐）
+const JOINT_ORDER = [
+    'fl_hip_abduction_joint', 'fl_hip_pitch_joint', 'fl_knee_joint',
+    'fr_hip_abduction_joint', 'fr_hip_pitch_joint', 'fr_knee_joint',
+    'rl_hip_abduction_joint', 'rl_hip_pitch_joint', 'rl_knee_joint',
+    'rr_hip_abduction_joint', 'rr_hip_pitch_joint', 'rr_knee_joint',
+    'fl_wheel_joint', 'fr_wheel_joint', 'rl_wheel_joint', 'rr_wheel_joint',
+];
+
+// MJCF → Three.js 坐标轴转换：让 MJCF 的 Z 轴(向上) 映射到 Three.js 的 Y 轴(向上)
+const MJCF_TO_THREE = new THREE.Matrix4().makeRotationX(-Math.PI / 2);
+// 或直接用 euler: (0, PI, 0)
+
+export class RobotViewer3D {
+    /**
+     * @param {HTMLCanvasElement} canvas
+     * @param {object} [opts]
+     * @param {string} [opts.meshBaseUrl='/meshes/']  STL mesh 文件的 HTTP 路径前缀
+     * @param {string} [opts.mjcfUrl='/mjcf/wheelleg.xml']
+     * @param {string} [opts.backgroundColor='#1a1d24']
+     */
+    constructor(canvas, opts = {}) {
+        this.canvas = canvas;
+        this.meshBaseUrl = opts.meshBaseUrl || '/meshes/';
+        this.mjcfUrl = opts.mjcfUrl || '/mjcf/wheelleg.xml';
+
+        // Three.js 核心
+        const w = canvas.clientWidth, h = canvas.clientHeight;
+        this.scene = new THREE.Scene();
+        // 移除背景色，使用透明背景，由 CSS 控制
+        // this.scene.background = new THREE.Color(opts.backgroundColor || '#1a1d24');
+
+        this.camera = new THREE.PerspectiveCamera(55, w / h, 0.05, 50);
+        this.camera.position.set(0.5, 0.35, 0.65);
+        this.camera.lookAt(0.2, 0, 0);
+
+        this.renderer = new THREE.WebGLRenderer({ canvas, antialias: true, alpha: true });
+        this.renderer.setSize(w, h);
+        this.renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2));
+        this.renderer.shadowMap.enabled = true;
+
+        // OrbitControls
+        this.controls = new OrbitControls(this.camera, canvas);
+        this.controls.target.set(0.15, 0.08, 0.0);
+        this.controls.enableDamping = true;
+        this.controls.dampingFactor = 0.12;
+        this.controls.update();
+
+        // 灯光
+        this._setupLights();
+
+        // 地面
+        const grid = new THREE.GridHelper(2, 20, 0x444444, 0x222222);
+        grid.position.y = -0.35;
+        this.scene.add(grid);
+
+        // 状态
+        this.model = null;
+        this.rootGroup = null;
+        this.jointMap = new Map();           // jointName → { joint, group }
+        this._isLoaded = false;
+        this._rafId = null;
+        this._stlCache = new Map();          // filename → BufferGeometry
+    }
+
+    _setupLights() {
+        const ambient = new THREE.AmbientLight(0x606060, 1.5);
+        this.scene.add(ambient);
+
+        const dir1 = new THREE.DirectionalLight(0xffffff, 2.5);
+        dir1.position.set(2, 3, 2);
+        this.scene.add(dir1);
+
+        const dir2 = new THREE.DirectionalLight(0x8899cc, 1.0);
+        dir2.position.set(-1, 1, -1);
+        this.scene.add(dir2);
+
+        const hemi = new THREE.HemisphereLight(0x8899cc, 0x334455, 1.2);
+        this.scene.add(hemi);
+    }
+
+    // ---- 加载模型 ----
+    async load(mjcfUrlOverride) {
+        const url = mjcfUrlOverride || this.mjcfUrl;
+        console.log('[RobotViewer3D] loading MJCF:', url);
+        const resp = await fetch(url);
+        if (!resp.ok) throw new Error(`MJCF 404: ${url}`);
+        const xmlText = await resp.text();
+
+        // 用 MJCFAdapter 解析 → UnifiedRobotModel
+        // fileMap 为空时不传；MeshLoader 会自动 fallback 到 URL 加载
+        const model = await MJCFAdapter.parse(xmlText, null);
+        this.model = model;
+        console.log('[RobotViewer3D] parsed:', model.links.size, 'links,', model.joints.size, 'joints');
+
+        // 取 rootGroup（MJCFAdapter.createThreeObject 已构建完整 hierarchy）
+        this.rootGroup = model.threeObject;
+        // 坐标轴转换：MJCF → Three.js
+        this.rootGroup.applyMatrix4(MJCF_TO_THREE);
+        this.scene.add(this.rootGroup);
+
+        // 遍历 joints，建立索引
+        this.jointMap.clear();
+        for (const [jointName, joint] of model.joints) {
+            if (joint.threeObject) {
+                this.jointMap.set(jointName, joint);
+            }
+        }
+        // 已建立映射的关节列表
+        const mapped = Array.from(this.jointMap.keys()).sort();
+        console.log('[RobotViewer3D] joint map:', mapped.length, 'joints');
+
+        this._isLoaded = true;
+        this._startRenderLoop();
+    }
+
+    // ---- 渲染循环（按需 + 持续） ----
+    _startRenderLoop() {
+        if (this._rafId) return;
+        const loop = () => {
+            this.controls.update();
+            this.renderer.render(this.scene, this.camera);
+            this._rafId = requestAnimationFrame(loop);
+        };
+        loop();
+    }
+
+    // ---- 实时更新关节角度 ----
+    /**
+     * @param {Float64Array|number[]} pos16 — 16 关节角度 (rad)，顺序同 SIM_JOINT_ORDER
+     *   索引 0-11: 腿关节 (fl_abd,fl_pitch,fl_knee,fr...,rl...,rr...)
+     *   索引 12-15: 轮子关节 (fl_wheel,fr_wheel,rl_wheel,rr_wheel)
+     */
+    updateJoints(pos16) {
+        if (!this._isLoaded) return;
+        for (let i = 0; i < JOINT_ORDER.length && i < pos16.length; i++) {
+            const name = JOINT_ORDER[i];
+            const joint = this.jointMap.get(name);
+            if (joint) {
+                MJCFAdapter.setJointAngle(joint, pos16[i]);
+            }
+        }
+    }
+
+    // ---- 重置相机 ----
+    resetCamera() {
+        this.camera.position.set(0.5, 0.35, 0.65);
+        this.controls.target.set(0.15, 0.08, 0.0);
+        this.controls.update();
+    }
+
+    // ---- 调整大小 ----
+    resize() {
+        const w = this.canvas.clientWidth, h = this.canvas.clientHeight;
+        this.camera.aspect = w / h;
+        this.camera.updateProjectionMatrix();
+        this.renderer.setSize(w, h);
+    }
+
+    dispose() {
+        if (this._rafId) cancelAnimationFrame(this._rafId);
+        this.renderer.dispose();
+    }
+}

05_software/real/sim2real_v2/web/static/viewer/UnifiedRobotModel.js

@@ -0,0 +1,181 @@
+/**
+ * Unified robot model data interface
+ * All formats (URDF, MJCF, USD) are converted to this unified format
+ */
+export class UnifiedRobotModel {
+    constructor() {
+        this.name = '';
+        this.links = new Map(); // Map<name, Link>
+        this.joints = new Map(); // Map<name, Joint>
+        this.materials = new Map(); // Map<name, Material>
+        this.constraints = new Map(); // Map<name, Constraint> - for parallel mechanism constraints
+        this.rootLink = null; // Root link name
+        this.threeObject = null; // Three.js object (if available)
+    }
+
+    addLink(link) {
+        this.links.set(link.name, link);
+    }
+
+    addJoint(joint) {
+        this.joints.set(joint.name, joint);
+    }
+
+    addConstraint(constraint) {
+        this.constraints.set(constraint.name, constraint);
+    }
+
+    getLink(name) {
+        return this.links.get(name);
+    }
+
+    getJoint(name) {
+        return this.joints.get(name);
+    }
+
+    getConstraint(name) {
+        return this.constraints.get(name);
+    }
+}
+
+/**
+ * Link interface
+ */
+export class Link {
+    constructor(name) {
+        this.name = name;
+        this.visuals = []; // VisualGeometry[]
+        this.collisions = []; // CollisionGeometry[]
+        this.inertial = null; // InertialProperties
+        this.threeObject = null; // Three.js object
+        this.userData = {}; // User-defined data (for adapters to store additional information)
+    }
+}
+
+/**
+ * VisualGeometry interface
+ */
+export class VisualGeometry {
+    constructor() {
+        this.name = '';
+        this.origin = { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+        this.geometry = null; // GeometryType
+        this.material = null; // Material
+        this.threeObject = null; // Three.js Mesh
+    }
+}
+
+/**
+ * CollisionGeometry interface
+ */
+export class CollisionGeometry {
+    constructor() {
+        this.name = '';
+        this.origin = { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+        this.geometry = null; // GeometryType
+        this.threeObject = null; // Three.js Mesh
+    }
+}
+
+/**
+ * GeometryType interface
+ */
+export class GeometryType {
+    constructor(type) {
+        this.type = type; // 'box' | 'sphere' | 'cylinder' | 'mesh'
+        this.size = null; // Size parameters (varies by type)
+        this.filename = null; // Mesh file path (if mesh type)
+    }
+
+    clone() {
+        const cloned = new GeometryType(this.type);
+        cloned.size = this.size ? { ...this.size } : null;
+        cloned.filename = this.filename;
+        return cloned;
+    }
+}
+
+/**
+ * InertialProperties interface
+ */
+export class InertialProperties {
+    constructor() {
+        this.mass = 0;
+        this.origin = { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+        this.ixx = 0;
+        this.iyy = 0;
+        this.izz = 0;
+        this.ixy = 0;
+        this.ixz = 0;
+        this.iyz = 0;
+    }
+}
+
+/**
+ * Joint interface
+ */
+export class Joint {
+    constructor(name, type) {
+        this.name = name;
+        this.type = type; // 'revolute' | 'prismatic' | 'fixed' | 'continuous'
+        this.parent = null; // Parent link name
+        this.child = null; // Child link name
+        this.origin = { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+        this.axis = { xyz: [0, 0, 1] }; // Default z-axis
+        this.limits = null; // JointLimits
+        this.currentValue = 0; // Current joint value
+        this.threeObject = null; // Three.js object (if available)
+    }
+}
+
+/**
+ * JointLimits interface
+ */
+export class JointLimits {
+    constructor() {
+        this.lower = -Math.PI;
+        this.upper = Math.PI;
+        this.effort = null;
+        this.velocity = null;
+    }
+}
+
+/**
+ * Material interface
+ */
+export class Material {
+    constructor(name) {
+        this.name = name;
+        this.color = { r: 0.8, g: 0.8, b: 0.8 };
+        this.texture = null;
+    }
+}
+
+/**
+ * Constraint interface - for describing closed-chain constraints of parallel mechanisms
+ * Supports MuJoCo equality constraint types
+ */
+export class Constraint {
+    constructor(name, type) {
+        this.name = name;
+        this.type = type; // 'connect' | 'weld' | 'joint' | 'tendon' | 'distance'
+
+        // Constraint objects (may be body, geom, joint, etc. depending on type)
+        this.body1 = null;
+        this.body2 = null;
+        this.anchor = null; // Connection point coordinates
+        this.torquescale = null; // Torque scale
+
+        // Joint constraint specific properties
+        this.joint1 = null;
+        this.joint2 = null;
+        this.polycoef = null; // Polynomial coefficients [a0, a1, a2, a3, a4]
+
+        // Visualization object
+        this.threeObject = null; // Three.js object for displaying constraint
+
+        // Original data (for debugging)
+        this.userData = {};
+    }
+}
+

05_software/train/README.md

@@ -2,15 +2,18 @@
 
 `rc_mjlab/` 保存 16DOF 轮足机器人的当前训练与 Sim2Sim 工程。历史快照由 Git Tag 保留，不在目录中复制 `old`、`new` 或 `final` 版本。
 
-当前内容对应 `v0.6.0`，是比赛使用的最终训练代码架构。训练过程可能先获得基模，再调整奖励、课程和环境参数继续训练；模型 checkpoint 的变化不等同于软件架构变化。
+当前内容对应 `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 未发生变化。
@@ -19,4 +22,10 @@
 
 `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.6.0`：比赛使用的最终训练架构。它在前两版训练代码上重新平衡随机化强度，引入分轴速度奖励、自适应命令课程、障碍逐步释放、楼梯稳定约束和训练诊断指标。当前目录中的 `model_rough.pt` 是早期参考权重；比赛最终使用的 `model_6800.onnx` 将随最终部署版本归档。
+> 当前目录对应 `v0.8.1`：保留比赛训练架构、后期 MuJoCo/Sim2Sim 和比赛最终 Rough 策略，并补充导航地图、打点工具、路线迭代与抽样 PCD。`model_rough.pt` 仍作为早期参考权重保留。
 
 ---
 
@@ -41,6 +41,10 @@ rc_mjlab/
 │       └── competition_terrains.py   # 竞赛自定义地形（高墙障碍、低杆障碍）
 ├── sim2sim/                      # Sim2Sim 物理部署与高精度交互回放工具
 │   ├── nav_sim2sim.py            # 主程序：2D Pygame 交互面板 + 全自动多地形导航追踪
+│   ├── nav_route_sim2sim_check.py # ONNX 策略批量路线检查
+│   ├── ik_slalom_sim2sim.py      # 纯 IK、路径跟踪与绕桩验证
+│   ├── ik_compensation_sweep.py  # IK 补偿参数扫描
+│   ├── export_onnx.py            # PT actor 导出与 ONNX 一致性检查
 │   ├── sim2sim.py                # 简易版键盘调试工具
 │   ├── interface/
 │   │   └── mujoco_io.py          # MuJoCo 输入输出与传感器、低通滤波器接口
@@ -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