[real] 整理 ROS 2 v2 里程计导航联调

.gitignore

@@ -6,6 +6,11 @@ __pycache__/
 *.py[cod]
 .venv/
 venv/
+*.egg-info/
+.pytest_cache/
+.ruff_cache/
+.mypy_cache/
+.uv-cache/
 
 # Build outputs
 build/
@@ -20,10 +25,22 @@ log/
 *.bin
 *.axf
 
+# Required vendored Odin runtime libraries in the early Sim2Real release
+!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
+!05_software/real/sim2real_ros2_v2/src/odin_ros_driver/lib/*.a
+
 # Training outputs
 logs/
 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/architecture/early_software_stack.md

@@ -0,0 +1,22 @@
+# 第一代软件闭环
+
+第一代 16DOF 软件的目标是先打通“训练、仿真验证、真机执行”闭环，而不是在初期建立复杂的分布式系统。
+
+## 训练与策略
+
+`05_software/train/rc_mjlab` 使用本地修改的 mjlab 和 MuJoCo 模型训练轮腿混合策略。训练任务包括 Flat、Rough 和 Crawl，腿部 12 个关节输出位置目标，4 个轮子输出速度目标。
+
+## MuJoCo 与 Sim2Sim
+
+- `mujoco_sim` 用于不加载 RL 策略时的模型、动力学和控制调试。
+- `sim2sim` 加载训练策略，在独立 MuJoCo 环境中验证观测、动作、地形和导航行为。
+- 两者与训练任务共同使用 `rc_mjlab/mjcf`，避免早期模型定义不一致。
+
+## 真机控制
+
+- `ik_real` 先以逆运动学和轨迹插值验证电机控制链路。
+- `sim2real` 再将训练策略部署到 Python 真机运行时，加入 IMU、站立、安全和 Web 调试。
+
+## 阶段特点
+
+这一版本的优势是链路完整、模块直观，便于快速验证；局限是训练、仿真和部署仍存在重复资源，Python 真机运行时的实时性和系统集成能力有限。这些问题推动了后续统一训练版本和 ROS 2/C++ 部署架构。

01_doc/training_evolution.md

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

01_doc/version_history.md

@@ -0,0 +1,120 @@
+# 版本演进
+
+本项目使用同一条 `16dof` 主线和里程碑 Tag 保存线性演进，不在源码目录中复制历史版本。
+
+| Tag | 阶段 | 核心内容 |
+| --- | --- | --- |
+| `v0.1.0` | 8DOF 中期检查 | 8DOF 串联足机械与大疆 A 板实机版本 |
+| `v0.2.0` | 16DOF 机械 | 16DOF 串联轮足机械 CAD 与 STEP |
+| `v0.3.0` | 第一代软件闭环 | 早期训练、MJCF、MuJoCo、Sim2Sim、IK 与 Python Sim2Real |
+| `v0.3.1` | 实机记录 | 补充第一代 Sim2Real 实机视频 |
+| `v0.4.0` | 新训练基线 | 第一份完整的新 MJCF、新 mjlab 框架和 Rough 策略工程 |
+| `v0.5.0` | 随机化增强 | 扩大观测、延迟和动力学随机化，加入持续外力扰动 |
+| `v0.6.0` | 比赛训练架构 | 分轴奖励、自适应指令课程、障碍释放课程和比赛站姿 |
+| `v0.7.0` | MuJoCo 工具 | 姿态优化、IK 扫描、动力学、MPC 和 GUI 调试工具 |
+| `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 |
+| `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 |
+| `v0.9.0` | Python Sim2Real v2 | 反馈新鲜度、Odin odom 诊断、Web 调试和安全监控增强 |
+| `v0.10.0` | ROS 2/C++ 初版 | 50 Hz C++ 推理、200 Hz CAN 热路径和 ROS 2 系统集成 |
+| `v0.11.0` | ROS 2 导航原型 | 简单导航、PCD 交互定位、任务点和 Web 导航调试 |
+| `v0.11.1` | Odin 与站姿调参 | 完整 Odin 驱动、TensorRT、多策略切换和调参站姿 |
+| `v0.12.0` | 里程计导航联调 | 纯里程计 fallback、A_min 路线、TF 冲突保护和 model_9600 |
+
+> 原先临时归档为 `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.10.0` 的 ROS 2/C++ Sim2Real 初版
+
+- 归档 `real/sim2real_ros2`，将 Python 部署契约迁移到 ROS 2 Humble 与 C++ 运行时。
+- 保留 53D 观测、16D 动作、50 Hz 策略循环和 200 Hz SocketCAN 电机热路径。
+- 增加消息接口、硬件桥、策略运行时、命令仲裁、Nav2 配置、Docker 和 Windows Web 调试工具。
+- 原始快照中的 `src/odin_ros_driver` 为空目录，因此本版本仍需外部 Odin 驱动，不能宣称传感器依赖已自包含。
+- 保留原始候选 ONNX 文件以记录初版部署试验；排除计划、任务和 walkthrough 草稿。
+
+## `v0.11.0` 的 ROS 2 Sim2Real v2
+
+- 归档 `real/sim2real_ros2_v2`，保持 `v0.10.0` 的 ROS 2/C++ 控制契约。
+- 增加 `simple_nav_node.py`、PCD 点击工具、任务点/任务序列配置和 Web 导航控制入口。
+- 默认命令源从遥控切换为 `NAV`，加入简单导航状态、PCD 位姿和地图显示链路。
+- 原始 `map1.pcd`、`map6.pcd` 分别约 49.05 MiB、44.24 MiB，归档时确定性抽样到 10 MB 以下并记录哈希。
+- 原始快照中的 Odin 驱动仍为空目录；排除设备运行日志和开发草稿。
+
+## `v0.11.1` 的 Odin、TensorRT 与站姿调参
+
+- 归档 `real/sim2real_ros2_v2(z=0.380 hip=0.670 knee=-1.390)`，在同一 `sim2real_ros2_v2` 目录中记录真实差异。
+- 首次随部署工程保留完整 Odin ROS 驱动、Apache-2.0 许可证、设备标定参数和预编译 SDK 静态库。
+- 策略运行时增加 TensorRT、ONNX 回退、Rough/Crawl 模式切换、事件日志和更完整的电机失效诊断。
+- 默认 Rough 策略为 `NEWmodel_1900`，默认站姿为髋俯仰 `0.670`、膝关节 `-1.390`；Crawl 配置使用 IK 后端。
+- `map_b.pcd` 从 1,080,047 点确定性抽样为 270,012 点，并保留原始和抽样哈希。
+- 排除嵌套 Git、Odin 运行日志、缓存、开发草稿和未被配置引用的候选策略。
+
+## `v0.12.0` 的里程计导航联调
+
+- 归档 `real/sim2real_ros2_v2(odom)`，继续沿用 `sim2real_ros2_v2` 目录的线性演进。
+- 将 Rough 策略切换为 `model_9600`，默认站姿恢复为髋俯仰 `0.550`、膝关节 `-1.125`。
+- Odin `custom_map_mode` 固定为纯里程计，加入 odom 新鲜度、外部 map/odom TF 冲突和任务结束交接保护。
+- 增加 A_min 路线、PCD 地图编辑工具和三份抽样点云；原始大 PCD 不直接进入 Git。
+- 保留完整 Odin 驱动、标定参数和 SDK 静态库；未找到的 `map_a.bin` 仍不伪造，重定位闭环不在本 Tag 声称已复现。
+
+## `v0.4.0` 的模型变化
+
+- 机械 CAD 不变。
+- MJCF 更新整机质量和惯性参数，旧、新 `wheelleg.xml` 的 SHA-256 不同。
+- mjlab 上游基准从 `00409797` 更新到 `40f8d93e`。
+- 保留轮腿分组执行器随机化所需的本地补丁。
+- 本阶段归档 `model_rough.pt`，不将生成日志、缓存和临时 XML 纳入版本库。
+
+## `v0.5.0` 的训练变化
+
+- MJCF、mjlab 基准和已有模型文件保持不变。
+- 投影重力噪声由 `±0.05` 扩大到 `±0.08`。
+- 腿与轮动作的最大随机延迟由 2 步增加到 4 步。
+- 地面摩擦随机范围由 `0.3–1.0` 扩大到 `0.15–1.25`。
+- 执行器刚度和阻尼缩放由 `0.9–1.1` 扩大到 `0.5–1.5`。
+- 增加膝部和轮部质量的 `0.7–1.3` 随机缩放。
+- 增加作用于机身的连续随机外力和力矩扰动。
+
+## `v0.6.0` 的比赛训练架构
+
+- 保持 `v0.4.0` 引入的新 MJCF 和 mjlab 框架不变。
+- 将线速度奖励拆分为 x/y 两轴，并独立配置偏航角速度奖励。
+- 增加自适应 x/y/yaw 指令范围课程。
+- 增加障碍地形逐步释放与更严格的地形晋级逻辑。
+- 增加楼梯横向速度和偏航漂移约束。
+- 默认站姿调整为髋俯仰 `0.550`、膝关节 `-1.125`，初始机身高度为 `0.42 m`。
+- 增加速度误差、轮速跟踪、动作和姿态等训练诊断指标。
+- 该 Tag 保存比赛训练代码架构，不把每次继续训练产生的 checkpoint 误记为新的软件版本。
+
+训练阶段的详细关系见 [`training_evolution.md`](training_evolution.md)。
+
+## `v0.7.0` 的独立 MuJoCo 工具
+
+- 比赛训练架构、MJCF、模型和依赖锁文件保持 `v0.6.0` 状态不变。
+- 增加解析姿态表、RL 友好姿态筛选和 MuJoCo 静态姿态优化。
+- 增加 IK/差速轮参数扫描，可导出 JSON 结果。
+- 增加 Robot、Controller、Dynamics、MPCController 和 GUI 调试链路。
+- 记录历史工具常量与新版 MJCF 质量、比赛默认站姿之间的参数边界，避免将分析结果直接当作已校准真机参数。
+
+## `v0.8.0` 的后期 Sim2Sim
+
+- 比赛训练任务、MJCF 和 `v0.7.0` 的 MuJoCo 工具保持不变。
+- 策略运行器增加 ONNX 加载，并允许在缺少 `pynput` 时关闭后台键盘监听继续运行。
+- MuJoCo 执行器重建同时兼容新旧 Spec 删除接口。
+- 增加 PT→ONNX 导出、IK 补偿扫描、纯 IK 绕桩和 ONNX 批量路线检查入口。
+- 归档比赛最终 Rough 策略 `model_6800.onnx`；其 SHA-256 为 `3C994BDD3434AD15770A52AC0E8D229F502F00D6511CDD42C2E2C742301AEF13`。
+- Crawl 权重、运行日志、临时 XML 和大量重复路线实验不在本阶段归档。
+
+## `v0.8.1` 的导航打点工具
+
+- 补充 Pygame 地图/PCD/航点综合编辑器、避障区域编辑器和坐标变换工具。
+- 补充路线安全检查、候选航点优化、XML/航点镜像和批量 Sim2Sim 实验入口。
+- 按源文件时间保留 14 份比赛路线 JSON，不将开发期文件名误解释为正式版本号。
+- 补充 `1hao.xml`、`2hao.xml`、`A_C.xml`，并为 `1B_FF.json` 补齐其引用的 `B_C.xml`。
+- 将两份约 915 MiB 的原始 ASCII PCD 确定性抽样为各小于 10 MB 的预览点云；抽样参数、点数和哈希记录在工具 README。
+- 训练代码、MJCF、比赛策略和历史依赖锁保持 `v0.8.0` 状态不变。

05_software/README.md

@@ -1,13 +1,42 @@
 # 软件
 
-本目录用于整理 16DOF 轮足机器人的上层软件。
+本目录保存 16DOF 轮足机器人的训练、仿真和真机软件演进。
 
 ```text
 05_software/
-├─ train/      # 强化学习训练任务和配置
-├─ sim/        # Sim2Sim 和独立仿真
-├─ real/       # Sim2Real 与 ROS 2 真机部署
-└─ tools/      # 导航、地图、诊断和转换工具
+├─ train/
+│  └─ rc_mjlab/       # 训练、MJCF、MuJoCo、Sim2Sim 和本地 mjlab 依赖
+└─ real/
+   ├─ ik_real/        # IK 轨迹与早期真机控制
+   ├─ sim2real/       # 第一代 Python 策略真机部署
+   ├─ sim2real_v2/    # Python Sim2Real v2
+   ├─ sim2real_ros2/  # ROS 2/C++ Sim2Real 初版
+   └─ sim2real_ros2_v2/ # ROS 2 导航原型及后续演进
 ```
 
-历史版本通过 Git Tag 保存，同一模块不使用 `old`、`final`、`v2` 等目录复制完整代码。
+## 数据流
+
+```text
+MJCF + mjlab task
+        |
+        v
+   PPO 训练策略
+        |
+        +----> MuJoCo 姿态 / IK / MPC 调试
+        |
+        +----> Sim2Sim 策略验证
+        |
+        +----> Python Sim2Real / v2 ----> 电机 / IMU
+        |
+        +----> ROS 2/C++ Sim2Real -----> CAN / IMU / 导航
+
+IK real --------------------------------> 电机
+```
+
+`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`，ROS 2/C++ 初版对应 `v0.10.0`，简单导航原型对应 `v0.11.0`，完整 Odin/TensorRT 与站姿调参对应 `v0.11.1`，纯里程计导航联调对应 `v0.12.0`。
+
+详细说明见：
+
+- [`train/README.md`](train/README.md)
+- [`real/README.md`](real/README.md)
+- [`../01_doc/architecture/early_software_stack.md`](../01_doc/architecture/early_software_stack.md)

05_software/real/README.md

@@ -0,0 +1,47 @@
+# 真机控制版本演进
+
+本目录保存 16DOF 轮足机器人从早期 Python 闭环到 ROS 2 部署的真机控制演进。
+
+## `ik_real`
+
+基于几何逆运动学和轨迹插值的真机控制探索，不依赖强化学习策略。主要用于验证电机接口、关节映射和姿态轨迹。
+
+## `sim2real`
+
+第一代 Python 策略部署栈，包含：
+
+- 53D 观测到 16D 动作的策略运行时
+- 电机映射和真机 IO
+- IMU 接入
+- 站立初始化与平衡
+- 运行时安全检查和阻尼刹车
+- Web 调试界面
+- 对齐、标定和独立检查工具
+
+部署说明见 [`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_ros2`
+
+ROS 2/C++ Sim2Real 初版，将策略热路径迁移为 50 Hz C++ 推理和 200 Hz CAN 电机循环，并加入 ROS 2 消息、命令仲裁、Nav2 与统一启动结构。该版本对应重排主线的 `v0.10.0`。
+
+原始快照没有随工程保存 Odin ROS 2 驱动源码，该依赖边界见 [`sim2real_ros2/README.md`](sim2real_ros2/README.md)。
+
+## `sim2real_ros2_v2`
+
+ROS 2 Sim2Real v2 导航原型，在初版基础上增加简单导航节点、PCD 交互定位、任务点/任务序列和 Web 导航调试。该版本对应重排主线的 `v0.11.0`。
+
+`v0.11.1` 在同一目录继续演进，首次随工程归档完整 Odin 驱动、TensorRT、多策略切换和硬件诊断，并使用 `hip=0.670`、`knee=-1.390` 的调参站姿。各 Tag 可恢复对应阶段，当前目录说明见 [`sim2real_ros2_v2/README.md`](sim2real_ros2_v2/README.md)。
+
+`v0.12.0` 继续在同一目录保存 odom 快照，固定纯里程计模式，加入 odom fallback 的 TF 冲突保护、A_min 路线和多地图工具；默认 Rough 策略为 `model_9600`，默认站姿回到比赛站姿。
+
+## 实机记录
+
+[![第一代 Sim2Real 真机验证](../../06_assets/images/early_sim2real_preview.jpg)](../../06_assets/videos/early_sim2real.mp4)
+
+该视频记录了这一阶段的早期真机测试，用于对应本目录中的第一代控制与部署实现。

05_software/real/ik_real/README.md

@@ -0,0 +1,9 @@
+# IK 真机控制探索
+
+该目录保存强化学习部署前的逆运动学真机控制代码。
+
+- `sim2real_control_api.py`：真机控制接口
+- `trajectory_interpolator.py`：关节/姿态轨迹插值
+- `sim_to_real_deploy_beifen.py`：早期部署脚本备份
+
+文件名中的 `beifen` 来自原始资料。为保持早期版本可追溯性，本次归档不修改源码和文件名。

05_software/real/ik_real/sim2real_control_api.py

@@ -0,0 +1,274 @@
+"""Sim-to-real control/IK API extracted from mature mujoco_sim controller.
+
+This module provides a deployment-friendly wrapper around:
+- wheel mode posture control
+- trot swing-leg IK + wheel assist
+- differential wheel speed mapping
+
+No MuJoCo runtime is required for using the API itself.
+For trot IK, Pinocchio model is used via Dynamics.
+"""
+
+from dataclasses import dataclass
+from typing import Dict, Optional, Tuple
+import numpy as np
+
+from config import (
+    LEG_NAMES,
+    LEG_JOINTS,
+    WHEEL_JOINT,
+    DEFAULT_JOINT_ANGLES,
+    WHEEL_RADIUS,
+    WHEEL_TRACK,
+    WHEEL_VEL_MAX,
+    KP_ROLL,
+    KP_PITCH,
+    GAIT_FREQ,
+    GAIT_DUTY,
+    SWING_HEIGHT,
+    PHASE_OFFSETS,
+)
+from dynamics import Dynamics
+
+
+@dataclass
+class DeployState:
+    """Minimal state for deployment control."""
+
+    rpy: np.ndarray  # (3,) roll, pitch, yaw
+
+
+class Sim2RealControlAPI:
+    """Deployment-friendly control and IK API.
+
+    Supported modes:
+    - wheel: wheel differential drive + leg posture hold
+    - trot: swing foot IK + stance posture + wheel assist
+    """
+
+    def __init__(self):
+        self.mode = "wheel"
+        self.prone = False
+
+        self.vel_x = 0.0
+        self.vel_y = 0.0
+        self.yaw_rate = 0.0
+        self.height = 0.33
+
+        self._gait_phase = 0.0
+        self._smooth_vx = 0.0
+        self._smooth_vy = 0.0
+        self._smooth_yaw = 0.0
+
+        self._default_q = np.array([
+            DEFAULT_JOINT_ANGLES["hip_abduction"],
+            DEFAULT_JOINT_ANGLES["hip_pitch"],
+            DEFAULT_JOINT_ANGLES["knee"],
+        ])
+
+        self.dynamics = Dynamics()
+        self._swing_start_foot = {leg: np.zeros(3) for leg in LEG_NAMES}
+        self._last_contact = {leg: True for leg in LEG_NAMES}
+
+    def set_mode(self, mode: str):
+        if mode not in ("wheel", "trot"):
+            raise ValueError("mode must be one of: wheel, trot")
+        self.mode = mode
+
+    def set_command(self, vel_x: float, vel_y: float, yaw_rate: float, height: Optional[float] = None):
+        self.vel_x = float(vel_x)
+        self.vel_y = float(vel_y)
+        self.yaw_rate = float(yaw_rate)
+        if height is not None:
+            self.height = float(height)
+
+    def compute(
+        self,
+        state: DeployState,
+        dt: float,
+        q_pin: Optional[np.ndarray] = None,
+        dq_pin: Optional[np.ndarray] = None,
+    ) -> Tuple[np.ndarray, np.ndarray]:
+        """Compute leg and wheel commands.
+
+        Returns:
+            leg_targets: (12,) [fl(3), fr(3), rl(3), rr(3)]
+            wheel_targets: (4,) [fl, fr, rl, rr] in rad/s
+
+        Notes:
+            - wheel mode does not require q_pin/dq_pin
+            - trot mode requires q_pin/dq_pin for IK/FK through Pinocchio
+        """
+        alpha = min(float(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)
+
+        if self.prone:
+            return self._prone_mode()
+
+        if self.mode == "wheel":
+            return self._wheel_mode(state)
+
+        if q_pin is None or dq_pin is None:
+            raise ValueError("trot mode requires q_pin and dq_pin")
+        return self._trot_mode(state, float(dt), q_pin, dq_pin)
+
+    def to_joint_dict(self, leg_targets: np.ndarray, wheel_targets: np.ndarray) -> Dict[str, float]:
+        """Convert array commands to named joint-command dictionary."""
+        out: Dict[str, float] = {}
+        for i, leg in enumerate(LEG_NAMES):
+            out[f"{leg}_{LEG_JOINTS[0]}"] = float(leg_targets[i * 3 + 0])
+            out[f"{leg}_{LEG_JOINTS[1]}"] = float(leg_targets[i * 3 + 1])
+            out[f"{leg}_{LEG_JOINTS[2]}"] = float(leg_targets[i * 3 + 2])
+            out[f"{leg}_{WHEEL_JOINT}"] = float(wheel_targets[i])
+        return out
+
+    def _prone_mode(self):
+        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 + 0] = side * 0.3
+            leg_targets[i * 3 + 1] = 1.5
+            leg_targets[i * 3 + 2] = -2.65
+        return leg_targets, np.zeros(4)
+
+    def _wheel_mode(self, state: DeployState):
+        wheel_targets = self._differential_drive(self._smooth_vx, self._smooth_yaw)
+        leg_targets = self._posture_control(state)
+        return leg_targets, wheel_targets
+
+    def _posture_control(self, state: DeployState) -> np.ndarray:
+        leg_targets = np.zeros(12)
+
+        _H = [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]
+        _HIP = [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]
+        _KNEE = [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]
+
+        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))
+
+        roll_corr = -KP_ROLL * float(state.rpy[0])
+        pitch_corr = -KP_PITCH * float(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 + 0] = np.clip(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.6, -0.3)
+
+        return leg_targets
+
+    def _trot_mode(self, state: DeployState, dt: float, q_pin: np.ndarray, dq_pin: np.ndarray):
+        self._gait_phase = (self._gait_phase + dt * GAIT_FREQ) % 1.0
+
+        contacts: Dict[str, bool] = {}
+        for leg in LEG_NAMES:
+            phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0
+            contacts[leg] = bool(phase < GAIT_DUTY)
+
+        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]:
+                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_targets[i] = self._differential_drive_single(self._smooth_vx, self._smooth_yaw, leg)
+            else:
+                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_targets[i] = 0.0
+
+        return leg_targets, wheel_targets
+
+    def _stance_leg_target(self, state: DeployState, leg: str) -> np.ndarray:
+        _H = [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]
+        _HIP = [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]
+        _KNEE = [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]
+
+        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 * float(state.rpy[0])
+        pitch_corr = -KP_PITCH * float(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.6, -0.3),
+        ])
+
+    def _differential_drive(self, vel_x: float, yaw_rate: float) -> np.ndarray:
+        vel_left = (vel_x - 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
+        vel_right = (vel_x + 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS
+        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 float(np.clip(v, -WHEEL_VEL_MAX, WHEEL_VEL_MAX))
+
+    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: DeployState, swing_phase: float) -> np.ndarray:
+        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 = 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: DeployState) -> np.ndarray:
+        td = self._swing_start_foot[leg].copy()
+
+        t_stance = (1.0 / GAIT_FREQ) * GAIT_DUTY
+        yaw = float(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
+        return td
+
+
+if __name__ == "__main__":
+    api = Sim2RealControlAPI()
+    api.set_mode("wheel")
+    api.set_command(vel_x=0.3, vel_y=0.0, yaw_rate=0.0, height=0.33)
+
+    state = DeployState(rpy=np.array([0.0, 0.0, 0.0]))
+    leg, wheel = api.compute(state=state, dt=0.004)
+    cmd = api.to_joint_dict(leg, wheel)
+
+    print("Example wheel-mode command:")
+    for k, v in sorted(cmd.items()):
+        print(f"{k}: {v:.6f}")

05_software/real/ik_real/sim_to_real_deploy_beifen.py

@@ -0,0 +1,234 @@
+#!/usr/bin/env python3
+import argparse, sys, threading, time
+from dataclasses import dataclass
+from pathlib import Path
+import numpy as np
+
+sys.path.append('/home/rc2/work/rcwork/control')
+from drivers.motor_driver import RobStrideDriver
+
+ROOT = Path('/home/rc2/work/rcwork/wheelleg_deploy_swj/wheelleg_deploy/wheelleg_mjlab/beifen')
+sys.path += [str(ROOT), str(ROOT / 'mujoco_sim')]
+from sim2real_control_api import DeployState, Sim2RealControlAPI  # type: ignore
+
+sys.path.append('/home/rc2/work/rcwork')
+from trajectory_interpolator import TrajectoryInterpolator
+
+LEGS = ('fl','fr','rl','rr')
+LJ = ('hip_abduction_joint','hip_pitch_joint','knee_joint')
+WJ = 'wheel_joint'
+JNS = [f'{l}_{j}' for l in LEGS for j in (*LJ, WJ)]
+
+@dataclass
+class Cfg:
+    mid:int; model:str; sign:float; off:float; bus:str
+
+class Deploy:
+    def __init__(self, can1, can2, hz=100.0, use_interpolation=True, interp_method='quintic', interp_time=1.5):
+        self.d1, self.d2 = RobStrideDriver(can1, False), RobStrideDriver(can2, False)
+        self.api = Sim2RealControlAPI(); self.dt = 1.0/hz; self.lk = threading.Lock()
+        self.run = False; self.enabled = False; self.estop = True
+        self.mode='stand'; self.vx=0.0; self.vy=0.0; self.yaw=0.0; self.h=0.33; self.roll=0.0; self.pitch=0.0
+        self.prone = False
+        self.kp_leg, self.kd_leg, self.kd_wheel = 80.0, 2.5, 2.0
+        self.cfg = self._cfg(); self.q=np.zeros(23); self.dq=np.zeros(22)
+        
+        self.use_interpolation = use_interpolation
+        if self.use_interpolation:
+            self.interpolator = TrajectoryInterpolator(method=interp_method, transition_time=interp_time)
+            print(f'[Interpolation] Enabled: method={interp_method}, transition_time={interp_time}s')
+        else:
+            self.interpolator = None
+            print('[Interpolation] Disabled')
+
+    def _cfg(self):
+        sign={'fl_hip_abduction_joint':-1,'fl_hip_pitch_joint':-1,'fl_knee_joint':-1,'fl_wheel_joint':-1,'fr_hip_abduction_joint':-1,'fr_hip_pitch_joint':1,'fr_knee_joint':1,'fr_wheel_joint':1,'rl_hip_abduction_joint':1,'rl_hip_pitch_joint':-1,'rl_knee_joint':-1,'rl_wheel_joint':-1,'rr_hip_abduction_joint':1,'rr_hip_pitch_joint':1,'rr_knee_joint':1,'rr_wheel_joint':1}
+        off={'fl_hip_abduction_joint':0.003,'fl_hip_pitch_joint':0.030,'fl_knee_joint':0.028,'fl_wheel_joint':0.0,'fr_hip_abduction_joint':0.004,'fr_hip_pitch_joint':0.038,'fr_knee_joint':0.011,'fr_wheel_joint':0.0,'rl_hip_abduction_joint':0.019,'rl_hip_pitch_joint':-0.034,'rl_knee_joint':0.025,'rl_wheel_joint':0.0,'rr_hip_abduction_joint':-0.001,'rr_hip_pitch_joint':0.039,'rr_knee_joint':0.018,'rr_wheel_joint':0.0}
+        ids={'fl_hip_abduction_joint':1,'fl_hip_pitch_joint':2,'fl_knee_joint':3,'fl_wheel_joint':4,'fr_hip_abduction_joint':5,'fr_hip_pitch_joint':6,'fr_knee_joint':7,'fr_wheel_joint':8,'rl_hip_abduction_joint':1,'rl_hip_pitch_joint':2,'rl_knee_joint':3,'rl_wheel_joint':4,'rr_hip_abduction_joint':5,'rr_hip_pitch_joint':6,'rr_knee_joint':7,'rr_wheel_joint':8}
+        bus={k:('can1' if k.startswith('f') else 'can2') for k in JNS}
+        return {jn:Cfg(ids[jn],'rs-06',float(sign[jn]),float(off[jn]),bus[jn]) for jn in JNS}
+
+    def _drv(self, jn): return self.d1 if self.cfg[jn].bus=='can1' else self.d2
+
+    def connect(self):
+        self.d1.connect(); self.d2.connect()
+        for jn,c in self.cfg.items(): self._drv(jn).add_motor(jn,c.mid,c.model)
+
+    def enable_all(self):
+        for jn in JNS: self._drv(jn).enable(jn)
+        with self.lk: self.enabled=True; self.estop=False; self.vx=self.vy=self.yaw=0.0
+
+    def disable_all(self):
+        for jn in JNS: self._drv(jn).disable(jn)
+        with self.lk: self.enabled=False
+
+    def clear(self):
+        for jn in JNS: self._drv(jn).clear_warnings(jn)
+
+    def set_estop(self,on):
+        with self.lk:
+            self.estop=on
+            if on: self.vx=self.vy=self.yaw=0.0
+        if on: self.disable_all()
+
+    def set_prone(self,on):
+        with self.lk: self.prone=on; self.api.prone=on
+
+    def _update_pin(self,leg,wheel):
+        q=np.zeros(23); dq=np.zeros(22); q[2]=self.h; q[6]=1.0
+        for i,_ in enumerate(LEGS):
+            b=7+i*4; q[b:b+3]=leg[i*3:i*3+3]; dq[6+i*4+3]=wheel[i]
+        self.q,self.dq=q,dq
+
+    def step(self):
+        with self.lk:
+            if self.estop or (not self.enabled): return
+            m=self.mode; vx=float(np.clip(self.vx,-0.8,0.8)); vy=float(np.clip(self.vy,-0.5,0.5)); yaw=float(np.clip(self.yaw,-3,3)); h=float(np.clip(self.h,0.157,0.448)); r=float(np.clip(self.roll,-0.4,0.4)); p=float(np.clip(self.pitch,-0.4,0.4)); prone=self.prone
+        cm='trot' if m=='trot' else 'wheel'
+        if m=='stand': vx=vy=yaw=0.0
+        self.api.prone=prone; self.api.set_mode(cm); self.api.set_command(vx,vy,yaw,height=h)
+        st=DeployState(rpy=np.array([r,p,0.0]))
+        leg,wheel = self.api.compute(st,self.dt,self.q,self.dq) if cm=='trot' else self.api.compute(st,self.dt)
+        self._update_pin(leg,wheel); cmd=self.api.to_joint_dict(leg,wheel)
+        
+        if self.use_interpolation and self.interpolator is not None:
+            leg_cmd = {jn: cmd[jn] for jn in JNS if not jn.endswith(WJ)}
+            self.interpolator.set_target(leg_cmd)
+            smooth_cmd = self.interpolator.update(self.dt)
+            for jn in leg_cmd:
+                cmd[jn] = smooth_cmd[jn]
+        
+        for jn in JNS:
+            d=self._drv(jn); c=self.cfg[jn]
+            if jn.endswith(WJ): d.control_mit(jn,0.0,c.sign*float(cmd.get(jn,0.0)),0.0,self.kd_wheel,0.0)
+            else: d.control_mit(jn,c.sign*float(cmd[jn])+c.off,0.0,self.kp_leg,self.kd_leg,0.0)
+
+    def loop(self):
+        while self.run:
+            t=time.time()
+            try: self.step()
+            except Exception as e: print('[control]',e)
+            time.sleep(max(0.0,self.dt-(time.time()-t)))
+
+    def start(self): self.run=True; threading.Thread(target=self.loop,daemon=True).start()
+    def stop(self):
+        self.run=False; time.sleep(0.05)
+        try: self.disable_all()
+        finally: self.d1.disconnect(); self.d2.disconnect()
+
+    def status(self):
+        with self.lk: return f'mode={self.mode} en={self.enabled} estop={self.estop} prone={self.prone} vx={self.vx:.2f} vy={self.vy:.2f} yaw={self.yaw:.2f} h={self.h:.3f}'
+
+class CLI:
+    def __init__(self,d): self.d=d
+    def run(self):
+        print('enable disable clear estop_on estop_off prone_on prone_off status')
+        print('mode stand|wheel|trot, vx vy yaw h roll pitch, stop, quit')
+        print('interp_on interp_off interp_time <sec>, interp_method linear|cubic|quintic|cosine')
+        while True:
+            try: s=input('cmd> ').strip().lower()
+            except (EOFError,KeyboardInterrupt): s='quit'
+            if s in ('quit','exit'): break
+            if s=='enable': self.d.enable_all(); continue
+            if s=='disable': self.d.disable_all(); continue
+            if s=='clear': self.d.clear(); continue
+            if s=='estop_on': self.d.set_estop(True); continue
+            if s=='estop_off': self.d.set_estop(False); continue
+            if s=='prone_on': self.d.set_prone(True); continue
+            if s=='prone_off': self.d.set_prone(False); continue
+            if s=='status': print(self.d.status()); continue
+            if s=='stop':
+                with self.d.lk: self.d.vx=self.d.vy=self.d.yaw=0.0
+                continue
+            if s=='interp_on':
+                with self.d.lk: self.d.use_interpolation=True
+                print('Interpolation enabled'); continue
+            if s=='interp_off':
+                with self.d.lk: self.d.use_interpolation=False
+                print('Interpolation disabled'); continue
+            if s.startswith('interp_time '):
+                try:
+                    t=float(s.split()[1])
+                    if self.d.interpolator: self.d.interpolator.set_transition_time(t)
+                    print(f'Interpolation time set to {t}s')
+                except Exception as e: print(f'Error: {e}')
+                continue
+            if s.startswith('interp_method '):
+                try:
+                    method=s.split()[1]
+                    if self.d.interpolator: self.d.interpolator.set_method(method)
+                    print(f'Interpolation method set to {method}')
+                except Exception as e: print(f'Error: {e}')
+                continue
+            if s.startswith('mode '):
+                m=s.split()[1]
+                if m in ('stand','wheel','trot'):
+                    with self.d.lk: self.d.mode=m
+                else: print('bad mode')
+                continue
+            try:
+                k,v=s.split()[0],float(s.split()[1])
+                with self.d.lk:
+                    if k=='vx': self.d.vx=v
+                    elif k=='vy': self.d.vy=v
+                    elif k=='yaw': self.d.yaw=v
+                    elif k=='h': self.d.h=v
+                    elif k=='roll': self.d.roll=v
+                    elif k=='pitch': self.d.pitch=v
+                    else: print('unknown')
+            except Exception: print('unknown/bad')
+
+class GUI:
+    def __init__(self,d):
+        import tkinter as tk
+        from tkinter import ttk
+        self.d=d; self.root=tk.Tk(); self.root.title('WheelLeg Deploy')
+        f=ttk.Frame(self.root,padding=8); f.grid(row=0,column=0,sticky='nsew')
+        self.state=tk.StringVar(value='E-STOP ON'); ttk.Label(f,textvariable=self.state).grid(row=0,column=0,columnspan=4,sticky='w')
+        ttk.Button(f,text='Enable',command=self.en).grid(row=1,column=0)
+        ttk.Button(f,text='Disable',command=self.dis).grid(row=1,column=1)
+        ttk.Button(f,text='E-STOP ON',command=lambda:self.es(True)).grid(row=1,column=2)
+        ttk.Button(f,text='E-STOP OFF',command=lambda:self.es(False)).grid(row=1,column=3)
+        ttk.Button(f,text='Prone ON',command=lambda:self.pr(True)).grid(row=2,column=2)
+        ttk.Button(f,text='Prone OFF',command=lambda:self.pr(False)).grid(row=2,column=3)
+        self.mode=tk.StringVar(value='stand'); self.vx=tk.DoubleVar(value=0.0); self.vy=tk.DoubleVar(value=0.0); self.yaw=tk.DoubleVar(value=0.0); self.h=tk.DoubleVar(value=0.33)
+        self.roll=tk.DoubleVar(value=0.0); self.pitch=tk.DoubleVar(value=0.0)
+        cb=ttk.Combobox(f,textvariable=self.mode,values=['stand','wheel','trot'],state='readonly'); cb.grid(row=3,column=0,columnspan=2,sticky='ew'); cb.bind('<<ComboboxSelected>>',lambda _:self.sync())
+        ttk.Button(f,text='Stop',command=self.stp).grid(row=3,column=3)
+        self.sl(f,4,'vx',self.vx,-0.8,0.8); self.sl(f,5,'vy',self.vy,-0.5,0.5); self.sl(f,6,'yaw',self.yaw,-3,3); self.sl(f,7,'height',self.h,0.157,0.448); self.sl(f,8,'roll',self.roll,-0.4,0.4); self.sl(f,9,'pitch',self.pitch,-0.4,0.4)
+        self.info=tk.StringVar(value=''); ttk.Label(f,textvariable=self.info).grid(row=10,column=0,columnspan=4,sticky='w'); self.tick()
+    def sl(self,f,r,n,v,lo,hi):
+        from tkinter import ttk
+        ttk.Label(f,text=n).grid(row=r,column=0,sticky='w'); ttk.Scale(f,from_=lo,to=hi,variable=v,command=lambda _:self.sync()).grid(row=r,column=1,columnspan=3,sticky='ew')
+    def sync(self):
+        with self.d.lk:
+            self.d.mode=self.mode.get(); self.d.vx=float(self.vx.get()); self.d.vy=float(self.vy.get()); self.d.yaw=float(self.yaw.get()); self.d.h=float(self.h.get()); self.d.roll=float(self.roll.get()); self.d.pitch=float(self.pitch.get())
+    def en(self): self.d.enable_all(); self.state.set('Enabled')
+    def dis(self): self.d.disable_all(); self.state.set('Disabled')
+    def es(self,on): self.d.set_estop(on); self.state.set('E-STOP ON' if on else 'E-STOP OFF')
+    def pr(self,on): self.d.set_prone(on)
+    def stp(self):
+        with self.d.lk: self.d.vx=self.d.vy=self.d.yaw=0.0
+        self.vx.set(0.0); self.vy.set(0.0); self.yaw.set(0.0)
+    def tick(self): self.info.set(self.d.status()); self.root.after(150,self.tick)
+    def run(self): self.root.mainloop()
+
+def main():
+    ap=argparse.ArgumentParser()
+    ap.add_argument('--port-can1',default='/dev/can1')
+    ap.add_argument('--port-can2',default='/dev/can2')
+    ap.add_argument('--hz',type=float,default=100.0)
+    ap.add_argument('--no-gui',action='store_true')
+    ap.add_argument('--no-interp',action='store_true',help='Disable trajectory interpolation')
+    ap.add_argument('--interp-method',default='quintic',choices=['linear','cubic','quintic','cosine'],help='Interpolation method')
+    ap.add_argument('--interp-time',type=float,default=0.3,help='Interpolation transition time (seconds)')
+    a=ap.parse_args()
+    d=Deploy(a.port_can1,a.port_can2,a.hz,use_interpolation=not a.no_interp,interp_method=a.interp_method,interp_time=a.interp_time); d.connect(); d.start()
+    try:
+        cli=CLI(d); t=threading.Thread(target=cli.run,daemon=True); t.start()
+        if a.no_gui:
+            while t.is_alive(): time.sleep(0.2)
+        else: GUI(d).run()
+    finally: d.stop()
+
+if __name__=='__main__': main()

05_software/real/ik_real/trajectory_interpolator.py

@@ -0,0 +1,237 @@
+#!/usr/bin/env python3
+"""
+轨迹插值模块 - 用于平滑关节角度过渡，避免突变和冲击
+
+支持多种插值方法：
+- linear: 线性插值
+- cubic: 三次多项式（速度连续）
+- quintic: 五次多项式（速度和加速度连续，最平滑）
+- cosine: 余弦 S 曲线
+
+使用示例：
+    interp = TrajectoryInterpolator(method='quintic', transition_time=0.5)
+    
+    # 设置新目标
+    interp.set_target({'joint1': 1.5, 'joint2': 0.8})
+    
+    # 每个控制周期调用
+    smooth_q = interp.update(dt=0.01, current_q={'joint1': 0.5, 'joint2': 0.3})
+"""
+
+import time
+from typing import Dict, Optional
+import numpy as np
+
+
+class TrajectoryInterpolator:
+    def __init__(self, method: str = 'quintic', transition_time: float = 0.5):
+        """
+        初始化轨迹插值器
+        
+        Args:
+            method: 插值方法 ('linear', 'cubic', 'quintic', 'cosine')
+            transition_time: 过渡时间（秒）
+        """
+        self.method = method
+        self.transition_time = transition_time
+        
+        self.q_start: Dict[str, float] = {}
+        self.q_target: Dict[str, float] = {}
+        self.q_current: Dict[str, float] = {}
+        
+        self.transition_start_time: Optional[float] = None
+        self.is_transitioning = False
+        
+        self._interpolation_funcs = {
+            'linear': self._linear,
+            'cubic': self._cubic,
+            'quintic': self._quintic,
+            'cosine': self._cosine,
+        }
+        
+        if method not in self._interpolation_funcs:
+            raise ValueError(f"Unknown interpolation method: {method}. "
+                           f"Available: {list(self._interpolation_funcs.keys())}")
+    
+    def set_target(self, q_target: Dict[str, float], force_restart: bool = False):
+        """
+        设置新的目标角度，开始新的过渡
+        
+        Args:
+            q_target: 目标关节角度字典 {joint_name: angle}
+            force_restart: 是否强制重新开始过渡（即使已经在过渡中）
+        """
+        if not self.q_current:
+            self.q_current = q_target.copy()
+            self.q_target = q_target.copy()
+            self.q_start = q_target.copy()
+            self.is_transitioning = False
+            return
+        
+        if not force_restart and self.is_transitioning:
+            self.q_target = q_target.copy()
+            return
+        
+        self.q_start = self.q_current.copy()
+        self.q_target = q_target.copy()
+        self.transition_start_time = time.time()
+        self.is_transitioning = True
+    
+    def update(self, dt: float, current_q: Optional[Dict[str, float]] = None) -> Dict[str, float]:
+        """
+        更新插值状态，返回当前应该下发的平滑角度
+        
+        Args:
+            dt: 时间步长（秒）
+            current_q: 可选的当前实际角度（用于初始化或同步）
+        
+        Returns:
+            平滑后的关节角度字典
+        """
+        if current_q is not None and not self.q_current:
+            self.q_current = current_q.copy()
+            self.q_start = current_q.copy()
+            self.q_target = current_q.copy()
+            return self.q_current.copy()
+        
+        if not self.is_transitioning:
+            return self.q_target.copy()
+        
+        elapsed = time.time() - self.transition_start_time
+        
+        if elapsed >= self.transition_time:
+            self.q_current = self.q_target.copy()
+            self.is_transitioning = False
+            return self.q_current.copy()
+        
+        s = elapsed / self.transition_time
+        alpha = self._interpolation_funcs[self.method](s)
+        
+        self.q_current = {}
+        for joint_name in self.q_target:
+            start_val = self.q_start.get(joint_name, 0.0)
+            target_val = self.q_target[joint_name]
+            self.q_current[joint_name] = start_val + (target_val - start_val) * alpha
+        
+        return self.q_current.copy()
+    
+    def reset(self, q_init: Optional[Dict[str, float]] = None):
+        """
+        重置插值器状态
+        
+        Args:
+            q_init: 初始角度，如果为 None 则清空所有状态
+        """
+        if q_init is None:
+            self.q_start = {}
+            self.q_target = {}
+            self.q_current = {}
+        else:
+            self.q_start = q_init.copy()
+            self.q_target = q_init.copy()
+            self.q_current = q_init.copy()
+        
+        self.transition_start_time = None
+        self.is_transitioning = False
+    
+    def is_done(self) -> bool:
+        """返回是否已完成当前过渡"""
+        return not self.is_transitioning
+    
+    def set_transition_time(self, t: float):
+        """动态修改过渡时间"""
+        self.transition_time = max(0.01, t)
+    
+    def set_method(self, method: str):
+        """动态修改插值方法"""
+        if method not in self._interpolation_funcs:
+            raise ValueError(f"Unknown method: {method}")
+        self.method = method
+    
+    @staticmethod
+    def _linear(s: float) -> float:
+        """线性插值：alpha = s"""
+        return np.clip(s, 0.0, 1.0)
+    
+    @staticmethod
+    def _cubic(s: float) -> float:
+        """三次多项式：alpha = 3s² - 2s³"""
+        s = np.clip(s, 0.0, 1.0)
+        return 3.0 * s**2 - 2.0 * s**3
+    
+    @staticmethod
+    def _quintic(s: float) -> float:
+        """五次多项式：alpha = 10s³ - 15s⁴ + 6s⁵"""
+        s = np.clip(s, 0.0, 1.0)
+        return 10.0 * s**3 - 15.0 * s**4 + 6.0 * s**5
+    
+    @staticmethod
+    def _cosine(s: float) -> float:
+        """余弦 S 曲线：alpha = (1 - cos(πs)) / 2"""
+        s = np.clip(s, 0.0, 1.0)
+        return (1.0 - np.cos(np.pi * s)) / 2.0
+
+
+if __name__ == '__main__':
+    import matplotlib.pyplot as plt
+    
+    print("轨迹插值模块测试")
+    print("=" * 60)
+    
+    methods = ['linear', 'cubic', 'quintic', 'cosine']
+    colors = ['blue', 'green', 'red', 'purple']
+    
+    fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 5))
+    
+    for method, color in zip(methods, colors):
+        interp = TrajectoryInterpolator(method=method, transition_time=1.0)
+        
+        interp.reset({'joint1': 0.0})
+        interp.set_target({'joint1': 1.0})
+        
+        times = []
+        positions = []
+        velocities = []
+        
+        t = 0.0
+        dt = 0.01
+        last_pos = 0.0
+        
+        while t <= 1.0:
+            q = interp.update(dt)
+            pos = q['joint1']
+            vel = (pos - last_pos) / dt if t > 0 else 0.0
+            
+            times.append(t)
+            positions.append(pos)
+            velocities.append(vel)
+            
+            last_pos = pos
+            t += dt
+        
+        ax1.plot(times, positions, label=method, color=color, linewidth=2)
+        ax2.plot(times, velocities, label=method, color=color, linewidth=2)
+    
+    ax1.set_xlabel('时间 (s)')
+    ax1.set_ylabel('位置 (rad)')
+    ax1.set_title('不同插值方法的位置曲线')
+    ax1.legend()
+    ax1.grid(True, alpha=0.3)
+    
+    ax2.set_xlabel('时间 (s)')
+    ax2.set_ylabel('速度 (rad/s)')
+    ax2.set_title('不同插值方法的速度曲线')
+    ax2.legend()
+    ax2.grid(True, alpha=0.3)
+    
+    plt.tight_layout()
+    plt.savefig('/home/rc2/work/rcwork/trajectory_interpolation_comparison.png', dpi=150)
+    print("已保存对比图到: trajectory_interpolation_comparison.png")
+    
+    print("\n测试完成")
+    print("=" * 60)
+    print("推荐使用:")
+    print("  - quintic: 最平滑，速度和加速度连续")
+    print("  - cosine: 平滑且计算简单")
+    print("  - cubic: 速度连续，比 quintic 稍快")
+    print("  - linear: 最简单但速度会突变")

05_software/real/sim2real/DEPLOYMENT.md

@@ -0,0 +1,50 @@
+# `sim2real` 部署说明
+
+## 模型
+
+当前只使用：
+
+- `sim2real/policies/model_rough.pt`
+
+## 模型契约
+
+- `obs_dim = 53`
+- `action_dim = 16`
+- 单帧输入
+- 无 `base_lin_vel`
+- 无 `height_scan`
+
+## 启动流程
+
+1. 连接硬件
+2. 使能电机
+3. 从当前实测姿态起立
+4. 进入 `stand_balance` 闭环站立
+5. prime 当前观测
+6. 进入 `50Hz` runtime
+
+## 为什么这样改
+
+- 之前版本在 `startup` 后只维持固定 `STAND_POSE`
+- 实机上纯 PD 不足以持续抗姿态扰动
+- 现在增加独立站立闭环，先保证身体支撑，再进入策略
+
+## 运行开关
+
+- `config.yaml > policy.enable_zero_cmd_suppression`
+- `config.yaml > stand_balance.enabled`
+- `config.yaml > policy.hold_zero_command_pose`
+- `config.yaml > policy.command_release_s`
+
+## 纯 Python 命令
+
+默认前提：当前目录就是 `sim2real/`
+
+```bash
+python -m pip install -r requirements-orin.txt
+python tools/alignment_check.py --policy policies/model_rough.pt --manifest deployment_manifest.yaml
+python tools/standalone_check.py
+python main.py --dry-run
+python main.py
+python web/server.py --host 0.0.0.0 --port 8080
+```

05_software/real/sim2real/FACTS_AND_ASSUMPTIONS.md

@@ -0,0 +1,48 @@
+# `FACTS_AND_ASSUMPTIONS`
+
+## 已确认
+
+- 当前部署模型：`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/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/README.md

@@ -0,0 +1,77 @@
+# `sim2real`
+
+当前版本只部署现在这套 `53D -> 16D` 模型，不再兼容旧版 `crawl`、多策略和历史观测。
+
+## 当前部署模型
+
+- 使用文件：`sim2real/policies/model_rough.pt`
+- 来源文件：`model_2000.pt`
+
+## 当前 actor 输入
+
+- 单帧 `53D`
+- 顺序：
+  - `base_ang_vel * 0.25`
+  - `projected_gravity`
+  - `command`
+  - `joint_pos_rel`（12）
+  - `joint_vel_rel * 0.05`（12）
+  - `wheel_vel * 0.05`（4）
+  - `last_actions`（16）
+
+不包含：
+
+- `base_lin_vel`
+- `height_scan`
+
+## 当前控制参数
+
+- 控制频率：`50Hz`
+- 站立保持：`startup/stand_balance.py`
+- `hip_abduction` 缩放：`0.125`
+- 其他腿关节缩放：`0.25`
+- 轮速缩放：`5.0`
+- 腿 LPF：`5Hz`
+- 轮 LPF：`15Hz`
+- 零命令抑制：默认开启，可通过 `config.yaml > policy.enable_zero_cmd_suppression` 关闭
+- 零命令保持：默认开启，可通过 `config.yaml > policy.hold_zero_command_pose` 控制
+- 首次命令解锁：默认开启，可通过 `config.yaml > policy.require_active_command_to_release` 控制
+
+## 当前站立逻辑
+
+- `startup`：从实测姿态过渡到默认站姿
+- `stand_balance`：根据 `IMU roll/pitch + gyro` 动态修正四条腿目标
+- `runtime`：只有站立稳定后才进入策略控制
+
+这次改动的重点是：策略不再承担“先把身体撑住”的职责。
+
+另外当前部署逻辑改成：
+
+- 零命令时默认不让策略直接接管腿和轮，保持站立目标
+- 命令从零变为非零时，策略输出在 `command_release_s` 内平滑放开
+
+## 启动命令
+
+默认前提：当前目录就是 `sim2real/`
+
+纯 `python`：
+
+```bash
+python -m pip install -r requirements-orin.txt
+python tools/alignment_check.py --policy policies/model_rough.pt --manifest deployment_manifest.yaml
+python tools/standalone_check.py
+python main.py --dry-run
+python main.py
+python web/server.py --host 0.0.0.0 --port 8080
+```
+
+Windows 本机：
+
+```bash
+D:\Minicoda3\envs\py10\python.exe -m pip install -r requirements-orin.txt
+D:\Minicoda3\envs\py10\python.exe tools\alignment_check.py --policy policies\model_rough.pt --manifest deployment_manifest.yaml
+D:\Minicoda3\envs\py10\python.exe tools\standalone_check.py
+D:\Minicoda3\envs\py10\python.exe main.py
+D:\Minicoda3\envs\py10\python.exe web\server.py --host 0.0.0.0 --port 8080
+```
+#sim2real/policies/model_rough.pt

05_software/real/sim2real/config.yaml

@@ -0,0 +1,74 @@
+can1_port: "/dev/can1"
+can2_port: "/dev/can2"
+motor_model: "rs-02"
+debug: false
+
+control_freq: 50
+imu_lib_path: null
+
+controller:
+  kp_leg: 80.0
+  kd_leg: 2.5
+  kd_wheel: 2.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
+  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
+  kp_pitch: 0.70
+  kd_roll_rate: 0.03
+  kd_pitch_rate: 0.025
+  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
+  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
+  max_ang_vel: 10.0
+  max_tilt_z: -0.3
+  clip_to_brake: 3
+  imu_age_warn_ms: 60.0
+  imu_age_stop_ms: 200.0
+
+log_dir: "logs"
+log_every: 1

05_software/real/sim2real/deployment_manifest.yaml

@@ -0,0 +1,71 @@
+model:
+  path: "/home/rc2/work/rcwork/real/rc_mjlab/rc_mjlab/model_rough.pt"
+  obs_dim: 53
+  action_dim: 16
+  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:
+  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
+  leg_lpf_hz: 5
+  wheel_lpf_hz: 15
+
+safety:
+  zero_cmd_lin_thresh: 0.05
+  zero_cmd_yaw_thresh: 0.05
+  zero_yaw_rate_thresh: 0.10

05_software/real/sim2real/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/interface/imu_client.py

@@ -0,0 +1,136 @@
+"""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 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):
+        # 优先级 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
+        except ImportError as e:
+            raise ImportError(
+                f"无法导入 Odin1ImuClient，已尝试的路径: {[str(c) for c in candidates]}: {e}"
+            )
+
+        # 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
+        self._initial_gravity: Optional[np.ndarray] = None
+        # 用本机时钟追踪数据新鲜度（stamp_ns 是设备单调时钟，不能和 time.time 混算）
+        self._last_seq: int = -1
+        self._last_fresh_time: float = 0.0
+
+    def version(self) -> str:
+        return self._client.version()
+
+    def start(self, timeout_ms: int = 8000):
+        """启动 IMU 流，并采集若干帧用于重力对齐。"""
+        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):
+        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 表示无新数据。"""
+        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 _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

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

@@ -0,0 +1,310 @@
+"""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._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]]:
+        """快照所有电机的 (pos, vel, torque, update_count)，process_messages 前后比较即可判 fresh。"""
+        snap: Dict[Tuple[int, int], Tuple[float, float, float, 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))
+        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] = {}
+            for key in post:
+                fresh = (pre.get(key) != post[key])
+                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] = {}
+            holdover_this_frame = 0
+            for key, (pos, vel, tor, _) in post.items():
+                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()
+            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,
+            }
+        
+    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))
+            
+            # 轮毂速度目标暂且用 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 != 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
+        return self.mapper.real_to_sim(real_pos)

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

@@ -0,0 +1,99 @@
+"""仿真→实机电机映射。
+
+数据来源：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:
+    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)
+
+    def sim_to_real(self, sim_angles: np.ndarray) -> 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])
+            out[self.CAN_ID_MAP[jk]] = 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 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/interface/real_io.py

@@ -0,0 +1,135 @@
+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
+
+
+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,
+        kp_leg: float = 80.0,
+        kd_leg: float = 2.5,
+        kd_wheel: float = 2.0,
+        debug: bool = False,
+    ):
+        self.control_dt = control_dt
+        self.kp_leg = kp_leg
+        self.kd_leg = 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)
+
+        self.imu_filter = MahonyFilter(kp=2.0, ki=0.0, dt=control_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=control_dt, dim=12)
+        self.lpf_wheels = LowPassFilter(cutoff_freq=15.0, dt=control_dt, dim=4)
+
+        self._last_imu_age_ms = -1.0
+        self._last_imu_fresh = False
+
+    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()
+
+    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()
+        self._last_imu_age_ms = age_ms
+        self._last_imu_fresh = fresh
+        self.quat_wxyz = self.imu_filter.update(accel, gyro)
+        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,
+            "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.kp_leg * kp_scale, self.kd_leg, self.kd_wheel)
+        return target

05_software/real/sim2real/main.py

@@ -0,0 +1,725 @@
+"""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
+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():
+    def factory(can1_port, can2_port, debug):
+        sim2real_root = Path(__file__).resolve().parent
+        for path in (
+            sim2real_root / "vendored",
+            "/home/rc2/work/rcwork/control",
+            "/home/rc2/work/rcwork",
+        ):
+            path_str = str(path)
+            if path_str not in sys.path and Path(path).exists():
+                sys.path.append(path_str)
+        from drivers.motor_driver import RobStrideDriver  # type: ignore
+
+        return RobStrideDriver(can1_port, debug), RobStrideDriver(can2_port, debug)
+
+    return 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 = Path(args.policy) if args.policy else sim2real_root / "policies" / "model_rough.pt"
+    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"],
+        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),
+    )
+    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"]["clip_to_brake"],
+    )
+    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),
+        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)),
+                    )
+                    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/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/mjcf/sim2sim_temp.xml

@@ -0,0 +1,327 @@
+<mujoco model="go2w scene">
+<include file="C:/Users/31560/Documents/00_legged/new_rl/rc_mjlab/mjcf/wheelleg.xml"/>
+<statistic center="3.7 -9.0 0.4" extent="5.0"/>
+<visual>
+<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
+<rgba haze="0.15 0.25 0.35 1"/>
+<global azimuth="90" elevation="-20"/>
+</visual>
+<asset>
+<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
+<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
+<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
+<hfield name="perlin_hfield" size="1.0 0.75 0.2 0.2" file="C:/Users/31560/Documents/00_legged/new_rl/rc_mjlab/sim2sim/terrain/height_field.png"/>
+<hfield name="image_hfield" size="1.0 1.0 0.02 0.1" file="C:/Users/31560/Documents/00_legged/new_rl/rc_mjlab/sim2sim/terrain/unitree_hfield.png"/>
+</asset>
+<worldbody>
+<light pos="0 0 1.5" dir="0 0 -1" directional="true" />
+<geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
+
+<!-- 30cm高墙：旋转90度，沿x轴方向放置，并与T型楼梯中心线 y=-3.50 对齐 -->
+<geom pos="1.8 -7.0 0.15"
+      type="box"
+      size="0.025 0.5 0.15"
+      quat="0.7071068 0.0 0.0 0.7071068" 
+      rgba="1.0 0.9 0.4 1.0"/>
+
+<!-- 沙砾碎木坑：x正方向边界与10度斜坡+x边界对齐，y正边界距斜坡y负边界4m -->
+
+<geom pos="4.8361 -12.5 0.075"
+      type="box"
+      size="0.5 0.5 0.075"
+      quat="0.0 0.0 0.0 1.0" 
+      rgba="0.75 0.72 0.55 1.0"/>
+
+<geom pos="5.8361 -12.0 0.075"
+      type="box"
+      size="0.5 1.0 0.075"
+      quat="0.0 0.0 0.0 1.0" 
+      rgba="0.75 0.72 0.55 1.0"/>
+
+
+
+
+<!-- 限高杆 -->
+<geom pos="6.2 -9.0 0.155"
+      type="cylinder"
+      size="0.025 0.155"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.8 0.1 0.1 1.0" />
+
+<geom pos="5.2 -9.0 0.155"
+      type="cylinder"
+      size="0.025 0.155"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.8 0.1 0.1 1.0" />
+<geom pos="5.7 -9.0 0.325"
+      type="cylinder"
+      size="0.015 0.5"
+      quat="0.7071068 0.0 0.7071068 0.0" 
+      rgba="1.0 0.9 0.4 1.0"/>
+
+
+<!-- 1m × 1m 正方形颜色块，出发区-->
+<geom pos="3.7 -9.0 0.0"
+      type="box"
+      size="0.5 0.5 0.001"
+      rgba="1.0 0.0 0.0 0.35"
+      contype="0"
+      conaffinity="0" />
+
+
+
+<!-- 10cm梯形台阶：T型楼梯，最高平台与 x=5.7 y=-3.5 平台在y轴方向对齐 -->
+
+<geom pos="1.80 -4.75 0.05" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0" />
+<geom pos="1.80 -4.45 0.15" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="1.80 -4.15 0.25" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
+
+<!-- 最高平台：y = -3.50，与目标平台y轴对齐 -->
+<geom pos="1.80 -3.50 0.35" type="box" size="0.5 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
+
+<geom pos="1.80 -2.85 0.25" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="1.80 -2.55 0.15" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="1.80 -2.25 0.05" type="box" size="0.15 0.5 0.05" quat="0.7071068 0.0 0.0 0.7071068" rgba="0.75 0.72 0.55 1.0"/>
+
+
+<!-- 顶部平台向 +x 方向连接地面的10cm台阶，同样y轴移动到 -3.50 -->
+
+<geom pos="2.45 -3.50 0.25" type="box" size="0.15 0.5 0.05" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="2.75 -3.50 0.15" type="box" size="0.15 0.5 0.05" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="3.05 -3.50 0.05" type="box" size="0.15 0.5 0.05" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+
+
+
+
+<!-- 斜坡木桥A木桥B -->
+<geom pos="1.8 -0.88 0.1" type="box" size="0.40 0.5 0.005" quat="0.701836 0.086175 -0.086175 0.701836" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="1.8 0.0 0.10" type="box" size="0.5 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="2.65 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="3.2 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="3.75 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="4.3 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="4.85 0.0 0.10" type="box" size="0.2 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="5.7 -0.5 0.10" type="box" size="0.5 1.0 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="4.8002 -1.0 0.0951"
+      type="box"
+      size="0.4133 0.5 0.005"
+      quat="0.992546 0.0 -0.121869 0.0" 
+      rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="5.35 -2.25 0.10" type="box" size="0.1 0.75 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="5.65 -2.25 0.10" type="box" size="0.1 0.75 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="5.95 -2.25 0.10" type="box" size="0.1 0.75 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<geom pos="5.7 -3.5 0.10" type="box" size="0.5 0.5 0.10" quat="1.0 0.0 0.0 0.0" rgba="0.75 0.72 0.55 1.0"/>
+<!-- 10度斜坡：宽4m，斜坡 y正方向边缘 与平台 y正方向边缘 对齐 -->
+<geom pos="4.6338 -5.0 0.0951"
+      type="box"
+      size="0.5759 2.0 0.005"
+      quat="0.9961947 0.0 -0.0871557 0.0" 
+      rgba="0.75 0.72 0.55 1.0"/>
+<!-- 新建10度斜坡：宽3m，高端与前一个10度斜坡高端衔接，向+x方向下坡 -->
+<geom pos="5.7681 -5.5 0.0951"
+      type="box"
+      size="0.5759 1.5 0.005"
+      quat="0.9961947 0.0 0.0871557 0.0" 
+      rgba="0.75 0.72 0.55 1.0"/>
+
+
+
+
+
+<!--绕杆-->
+<!-- 直径1m圆形颜色块，仅显示，不碰撞 -->
+<geom pos="1.8 -10.1 0.0"
+      type="cylinder"
+      size="0.1 0.001"
+      rgba="1.0 0.0 0.0 0.35"
+      contype="0"
+      conaffinity="0" />
+
+<geom pos="3.2 -12.5 0.0"
+      type="cylinder"
+      size="0.1 0.001"
+      rgba="1.0 0.0 0.0 0.35"
+      contype="0"
+      conaffinity="0" />
+
+<geom pos="1.55 -12.75 0.0"
+      type="cylinder"
+      size="0.1 0.001"
+      rgba="1.0 0.0 0.0 0.35"
+      contype="0"
+      conaffinity="0" />
+
+<!-- 原杆 -->
+<geom pos="1.8 -10.5 0.02"
+      type="cylinder"
+      size="0.05 0.02"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+<geom pos="1.8 -10.5 0.37"
+      type="cylinder"
+      size="0.015 0.33"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+
+<!-- y轴负方向第1根：间隔1m -->
+<geom pos="1.8 -11.5 0.02"
+      type="cylinder"
+      size="0.05 0.02"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+<geom pos="1.8 -11.5 0.37"
+      type="cylinder"
+      size="0.015 0.33"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+
+<!-- y轴负方向第2根：继续间隔1m -->
+<geom pos="1.8 -12.5 0.02"
+      type="cylinder"
+      size="0.05 0.02"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+<geom pos="1.8 -12.5 0.37"
+      type="cylinder"
+      size="0.015 0.33"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+
+<!-- x轴正方向第3根：继续间隔1m -->
+<geom pos="2.8 -12.5 0.02"
+      type="cylinder"
+      size="0.05 0.02"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+<geom pos="2.8 -12.5 0.37"
+      type="cylinder"
+      size="0.015 0.33"
+      quat="1.0 0.0 0.0 0.0"
+      rgba="0.75 0.72 0.55 1.0" />
+
+
+<!--===================================================================其他障碍=====================================================================================-->
+
+<!-- 5cm台阶 -->
+<geom pos="1.0 2.0 0.025" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="1.3 2.0 0.075" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="1.6 2.0 0.125" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="1.9 2.0 0.175" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="2.2 2.0 0.225" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="2.5 2.0 0.275" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="2.8 2.0 0.325" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="3.45 2.0 0.375" type="box" size="0.5 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="4.1 2.0 0.325" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="4.4 2.0 0.275" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="4.7 2.0 0.225" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="5.0 2.0 0.175" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="5.3 2.0 0.125" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="5.6 2.0 0.075" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+<geom pos="5.9 2.0 0.025" type="box" size="0.15 1.0 0.025" quat="1.0 0.0 0.0 0.0" />
+
+<!-- 斜坡 -->
+<geom pos="2.0 4.0 0.1" type="box" size="1.5 0.75 0.005" quat="0.9950041652780258 0.0 -0.09983341664682815 0.0" />
+
+
+
+
+<geom pos="1.4 6.0 0.165" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="1.6 6.0 0.275" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="1.8 6.0 0.385" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="2.0 6.0 0.495" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="2.2 6.0 0.605" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="2.4 6.0 0.715" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="2.5999999999999996 6.0 0.825" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="2.8 6.0 0.9349999999999999" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="3.0 6.0 1.045" type="box" size="0.1 0.75 0.0049999999999999975" quat="1.0 0.0 0.0 0.0"/>
+<geom pos="-2.3179973398407565 5.173660321080885 -0.25" type="box" size="0.2568216778785459 0.2608020098770089 0.2619541072037832" quat="0.9930658271270357 -0.05910360995856133 -0.06731065544310916 0.0761334482746007"/>
+<geom pos="-2.3179973398407565 5.3620612607436735 -0.25" type="box" size="0.23778028947050467 0.26051225137569556 0.27137457425982286" quat="0.9959802827127009 0.07435995796871331 0.0004862731479656538 -0.04993632582444846"/>
+<geom pos="-2.3179973398407565 5.545897059602109 -0.25" type="box" size="0.23841994611378936 0.2717839884518381 0.22585827399286504" quat="0.9983504429181294 -0.004811901627841528 0.03471877670768104 -0.045473566737405234"/>
+<geom pos="-2.3179973398407565 5.7436240471772795 -0.25" type="box" size="0.2552179019048769 0.2548992578792955 0.22547735976326444" quat="0.9968270877924189 -0.029673908987198697 0.06777847718526858 -0.029347814214192768"/>
+<geom pos="-2.3179973398407565 5.940214647011584 -0.25" type="box" size="0.24313116620329878 0.2372064979204117 0.26079933745117434" quat="0.9952106364954509 0.05088987714749159 -0.07605843245051555 -0.03436748846516202"/>
+<geom pos="-2.3179973398407565 6.165430585901471 -0.25" type="box" size="0.24786042386990592 0.2322559052231109 0.2644037606269708" quat="0.9936075351397807 -0.05017393314139304 0.06986162224674641 -0.07311632022760194"/>
+<geom pos="-2.3179973398407565 6.315657865031069 -0.25" type="box" size="0.23704265198840277 0.24982080672772003 0.2530694373586838" quat="0.9981716459301547 0.036179123385437884 0.04523497974247521 0.017269420947002005"/>
+<geom pos="-2.3179973398407565 6.489372835072359 -0.25" type="box" size="0.2647428927965494 0.2716292502682415 0.23725049444938928" quat="0.9954041516289313 0.019099981466976248 0.07663366510923347 0.05415761257454444"/>
+<geom pos="-2.094119617957536 5.194943631570213 -0.25" type="box" size="0.23176840693038148 0.23782936054799508 0.2282032657053922" quat="0.9973366591969123 0.030692664684553082 -0.030015392527106853 0.058962910104125923"/>
+<geom pos="-2.094119617957536 5.441090326561234 -0.25" type="box" size="0.2602142310926322 0.27213502289176367 0.2574009440402366" quat="0.9948915480473169 0.023650461407535205 0.08508706956405496 -0.04890453856469255"/>
+<geom pos="-2.094119617957536 5.642958951230403 -0.25" type="box" size="0.24800055056479955 0.24050676557282252 0.23522489807277194" quat="0.9912055235117067 0.06188914817453858 -0.09720856678264424 -0.0650525790586179"/>
+<geom pos="-2.094119617957536 5.884810142659838 -0.25" type="box" size="0.24637516954806898 0.24364583504893206 0.2682443460752295" quat="0.9964495083946477 -0.07467561549070643 -0.038754439691442995 0.003165924091257204"/>
+<geom pos="-2.094119617957536 6.129893093768145 -0.25" type="box" size="0.2378958269703999 0.25045408022075055 0.24760364656411665" quat="0.9946240462872651 -0.03963038800955788 -0.08917648605658504 0.03464091840526007"/>
+<geom pos="-2.094119617957536 6.31135792935651 -0.25" type="box" size="0.2612154064649683 0.2252849660353503 0.26933214617336004" quat="0.997742809172301 0.019590451180868298 0.06391648942914147 -0.006339033565912353"/>
+<geom pos="-2.094119617957536 6.4905285510291915 -0.25" type="box" size="0.22768983323309258 0.23025468157122184 0.2748062344121069" quat="0.9940843963100783 0.05105793020576152 0.009164090402224958 -0.0954218016127878"/>
+<geom pos="-2.094119617957536 6.650993287072798 -0.25" type="box" size="0.25443787917913946 0.24575387105878618 0.22934424641420587" quat="0.9953832160779188 -0.0075142647566912866 0.08305501342257929 0.04751477371219154"/>
+<geom pos="-1.8951777534978893 5.178742263102535 -0.25" type="box" size="0.256127371544264 0.24353861032614957 0.2714578153598507" quat="0.996547165679354 -0.008558846106514282 0.009384988866828401 0.08205129318749771"/>
+<geom pos="-1.8951777534978893 5.353646654030374 -0.25" type="box" size="0.23858897994497189 0.2426691342857623 0.26961659613969635" quat="0.997689480058865 0.014145078555906384 0.009115651940537827 -0.06582190381793727"/>
+<geom pos="-1.8951777534978893 5.575640096199311 -0.25" type="box" size="0.24430646334884096 0.2676093509240798 0.23693271670520474" quat="0.9976019172675967 -0.06794745094616256 0.006187887696065785 -0.011630503849579829"/>
+<geom pos="-1.8951777534978893 5.728196634759373 -0.25" type="box" size="0.25668697148716296 0.2743986770827004 0.23696403861156407" quat="0.9946311966964959 -0.0774649076913635 0.05866511796477466 0.03558615698054331"/>
+<geom pos="-1.8951777534978893 5.954288148947323 -0.25" type="box" size="0.2748179458028994 0.25407956175122554 0.25142548243710355" quat="0.9927720663552536 0.03885622734718657 0.06180632563910765 0.09525647469886914"/>
+<geom pos="-1.8951777534978893 6.198076908897641 -0.25" type="box" size="0.23522918501552104 0.2714895927340788 0.23659922178360912" quat="0.9944408196408429 0.027722532803612636 -0.06883505448003607 -0.07470376618171105"/>
+<geom pos="-1.8951777534978893 6.356335079330304 -0.25" type="box" size="0.253243646653926 0.26648639763264487 0.22751627090926196" quat="0.9924345646356405 0.06351743627133029 0.09661714031088077 -0.041283149154730255"/>
+<geom pos="-1.8951777534978893 6.6037178705715744 -0.25" type="box" size="0.2664884535468762 0.26472237049442093 0.2545826559482188" quat="0.996208812433608 0.04792667434016892 0.03088022810200046 -0.06570728596343135"/>
+<geom pos="-1.7450143557797366 5.181798049538029 -0.25" type="box" size="0.22764974830145798 0.2314500042225232 0.26635647118774647" quat="0.996420562587967 -0.02530598413067966 -0.01712092616039989 -0.07881968983996092"/>
+<geom pos="-1.7450143557797366 5.396539066742657 -0.25" type="box" size="0.24515338305934362 0.25502436912192245 0.23509532716059323" quat="0.9974817825235419 0.033698559354477776 -0.06057332552735373 -0.01501242370995589"/>
+<geom pos="-1.7450143557797366 5.5477605493550115 -0.25" type="box" size="0.2368415768982088 0.2653984778068547 0.25193186806340717" quat="0.9933816957092091 -0.04426535864216467 0.0892264769079807 0.057201577537394625"/>
+<geom pos="-1.7450143557797366 5.764238738853998 -0.25" type="box" size="0.257475541143157 0.25587521442146555 0.2684267956125346" quat="0.9955849335110497 0.004076118120639651 -0.09299366924131539 0.012091439447074191"/>
+<geom pos="-1.7450143557797366 5.942956213887727 -0.25" type="box" size="0.23647345784635188 0.22779489919605103 0.2690566454457882" quat="0.9997372307105926 0.020286738832439623 -0.010113822112874779 0.003410038259163467"/>
+<geom pos="-1.7450143557797366 6.162981335139796 -0.25" type="box" size="0.2336152227884161 0.23785626414299832 0.26272786991330355" quat="0.9954569549676405 0.08638675499005252 -0.03150236191317218 -0.024705881136540285"/>
+<geom pos="-1.7450143557797366 6.344025407207907 -0.25" type="box" size="0.2364006101773331 0.23674709170116234 0.2660167000427503" quat="0.9941020005048972 -0.09350614404282417 -0.05493059192638792 0.0006660998579442658"/>
+<geom pos="-1.7450143557797366 6.5791872438688825 -0.25" type="box" size="0.259653062502181 0.26359758888480966 0.27170867851854713" quat="0.9955751902075182 0.06572529509454274 0.04059253213564211 -0.05350207998588654"/>
+<geom pos="-1.4950537649717406 5.196975242498044 -0.25" type="box" size="0.24902209995429173 0.24604186796843594 0.26555264385759036" quat="0.9981753741369879 0.027059282981497578 0.02078151255996191 0.049818133312398136"/>
+<geom pos="-1.4950537649717406 5.415093649392617 -0.25" type="box" size="0.22561317519118573 0.23246623591498758 0.2516053992906602" quat="0.9961431033663982 -0.019679813255757937 0.07777937635153075 0.035524515199325105"/>
+<geom pos="-1.4950537649717406 5.596072881787104 -0.25" type="box" size="0.2545347271371952 0.2527292932516396 0.272364707011277" quat="0.9987990019474599 -0.0468132050914785 0.002764373695484487 -0.01419280718860587"/>
+<geom pos="-1.4950537649717406 5.788444207457658 -0.25" type="box" size="0.257823578756761 0.22815201323013437 0.2506904868770564" quat="0.9907800223935689 0.08207700037219429 -0.06679273748567588 -0.08459931119619979"/>
+<geom pos="-1.4950537649717406 5.962479724512656 -0.25" type="box" size="0.23447002037921025 0.260091883647859 0.2613547781123637" quat="0.994185747243896 -0.045779429335849345 -0.09537527521663794 -0.020062420196245392"/>
+<geom pos="-1.4950537649717406 6.137620949716146 -0.25" type="box" size="0.25502378303486756 0.24137626830945555 0.26521755821448284" quat="0.9954511067004865 0.04827241542514884 0.023378972724717166 -0.07874193109224191"/>
+<geom pos="-1.4950537649717406 6.345742058941403 -0.25" type="box" size="0.23763376291279575 0.27259418014745557 0.24184880568666417" quat="0.9979730938550316 -0.0521706328108408 -0.03256129013590577 0.01636127740178204"/>
+<geom pos="-1.4950537649717406 6.548966807280673 -0.25" type="box" size="0.23004532593424165 0.24736965888987583 0.22917624237245732" quat="0.993938322382385 -0.041286377558295506 -0.09233464220936707 0.04308549844056845"/>
+<geom pos="-1.2872521554407157 5.194072124237239 -0.25" type="box" size="0.2725857334366114 0.23611730648841156 0.25109418723334265" quat="0.9870880287314421 -0.09538067591152324 -0.0888119213223197 -0.09312460914696315"/>
+<geom pos="-1.2872521554407157 5.418639818976418 -0.25" type="box" size="0.2326397456179607 0.2609646699674687 0.2717115772948157" quat="0.9928206111485406 -0.07383059897166898 -0.08473752136579614 0.040936892980589765"/>
+<geom pos="-1.2872521554407157 5.655974569843163 -0.25" type="box" size="0.22633109461398734 0.25911291311168594 0.23532484499883452" quat="0.9947769304653071 -0.023834507399340135 0.09176246279588879 -0.037820963666801724"/>
+<geom pos="-1.2872521554407157 5.8981628648303595 -0.25" type="box" size="0.27278934520816167 0.2559269445001904 0.26076472835929454" quat="0.9936318869346414 0.0561019165840355 0.09234219275279033 0.0319557140415977"/>
+<geom pos="-1.2872521554407157 6.064939027275534 -0.25" type="box" size="0.2481756437845515 0.2613413397088905 0.24788858471207542" quat="0.9992310163557966 0.006919915638721734 0.03652449975321466 -0.012468024618680441"/>
+<geom pos="-1.2872521554407157 6.288670079370122 -0.25" type="box" size="0.2574127585385611 0.27445220033632356 0.22507618952620437" quat="0.9984337612791753 0.04947883993440395 -0.0010074967830211658 -0.026093173185657285"/>
+<geom pos="-1.2872521554407157 6.4987760234638605 -0.25" type="box" size="0.2442462188069663 0.2639082925274208 0.24918893213917415" quat="0.9910480688126146 0.08639971284060838 -0.0903343762232188 -0.04688832899783648"/>
+<geom pos="-1.2872521554407157 6.675611985491267 -0.25" type="box" size="0.2507168814441121 0.26699708557208374 0.26588306060638556" quat="0.9968882348111167 0.009466759168649483 -0.06920917552433178 0.03652831489763918"/>
+<geom pos="-1.0678149575697586 5.238251070535694 -0.25" type="box" size="0.2331754586513582 0.22873009754409884 0.2593258638743009" quat="0.9962563091989393 0.014506022500180732 0.069646748226122 0.049114887295595266"/>
+<geom pos="-1.0678149575697586 5.471732496077581 -0.25" type="box" size="0.2653495693182789 0.26581370557074685 0.2509273010188512" quat="0.9948019811445705 -0.05898118120008214 0.04034485064897968 -0.07254330845220838"/>
+<geom pos="-1.0678149575697586 5.691153717662407 -0.25" type="box" size="0.26309077142424103 0.26536949948987093 0.26566703066149744" quat="0.998331608401392 -0.025703385975953945 -0.05139460706256987 0.005650661991725779"/>
+<geom pos="-1.0678149575697586 5.938910564562482 -0.25" type="box" size="0.23082606038737274 0.23890770539441533 0.25941695887199245" quat="0.99040745217393 -0.09494180013051996 0.05600240658814475 -0.08332384846283197"/>
+<geom pos="-1.0678149575697586 6.13746533389903 -0.25" type="box" size="0.2676431215498913 0.2569308659994288 0.24597694927356487" quat="0.9955894165044442 0.04511524606234168 -0.003741035154326279 -0.08217258042101841"/>
+<geom pos="-1.0678149575697586 6.328213254628996 -0.25" type="box" size="0.24188540919954635 0.25556145122229207 0.2605619987765001" quat="0.9929731934387934 -0.06598945456828725 -0.06809928840246085 0.07079629875087447"/>
+<geom pos="-1.0678149575697586 6.554891603524267 -0.25" type="box" size="0.23121916641107804 0.25266417867731916 0.25489063309084503" quat="0.9984565221200774 -0.02714563973423854 0.029423903667849492 0.03849573446816443"/>
+<geom pos="-1.0678149575697586 6.718482966003368 -0.25" type="box" size="0.27418971733780156 0.2623437838864593 0.23694037285314332" quat="0.9968347573500278 0.015128289848683302 0.06990516881209438 0.03471121949050888"/>
+<geom pos="-0.8851136474992356 5.233272057743225 -0.25" type="box" size="0.2587161160845902 0.2542459313914242 0.25268742624288776" quat="0.9956323623605605 0.07455213252171443 -0.05585558926184348 0.006191260373025539"/>
+<geom pos="-0.8851136474992356 5.410759300563648 -0.25" type="box" size="0.24019625793101548 0.2509280955260936 0.26698317271101046" quat="0.9960056830073374 0.025487333399660517 0.08516393482003458 -0.008377318140719903"/>
+<geom pos="-0.8851136474992356 5.622546965631826 -0.25" type="box" size="0.22527085609750916 0.22924847380626232 0.23073331588883172" quat="0.9982201065088221 0.016292210993739946 0.013620740418279952 0.05572843307423328"/>
+<geom pos="-0.8851136474992356 5.856118124892678 -0.25" type="box" size="0.26833255247166926 0.2512767990265972 0.2502231376336179" quat="0.9914910532543448 0.0657880493114159 0.05073139628229043 -0.10021850784978792"/>
+<geom pos="-0.8851136474992356 6.010914595891683 -0.25" type="box" size="0.24926441451407527 0.22868800964152894 0.26501630221174116" quat="0.990736605756697 -0.0765632176593298 -0.09876375004658954 0.05314859727296924"/>
+<geom pos="-0.8851136474992356 6.1657945570092965 -0.25" type="box" size="0.26823427325895977 0.263134568634285 0.23692064318485426" quat="0.9903439387899988 0.07219524819688558 0.09310840228148876 0.07305856872598579"/>
+<geom pos="-0.8851136474992356 6.383066094410735 -0.25" type="box" size="0.23772929143507357 0.2619329708548053 0.23258884134606547" quat="0.9928463525385446 0.07348810370848206 0.09252710448645944 -0.017156742103069993"/>
+<geom pos="-0.8851136474992356 6.551482176174038 -0.25" type="box" size="0.25127741834221506 0.25307864976328337 0.234931895271364" quat="0.994062479132312 -0.084918495271965 -0.05565932062022146 -0.03912386445844099"/>
+<geom pos="-0.6441452943469552 5.193190826541208 -0.25" type="box" size="0.27016649011158844 0.23778885128164629 0.25859862297032477" quat="0.9957774962333289 0.08893738517887846 -0.021654911548168992 0.006955883744530942"/>
+<geom pos="-0.6441452943469552 5.407266358883468 -0.25" type="box" size="0.24982888074520473 0.26972381569065607 0.2275629646713632" quat="0.9942571418988088 0.02444442288698024 0.050336521935401994 -0.09122193010664256"/>
+<geom pos="-0.6441452943469552 5.56363948710079 -0.25" type="box" size="0.24725848381417467 0.2326432801330426 0.2476341019968084" quat="0.9935769557551848 -0.05688836927692267 -0.06858315894908604 0.0697488117593134"/>
+<geom pos="-0.6441452943469552 5.716461923308758 -0.25" type="box" size="0.226746163574222 0.25188961955216527 0.24650452053954758" quat="0.9981029078160676 0.03471313574303335 -0.0493522060742233 -0.012245136651074443"/>
+<geom pos="-0.6441452943469552 5.896430073001983 -0.25" type="box" size="0.25284949298017617 0.23421620066432108 0.2621382648463894" quat="0.9996413412447853 0.009523771195346772 -0.02374945051322109 -0.007902547492127014"/>
+<geom pos="-0.6441452943469552 6.110845613879558 -0.25" type="box" size="0.23421403581041014 0.2504556848552827 0.24096555776925194" quat="0.9982371379292613 0.05906199571247059 0.004184466367765714 0.004097238396071534"/>
+<geom pos="-0.6441452943469552 6.301410327752269 -0.25" type="box" size="0.2296817145699627 0.2701766916372966 0.22803599234835198" quat="0.9993879202059675 0.00903361239651759 0.0071600522792900426 -0.03302896372607247"/>
+<geom pos="-0.6441452943469552 6.470340592771366 -0.25" type="box" size="0.2742166750011587 0.23990439594655139 0.2609404931878803" quat="0.9954980192986956 -0.06477558432537366 -0.03478863652617152 0.059812774691783824"/>
+<geom pos="-0.43033316974190594 5.176946043237525 -0.25" type="box" size="0.2511344405622137 0.26383180387822514 0.2729516287367074" quat="0.9939152287364423 -0.054017256979398534 -0.08418383902399898 -0.0461273810376057"/>
+<geom pos="-0.43033316974190594 5.409693792302493 -0.25" type="box" size="0.23878438774624536 0.22858057493504688 0.24808981791323623" quat="0.9942721168135948 -0.06322741578652484 -0.032958930763236916 -0.0796175891553822"/>
+<geom pos="-0.43033316974190594 5.573593446642387 -0.25" type="box" size="0.2647545698926724 0.25215736713350106 0.25522885339490087" quat="0.9971918839448468 -0.004732654353889047 0.07222000940536273 0.019241071144380294"/>
+<geom pos="-0.43033316974190594 5.819531161619798 -0.25" type="box" size="0.2538318010250847 0.23661725487197 0.26323696729639623" quat="0.9929867679849618 0.07588268800385974 -0.046355180482866284 -0.07791208834634974"/>
+<geom pos="-0.43033316974190594 6.047888701789067 -0.25" type="box" size="0.25684720236482583 0.25568474221031684 0.24397094296107352" quat="0.9925256546500537 -0.08214715587137877 0.06615741576234473 0.06138294537877935"/>
+<geom pos="-0.43033316974190594 6.283676056613722 -0.25" type="box" size="0.2432692787269645 0.2742601402399693 0.2689467974776609" quat="0.9989897410981501 0.0047037010179340685 0.0014590493232283363 -0.04466814919444852"/>
+<geom pos="-0.43033316974190594 6.458239009480634 -0.25" type="box" size="0.2474330649412728 0.2551435998627988 0.23002773988805483" quat="0.9982311862818315 0.0506459597703481 0.025989046975463247 -0.01714802993392417"/>
+<geom pos="-0.43033316974190594 6.703309377175586 -0.25" type="box" size="0.2301029552870502 0.2556223065028505 0.2349557527965884" quat="0.9948672058648923 -0.034182171957127715 -0.0030330068097560517 -0.09519255582537542"/>
+<geom type="hfield" hfield="perlin_hfield" pos="-1.5 4.0 0.0" quat="1.0 0.0 0.0 0.0"/>
+<geom type="hfield" hfield="image_hfield" pos="-1.5 2.0 0.0" quat="0.7073882691671998 0.0 0.0 -0.706825181105366"/>
+</worldbody>
+</mujoco>

05_software/real/sim2real/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/policy/policy_runner.py

@@ -0,0 +1,176 @@
+from pathlib import Path
+
+import numpy as np
+import torch
+import torch.nn as nn
+
+
+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)
+
+
+def load_policy(model_path: Path, device: torch.device) -> PolicyMLP:
+    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
+    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,
+    ):
+        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)
+        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"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) -> 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}."
+            )
+
+        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 + 0.02 / 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/requirements-orin.txt

@@ -0,0 +1,7 @@
+numpy
+PyYAML
+torch
+pyserial
+
+# Optional:
+# pynput        # only needed for CLI keyboard control

05_software/real/sim2real/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/safety/safety_monitor.py

@@ -0,0 +1,107 @@
+"""三级安全监控（对应方法论 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 = 3):
+        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.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.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/startup/pose_initializer.py

@@ -0,0 +1,329 @@
+"""起立姿态初始化器（实测起点版本）。
+
+设计：
+  - 不再假设机器人的物理起始姿态（不再有 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)
+
+
+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,
+                 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.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(self.io.kp_leg * kp_scale),
+                kd_leg_cmd=float(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)
+            while time.perf_counter() < next_exec:
+                pass
+        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)
+
+        start_pose = self.io.read_measured_pose().astype(np.float32).copy()
+        start_pose[12:] = 0.0   # 轮子起点固定为 0 速度
+
+        # === 2. 偏差检查 ===
+        diff = np.abs(start_pose[:12] - target_pose[:12])
+        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 * (target_pose[:12] - start_pose[:12])
+            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(joint_pos[:12] - target_pose[:12])))
+            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/startup/stand_balance.py

@@ -0,0 +1,120 @@
+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
+    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.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.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._last_debug = StandBalanceDebug(0.0, 0.0, 0.0, 0.0, 0.9, -1.8, 0.0, 0.0, False)
+
+    @property
+    def last_debug(self) -> StandBalanceDebug:
+        return self._last_debug
+
+    def reset(self) -> None:
+        self._stable_time = 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
+        pitch_corr = -self.kp_pitch * pitch - self.kd_pitch_rate * pitch_rate
+        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
+            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 + 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,
+            stable=stable,
+        )
+        return target
+
+    def is_stable(self) -> bool:
+        return self._stable_time >= self.enter_hold_s

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

@@ -0,0 +1,171 @@
+"""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 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
+
+    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")
+
+    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,
+    )
+    obs_mean = runner.policy.obs_mean.detach().cpu().numpy()
+    obs_std = runner.policy.obs_std.detach().cpu().numpy()
+    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_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")
+
+        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 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 .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/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/tools/logger.py

@@ -0,0 +1,239 @@
+"""Logging helpers for sim2real runs.
+
+Each session writes:
+- `state.csv`: high-rate state stream
+- `events.jsonl`: event / milestone stream
+"""
+
+import json
+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._write_state_header()
+        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 += ["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,
+        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 += [_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._state_fp.write(",".join(parts) + "\n")
+
+    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._events_fp.write(json.dumps(record, ensure_ascii=False) + "\n")
+        self._events_fp.flush()
+        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._state_fp.flush()
+            self._events_fp.flush()
+
+    def close(self):
+        if self._closed:
+            return
+        self.event("LOG_END")
+        self._state_fp.flush()
+        self._state_fp.close()
+        self._events_fp.flush()
+        self._events_fp.close()
+        self._closed = True
+        print(f"[Log] saved -> {self.dir}")
+
+
+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,
+        "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/tools/math_utils.py

@@ -0,0 +1,108 @@
+"""数学工具 — 与 rc_mjlab/sim2sim/tools/math_utils.py 数值完全一致。"""
+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) -> np.ndarray:
+        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 * self.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 * self.dt
+        self.q /= (np.linalg.norm(self.q) + 1e-9)
+        return self.q

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

@@ -0,0 +1,97 @@
+"""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",
+    "policies/model_rough.pt",
+    "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",
+    "torch",
+    "serial",
+]
+
+OPTIONAL_IMPORTS = [
+    ("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}")
+
+    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/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/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/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/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/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/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/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/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/vendored/odin1_imu/include/odin1_imu_bridge.h

@@ -0,0 +1,86 @@
+#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 共享使用
+ */
+typedef struct odin1_imu_sample_t {
+    float accel_x;
+    float accel_y;
+    float accel_z;
+    float gyro_x;
+    float gyro_y;
+    float gyro_z;
+    uint64_t stamp_ns;
+    uint64_t sequence;
+} odin1_imu_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);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

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

@@ -0,0 +1,124 @@
+#!/usr/bin/python3
+"""ODIN1 IMU ctypes 封装."""
+
+from __future__ import annotations
+
+import ctypes
+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 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
+
+    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 数据到达."""
+
+        result = self._lib.odin1_imu_wait_for_data(timeout_ms)
+        if result < 0:
+            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
+
+    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

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

@@ -0,0 +1,376 @@
+#include "odin1_imu_bridge.h"
+
+#include "lidar_api.h"
+#include "lidar_api_type.h"
+
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <cstring>
+#include <deque>
+#include <mutex>
+#include <string>
+#include <thread>
+
+namespace {
+
+constexpr const char* kBridgeVersion = "0.1.0";
+constexpr std::size_t kMaxQueueSize = 1024;
+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;
+
+std::mutex g_error_mutex;
+std::string g_last_error = "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;
+}
+
+/**
+ * 输入: 无
+ * 输出: 无
+ * 作用: 清空内部 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;
+}
+
+/**
+ * 输入: raw_sample[const imu_convert_data_t*]
+ * 输出: odin1_imu_sample_t
+ * 作用: 将 SDK IMU 结构转换为 bridge 对外结构
+ */
+odin1_imu_sample_t convert_sample(const imu_convert_data_t* raw_sample) {
+    odin1_imu_sample_t converted{};
+    if (raw_sample == nullptr) {
+        return converted;
+    }
+
+    converted.accel_x = raw_sample->accel_x;
+    converted.accel_y = raw_sample->accel_y;
+    converted.accel_z = raw_sample->accel_z;
+    converted.gyro_x = raw_sample->gyro_x;
+    converted.gyro_y = raw_sample->gyro_y;
+    converted.gyro_z = raw_sample->gyro_z;
+    converted.stamp_ns = raw_sample->stamp;
+    converted.sequence = raw_sample->sequence;
+    return converted;
+}
+
+/**
+ * 输入: 无
+ * 输出: 无
+ * 作用: 安全关闭当前设备与 SDK 资源
+ */
+void cleanup_device_and_sdk() {
+    std::lock_guard<std::mutex> lock(g_state_mutex);
+
+    if (g_device != nullptr) {
+        try {
+            if (g_stream_started.load()) {
+                lidar_deactivate_stream_type(g_device, LIDAR_DT_RAW_IMU);
+                lidar_stop_stream(g_device, kDefaultMode);
+                g_stream_started = false;
+            }
+
+            lidar_unregister_stream_callback(g_device);
+            lidar_close_device(g_device);
+            lidar_destory_device(g_device);
+        } catch (...) {
+        }
+        g_device = nullptr;
+    }
+
+    if (g_sdk_initialized.load()) {
+        try {
+            lidar_system_deinit();
+        } catch (...) {
+        }
+        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;
+    }
+
+    if (data->type != LIDAR_DT_RAW_IMU) {
+        return;
+    }
+
+    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;
+    }
+
+    g_queue_cv.notify_all();
+}
+
+/**
+ * 输入: 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;
+    }
+
+    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;
+    cleanup_device_and_sdk();
+    clear_queue_locked_state();
+    g_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;
+    }
+
+    std::lock_guard<std::mutex> lock(g_queue_mutex);
+    if (!g_has_latest_sample) {
+        return 0;
+    }
+
+    *out_sample = g_latest_sample;
+    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();
+}
+
+}  // extern "C"

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

@@ -0,0 +1,353 @@
+"""Minimal HTTP + SSE server for the sim2real web console."""
+
+from __future__ import annotations
+
+import argparse
+import json
+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():
+        def factory(can1_port, can2_port, debug):
+            sim2real_root = Path(__file__).resolve().parents[1]
+            for path in (
+                sim2real_root / "vendored",
+                "/home/rc2/work/rcwork/control",
+                "/home/rc2/work/rcwork",
+            ):
+                path_str = str(path)
+                if path_str not in sys.path and Path(path).exists():
+                    sys.path.append(path_str)
+            from drivers.motor_driver import RobStrideDriver  # type: ignore
+
+            return RobStrideDriver(can1_port, debug), RobStrideDriver(can2_port, debug)
+
+        return 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
+
+                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):
+                pass
+
+            def control_mit(self, *args, **kwargs):
+                pass
+
+        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.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/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:
+        try:
+            SESSION.estop()
+        except Exception:
+            pass
+        try:
+            SESSION._do_disconnect()
+        except Exception:
+            pass
+        httpd.server_close()
+
+
+if __name__ == "__main__":
+    main()

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

@@ -0,0 +1,480 @@
+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;
+
+  setButtonEnabled("btn-connect", !busy && stage === "DISCONNECTED");
+  setButtonEnabled("btn-disconnect", !busy && connected);
+  setButtonEnabled("btn-enable", !busy && ["CONNECTED", "FAULTED"].includes(stage));
+  setButtonEnabled("btn-disable", !busy && enabled);
+  setButtonEnabled("btn-startup", !busy && canStartup);
+  setButtonEnabled("btn-runtime-start", !busy && canRuntimeStart);
+  setButtonEnabled("btn-runtime-stop", !busy && canRuntimeStop);
+  setButtonEnabled("btn-reset-estop", !busy && stage === "ESTOPPED");
+  setButtonEnabled("btn-estop", connected);
+}
+
+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 trackingErr = Math.max(
+    ...(state.joint_pos || []).slice(0, 12).map((pos, i) => Math.abs(pos - ((state.target || [])[i] || 0))),
+    0,
+  );
+  el.innerHTML = [
+    metric("phase", safeText(state.phase, "?")),
+    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("track_err", trackingErr.toFixed(3), trackingErr > 0.5 ? "bad" : trackingErr > 0.2 ? "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-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>
+      <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 || [];
+  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" : "";
+  }
+}
+
+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-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/web/static/index.html

@@ -0,0 +1,190 @@
+<!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>
+      </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</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">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="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/web/static/style.css

@@ -0,0 +1,394 @@
+/* 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); }
+
+/* Toggle Switch */
+.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; }
+
+.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/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/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/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/real/sim2real_ros2/.gitignore

@@ -0,0 +1,6 @@
+build/
+install/
+log/
+.colcon/
+.vscode/
+compile_commands.json

05_software/real/sim2real_ros2/DEPLOYMENT_GUIDE.md

@@ -0,0 +1,178 @@
+# ROS2 C++ Sim2Real 运动控制栈 - 部署指南
+
+本工作区提供了一个自包含、独立的 C++ ROS2 Humble 实现，用于在 Jetson Orin 目标机上部署轮腿四足机器人控制策略。
+
+---
+
+## 1. 前提条件与环境
+
+### 硬件
+* **目标计算机**：运行 Ubuntu 22.04 LTS 的 Jetson Orin Nano / Orin NX / AGX Orin。
+* **IMU 传感器**：Odin 集成 IMU，发布至 `/odin1/imu`。
+* **CAN 总线适配器**：Peak CAN、USB-to-CAN 或板载 SocketCAN 接口，使用 CAN0 和 CAN1。
+
+### 主机依赖
+* **操作系统**：Ubuntu 22.04 LTS (Jammy Jellyfish)。
+* **ROS 2 发行版**：ROS 2 Humble（Desktop-Base 或 ROS-Base）。
+* **C++ 编译器**：支持 C++17 的 GCC/G++ 9.0+。
+* **库与 ROS2 包**：
+  * `libyaml-cpp-dev`
+  * `libeigen3-dev`
+  * `libusb-1.0-0-dev`（Odin USB 传感器通信）
+  * `libpcl-dev` 和 `libopencv-dev`（3D 点云与相机处理）
+  * `ros-humble-navigation2` 和 `ros-humble-nav2-bringup`（Nav2 规划器/控制器服务器）
+  * `ros-humble-pointcloud-to-laserscan`（点云转激光扫描，供 AMCL 使用）
+  * `ros-humble-cv-bridge` 和 `ros-humble-pcl-conversions`（Odin 传感器驱动图像与点云处理）
+  * `can-utils`（SocketCAN 验证工具）
+
+---
+
+## 2. 本地编译与部署
+
+按以下步骤在主机系统上编译运行整个栈：
+
+### 步骤 1：安装系统依赖
+```bash
+sudo apt-get update
+sudo apt-get install -y build-essential cmake can-utils libyaml-cpp-dev libeigen3-dev \
+    libusb-1.0-0-dev libpcl-dev libopencv-dev ros-humble-navigation2 \
+    ros-humble-nav2-bringup ros-humble-pointcloud-to-laserscan \
+    ros-humble-cv-bridge ros-humble-pcl-conversions
+```
+
+### 步骤 2：下载 ONNXRuntime C++ SDK
+策略需要 ONNXRuntime 库来运行推理。必须下载并解压到已知目录：
+
+```bash
+# 创建目录
+sudo mkdir -p /opt/onnxruntime
+cd /opt
+
+# 针对 Jetson Orin (ARM64 / aarch64)：
+sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-aarch64-1.16.3.tgz
+sudo tar -zxvf onnxruntime-linux-aarch64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime
+
+# 或标准桌面仿真 (x86_64 / amd64)：
+# sudo wget https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz
+# sudo tar -zxvf onnxruntime-linux-x64-1.16.3.tgz --strip-components=1 -C /opt/onnxruntime
+```
+
+导出 CMake 辅助变量：
+```bash
+export ONNXRUNTIME_DIR=/opt/onnxruntime
+```
+
+### 步骤 3：构建工作区
+进入包含 `src/` 的本包根目录，运行 `colcon`：
+```bash
+colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+```
+
+### 步骤 4：配置 SocketCAN 接口
+启动前，以 1 Mbps 波特率激活 CAN 接口：
+```bash
+sudo ip link set can0 up type can bitrate 1000000
+sudo ip link set can1 up type can bitrate 1000000
+```
+使用 `ifconfig` 或 `ip link` 验证接口已启动。
+
+### 步骤 5：启动节点
+使启动脚本可执行并运行：
+```bash
+chmod +x start_sim2real.sh
+./start_sim2real.sh
+```
+
+---
+
+## 3. Docker 部署（推荐）
+
+强烈推荐使用 Docker 隔离依赖，避免 Jetson Orin 上的库版本冲突。
+
+### 步骤 1：构建镜像
+确保在 `sim2real_ros2` 目录中（包含 `Dockerfile`）：
+```bash
+# 使用标准 docker build：
+docker build -t sim2real_ros2:latest .
+
+# 或使用 Docker Compose：
+docker compose build
+```
+
+### 步骤 2：运行容器
+对于真实硬件部署，容器**必须**共享主机网络栈（用于 ROS2 DDS 和 SocketCAN）并具备线程优先级能力以实现实时调度：
+
+```bash
+# 选项 A：手动运行
+docker run -it \
+  --network host \
+  --privileged \
+  --cap-add=sys_nice \
+  --volume=/dev:/dev \
+  --shm-size=2g \
+  --name sim2real_ros2_run \
+  sim2real_ros2:latest
+
+# 选项 B：通过 Docker Compose 运行（最简单）
+docker compose up -d
+```
+
+---
+
+## 4. 系统拓扑与话题
+
+控制节点通过标准 ROS 2 DDS 消息与传感器驱动和导航栈交互：
+
+* **IMU 输入**：订阅 `/odin1/imu`（`sensor_msgs/msg/Imu`）。硬件节点自动执行逆轴旋转（`x_raw = -y_ros`，`y_raw = x_ros`）以重建 RL 策略期望的原始坐标系。
+* **控制命令**：订阅 `/cmd_vel` 和 `/cmd_vel_stamped`（`geometry_msgs/msg/Twist` / `TwistStamped`），由导航栈或手动键盘节点发布。
+* **里程计输入**：订阅 `/odom`（`nav_msgs/msg/Odometry`），由 `odom_relay_node` 从 `/odin1/odometry` 中继并重映射帧名后提供。
+* **急停**：订阅 `/safety/estop`（`std_msgs/msg/Bool`）。发布 `true` 触发软件急停，机器人进入低刚度阻尼刹车。
+* **状态遥测**：发布 `runtime/state`（`sim2real_interfaces/msg/RuntimeState`），包含当前关节速度、温度、IMU 输出和诊断信息。
+* **策略目标**：发布 `runtime/target`（`sim2real_interfaces/msg/RuntimeTarget`），包含策略推理输出的目标关节位置。
+
+### TF 树
+```
+odom ──→ base_link    （由 odom_relay_node 广播）
+map  ──→ odom         （由 AMCL / Odin SLAM 发布，取决于运行模式）
+```
+
+---
+
+## 5. 集成 ROS 2 导航与传感器驱动
+
+### USB 设备权限（Odin 传感器）
+要运行物理 Odin 传感器驱动（`odin_ros_driver`），目标计算机必须具有传感器 USB 接口的读写权限。在主机系统上添加以下 udev 规则：
+
+```bash
+# 1. 添加 udev 规则
+echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="2207", ATTR{idProduct}=="0019", MODE="0666", GROUP="plugdev"' | sudo tee /etc/udev/rules.d/99-odin-usb.rules
+
+# 2. 重新加载 udev 规则并重新插拔传感器
+sudo udevadm control --reload
+sudo udevadm trigger
+```
+
+### 集成启动参数
+统一启动文件 `sim2real_system.launch.py` 支持模块化激活传感器驱动和 Nav2 导航栈：
+
+* `launch_driver`（默认：`true`）：启动 `odin_ros_driver` 节点以获取 IMU 和点云遥测。
+* `launch_nav2`（默认：`true`）：启动 ROS2 Navigation2 规划器、控制器、costmap、AMCL 和 pointcloud_to_laserscan。
+
+#### 1. 完整真实硬件闭环（默认）
+启动运动控制运行时、物理 CAN 桥接、Odin 传感器驱动和 Nav2 导航：
+```bash
+ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=false launch_driver:=true launch_nav2:=true
+```
+
+#### 2. Dry-Run / 仿真航点测试
+在 dry-run 模式下运行策略运行时和 Nav2 导航（不访问 CAN 总线或物理 USB 传感器，适合测试导航话题路由）：
+```bash
+ros2 launch sim2real_bringup sim2real_system.launch.py dry_run:=true launch_driver:=false launch_nav2:=true
+```
+
+#### 3. 仅运动控制（无导航）
+禁用传感器驱动和 Nav2，让运动策略等待 `/cmd_vel` 上的手动速度输入（如键盘遥操作）：
+```bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_driver:=false launch_nav2:=false
+```
+

05_software/real/sim2real_ros2/Dockerfile

@@ -0,0 +1,78 @@
+# 使用 ROS2 官方 Humble 基础镜像
+FROM ros:humble-ros-base-jammy
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+# 安装 C++ 编译依赖、SocketCAN 调试工具及 Eigen 等核心库
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    build-essential \
+    cmake \
+    git \
+    can-utils \
+    libyaml-cpp-dev \
+    libeigen3-dev \
+    libusb-1.0-0-dev \
+    libpcl-dev \
+    libopencv-dev \
+    ros-humble-navigation2 \
+    ros-humble-nav2-bringup \
+    ros-humble-pointcloud-to-laserscan \
+    ros-humble-cv-bridge \
+    ros-humble-pcl-conversions \
+    wget \
+    tar \
+    python3-pip \
+    && rm -rf /var/lib/apt/lists/*
+
+# ============================================================================
+# ONNX Runtime — 架构自适应，aarch64 启用 CUDA GPU 加速
+# ============================================================================
+#   - Orin Nano (aarch64): pip 安装 onnxruntime-gpu（含 CUDA EP）
+#   - x86_64 开发机: 下载 CPU-only 预编译包（GPU 不可用）
+WORKDIR /opt
+RUN ARCH=$(uname -m) && \
+    if [ "$ARCH" = "aarch64" ]; then \
+        echo "[ONNX] Installing CUDA-enabled ONNX Runtime for Jetson Orin..." && \
+        pip3 install --no-cache-dir onnxruntime-gpu && \
+        SITE_PKGS=$(python3 -c "import site; print(site.getsitepackages()[0])") && \
+        mkdir -p onnxruntime/include onnxruntime/lib && \
+        cp -r "$SITE_PKGS/onnxruntime/include/"* onnxruntime/include/ && \
+        cp "$SITE_PKGS/onnxruntime/capi/libonnxruntime.so"* onnxruntime/lib/ && \
+        echo "[ONNX] CUDA ONNX Runtime installed."; \
+    else \
+        echo "[ONNX] Installing CPU-only ONNX Runtime for x86_64 dev..." && \
+        wget -q https://github.com/microsoft/onnxruntime/releases/download/v1.16.3/onnxruntime-linux-x64-1.16.3.tgz && \
+        tar -zxf onnxruntime-linux-x64-1.16.3.tgz && \
+        mv onnxruntime-linux-x64-1.16.3 onnxruntime && \
+        rm onnxruntime-linux-x64-1.16.3.tgz; \
+    fi
+
+ENV ONNXRUNTIME_DIR=/opt/onnxruntime
+
+# 创建工作空间，将所有 C++ 源码包拷入
+WORKDIR /sim2real_ws/src
+COPY src/sim2real_bringup sim2real_bringup
+COPY src/sim2real_common sim2real_common
+COPY src/sim2real_hw sim2real_hw
+COPY src/sim2real_interfaces sim2real_interfaces
+COPY src/sim2real_runtime sim2real_runtime
+COPY src/odin_ros_driver odin_ros_driver
+COPY src/sim2real_nav2 sim2real_nav2
+
+# 拷贝策略文件与运行脚本
+WORKDIR /sim2real_ws
+COPY policies policies
+COPY start_sim2real.sh start_sim2real.sh
+RUN chmod +x start_sim2real.sh
+
+# 编译 ROS2 工作空间
+SHELL ["/bin/bash", "-c"]
+RUN source /opt/ros/humble/setup.bash && \
+    colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+
+# 拷贝 Docker 入口脚本并设置
+COPY docker_entrypoint.sh /docker_entrypoint.sh
+RUN chmod +x /docker_entrypoint.sh
+
+ENTRYPOINT ["/docker_entrypoint.sh"]
+CMD ["./start_sim2real.sh"]

05_software/real/sim2real_ros2/README.md

@@ -0,0 +1,74 @@
+# ROS 2/C++ Sim2Real 初版
+
+本目录归档 `real/sim2real_ros2`，对应重排主线的 `v0.10.0`。这是轮腿机器人 Sim2Real 部署栈从 Python 运行时迁移到 ROS 2 + C++ 的第一版系统工程。
+
+本工程保留当前 `sim2real` 已验证的部署契约，同时将运行时热路径迁移到 C++：
+
+- `53D` 策略观测契约不变
+- `16D` 动作契约不变
+- `50Hz` 策略循环与训练对齐
+- `200Hz` 电机循环为专用 C++ 热路径
+- ROS 2 作为导航、TF、诊断和启动管理的系统集成层
+
+## 工作区布局
+
+- `src/sim2real_interfaces`
+  硬件桥接与策略运行时共享的 ROS 2 消息定义。
+- `src/sim2real_common`
+  共享常量、部署契约辅助函数、Mahony 姿态滤波器、站立平衡控制器、安全监控。
+- `src/sim2real_hw`
+  面向硬件的桥接节点：RobStride CAN 收发、IMU/Odin 数据采集、看门狗、状态发布。
+- `src/sim2real_runtime`
+  策略运行时节点：`53D→16D` ONNX 推理、命令滤波/仲裁、目标发布。
+  同时包含 `odom_relay_node`（里程计中继与 TF 广播）。
+- `src/sim2real_nav2`
+  ROS 2 Navigation2 (Nav2) 配置包：参数、启动文件、AMCL、costmap、planner/controller。
+- `src/sim2real_bringup`
+  统一启动文件与运行时参数配置。
+- `src/odin_ros_driver`
+  Odin 传感器 ROS 2 驱动（含 IMU、点云、里程计发布）。
+- `docs`
+  架构说明与迁移计划。
+
+## 目标架构
+
+```text
+Odin / IMU / Odom  --->  sim2real_hw  --->  sim2real_runtime  --->  sim2real_hw
+                              |                  |                    |
+                              v                  v                    v
+                         RuntimeState       RuntimeTarget          电机 CAN 指令
+                              |                  |
+                              +-------> 诊断 / 遥测
+
+Nav2 / cmd_vel  ------------------------------>  sim2real_runtime
+                   (经 odom_relay_node 提供 odom→base_link TF)
+```
+
+## 当前状态
+
+已完成 Phase 0-5 的全部迁移：
+
+1. ✅ 冻结部署契约（deployment_contract.hpp）
+2. ✅ ROS 2 包结构搭建
+3. ✅ 硬件热路径迁移至 C++（SocketCAN 驱动、200Hz 电机循环）
+4. ✅ ONNX 策略运行时迁移至 C++（50Hz 推理循环）
+5. ✅ 导航与诊断通过 ROS 2 接入（Nav2 + odom_relay + TF）
+
+## 契约来源
+
+迁移过程中以下文件被视为真值源：
+
+- `sim2real/deployment_manifest.yaml`
+- `sim2real/interface/motor_mapping.py`
+- `sim2real/interface/real_io.py`
+- `sim2real/policy/policy_runner.py`
+- `sim2real/web/session.py`
+
+## 注意事项
+
+- 开发目标为 Linux + ROS 2 Humble，运行于 Jetson Orin / x86_64。
+- Windows 仅作为编辑环境使用。
+- 观测顺序、动作缩放、默认站姿、电机映射不得独立修改，
+  除非训练与部署同步更新。
+- 原始快照中的 `src/odin_ros_driver` 是空目录，本版本仍需要另行提供兼容的 Odin ROS 2 驱动；其源码从后续版本开始随工程归档。
+- 自研 ROS 包保留原始 `Proprietary` 清单字段，公开发布前仍需统一许可证和维护者信息。

05_software/real/sim2real_ros2/docker-compose.yml

@@ -0,0 +1,22 @@
+version: '3.8'
+
+services:
+  sim2real_ros2:
+    build:
+      context: .
+      dockerfile: Dockerfile
+    container_name: sim2real_ros2_node
+    runtime: nvidia
+    network_mode: host
+    privileged: true
+    stdin_open: true
+    tty: true
+    environment:
+      - NVIDIA_VISIBLE_DEVICES=all
+      - NVIDIA_DRIVER_CAPABILITIES=compute,utility
+    cap_add:
+      - SYS_NICE
+    shm_size: '2gb'
+    volumes:
+      - /dev:/dev
+    restart: unless-stopped

05_software/real/sim2real_ros2/docker_entrypoint.sh

@@ -0,0 +1,12 @@
+#!/bin/bash
+set -e
+
+# Source ROS2 Humble environment
+source /opt/ros/humble/setup.bash
+
+# Source workspace install setup if compiled
+if [ -f "/sim2real_ws/install/setup.bash" ]; then
+    source /sim2real_ws/install/setup.bash
+fi
+
+exec "$@"

05_software/real/sim2real_ros2/docs/ARCHITECTURE.md

@@ -0,0 +1,102 @@
+# sim2real_ros2 架构说明
+
+## 设计目标
+
+- 保留已验证的 RL 部署契约不变
+- 将低延迟循环从 Python 迁移至 C++
+- 暴露标准 ROS 2 接口用于导航和系统集成
+- 保持安全边界独立于策略正确性
+
+## 各包职责
+
+### `sim2real_interfaces`（接口消息）
+
+定义最小化的运行时消息：
+
+- `RuntimeState`
+  硬件桥接发布的归一化运行时状态快照
+- `RuntimeTarget`
+  策略运行时发送至硬件桥接的最新策略目标
+
+### `sim2real_common`（共享常量）
+
+存储编译期常量和部署契约辅助：
+
+- 观测维度和字段布局
+- 动作维度和轮子索引
+- 关节顺序和默认站姿
+- 动作缩放因子和默认循环频率
+- Mahony 姿态滤波器
+- 站立平衡控制器
+- 安全监控器（SafetyMonitor / RuntimeGuard）
+
+### `sim2real_hw`（硬件桥接）
+
+拥有硬件侧执行循环和安全边界：
+
+- RobStride CAN 收发
+- IMU 与 Odin 状态采集
+- 电机丢帧检测与保活逻辑（holdover）
+- 看门狗与阻尼刹车
+- 发布 `RuntimeState`
+- 订阅 `RuntimeTarget`
+- 订阅 `/odom` 里程计数据
+
+目标热路径：
+
+- 以 `200Hz` 频率读取状态
+- 应用最新安全目标
+- 超时或安全违规时立即停机
+
+### `sim2real_runtime`（策略运行时）
+
+拥有策略侧执行：
+
+- 订阅 `RuntimeState`
+- 按当前部署契约精确构建 `53D` 观测
+- 以 `50Hz` 运行 ONNXRuntime 推理
+- 对 raw_action 做 `[-10, 10]` 安全裁剪
+- 发布 `RuntimeTarget`
+- 仲裁命令来源：estop > safety_hold > startup > navigation > web
+
+同时包含：
+- `odom_relay_node`：将 `/odin1/odometry` 中继为 `/odom`，帧名 `odin1_base_link` → `base_link`，并广播 TF
+
+### `sim2real_nav2`（导航配置）
+
+拥有：
+
+- Nav2 参数文件（planner、controller、costmap、AMCL、behavior）
+- Nav2 启动文件（含 AMCL、costmap 生命周期节点、pointcloud_to_laserscan）
+
+### `sim2real_bringup`（启动管理）
+
+拥有：
+
+- 参数文件
+- 启动组合
+- 运行时模式选择
+- 集成 odin_ros_driver、sim2real_nav2 的条件启动
+
+## 迁移规则
+
+1. 优化之前先冻结当前契约
+2. 先迁移传输和循环结构，再调整控制算法
+3. C++ 运行时未达到影子模式一致性前，保留 Python 运行时可用
+4. 按段测量延迟：
+   - 观测延迟
+   - 策略推理延迟
+   - 目标传输延迟
+   - 执行器响应延迟
+
+## 首个里程碑
+
+首个里程碑不是"机器人在 ROS 2 下行走"，而是：
+
+1. `sim2real_hw` 发布稳定的 `RuntimeState`
+2. `sim2real_runtime` 从该状态构建正确的 `53D` 观测
+3. `sim2real_runtime` 以 `50Hz` 发布 `RuntimeTarget`
+4. `sim2real_hw` 消费最新目标并执行超时刹车
+5. `cmd_vel` 可通过 ROS 2 注入而不改变策略契约
+
+> ✅ 以上里程碑已全部完成。

05_software/real/sim2real_ros2/docs/MIGRATION_PLAN.md

@@ -0,0 +1,80 @@
+# 迁移计划
+
+## Phase 1: 硬件核心迁移 ✅ 已完成
+
+将当前高频热路径从 Python 迁出。
+
+吸收的源文件：
+
+- `sim2real/interface/motor_driver.py`
+- `sim2real/interface/motor_mapping.py`
+- `sim2real/interface/imu_client.py`
+- `sim2real/safety/runtime_guard.py`
+- `sim2real/web/session.py`
+
+交付物：
+
+- C++ SocketCAN 电机总线封装
+- C++ 状态缓存
+- target 超时保活（timeout hold）
+- 阻尼刹车 / 急停通路
+- 发布 `RuntimeState`
+
+## Phase 2: 策略运行时迁移 ✅ 已完成
+
+吸收的源文件：
+
+- `sim2real/policy/policy_runner.py`
+- `sim2real/interface/real_io.py`
+- `sim2real/web/session.py`
+
+交付物：
+
+- 精确的 `53D` 观测构造器
+- ONNXRuntime C++ 推理封装
+- `50Hz` 策略定时器
+- 命令平滑与来源仲裁
+- raw_action `[-10, 10]` 安全裁剪
+- 发布 `RuntimeTarget`
+
+## Phase 3: ROS 2 系统集成 ✅ 已完成
+
+参考的源项目：
+
+- `00_ reference/odin_ros_driver`
+- `00_ reference/EDULITE_A3/el_a3_ros`
+- `00_ reference/rl_sar`
+
+交付物：
+
+- `cmd_vel` / `cmd_vel_stamped` 输入（支持 Twist 和 TwistStamped）
+- `odom_relay_node`：里程计中继 + TF 广播（odom → base_link）
+- 诊断话题
+- rosbag/foxglove 可观测性
+
+## Phase 4: 导航集成 ✅ 已完成
+
+目标：
+
+- 导航通过 ROS 2 发送身体速度指令
+- RL 运行时保持为 locomotion 控制器
+- 看门狗和安全边界始终在导航之下
+
+规则：
+
+- 导航绝不直接写电机指令
+- 策略契约在重新训练前保持不变
+- 任何新增历史项或里程计项必须版本化
+
+## 当前状态
+
+所有 4 个 Phase 已全部完成。以下为已实现的关键组件：
+
+| 组件 | 节点 | 说明 |
+|------|------|------|
+| 硬件桥接 | `sim2real_hw_node` | 200Hz CAN 收发 + IMU + Mahony + 安全 |
+| 策略运行时 | `sim2real_runtime_node` | 50Hz ONNX 推理 + 53D 观测 + raw_action clip |
+| 里程计中继 | `odom_relay_node` | /odin1/odometry → /odom + odom→base_link TF |
+| 导航栈 | Nav2 全套节点 | AMCL + costmap + DWB + Navfn + BT + lifecycle |
+| 传感器驱动 | `odin_ros_driver` | IMU + 点云 + 里程计原始发布 |
+| 点云转换 | `pointcloud_to_laserscan` | /odin1/cloud_slam → /scan (供 AMCL 使用) |

05_software/real/sim2real_ros2/docs/REMOTE_CONTROL_USAGE.md

@@ -0,0 +1,495 @@
+# sim2real_ros2 遥控器调用说明
+
+本文档说明如何在 `sim2real_ros2` 中调用已接入的 SBUS UART 遥控器节点，以及执行后系统会产生什么效果。
+
+## 1. 当前接入关系
+
+遥控器节点位于：
+
+```text
+src/sim2real_runtime/src/remote_uart_node.py
+```
+
+该节点读取 SBUS 串口数据，并发布标准 ROS 2 控制话题：
+
+| 输入 | 输出 | 作用 |
+|---|---|---|
+| SBUS UART 遥控器 | `/cmd_vel` | 给策略运行时发送速度命令 |
+| SBUS CH7 高位 | `/safety/estop` | 触发软件急停 |
+
+策略节点 `sim2real_runtime_node` 已经订阅 `/cmd_vel` 和 `/safety/estop`，所以遥控器不直接控制电机，而是通过 ROS 2 标准速度接口进入策略控制链路。
+
+## 2. 通道映射
+
+通道映射与前一阶段 Python Sim2Real 中的遥控器实现保持一致。
+
+| 遥控器通道 | ROS 2 输出 | 含义 | 默认最大值 |
+|---|---|---|---:|
+| `CH2` | `cmd_vel.linear.x` | 前后速度 `vx` | `0.8 m/s` |
+| `CH4` | `cmd_vel.linear.y` | 左右速度 `vy` | `0.3 m/s` |
+| `CH1` | `cmd_vel.angular.z` | 转向角速度 `yaw` | `0.5 rad/s` |
+| `CH7 HIGH` | `/safety/estop = true` | 软件急停 | - |
+
+默认方向反转配置：
+
+| 参数 | 默认值 | 含义 |
+|---|---:|---|
+| `remote_invert_vx` | `true` | 反转前后方向 |
+| `remote_invert_vy` | `false` | 不反转横移方向 |
+| `remote_invert_yaw` | `true` | 反转转向方向 |
+
+## 3. 参数位置
+
+遥控器参数在：
+
+```text
+src/sim2real_bringup/config/runtime.yaml
+```
+
+当前默认参数：
+
+```yaml
+remote_enabled: true
+remote_port: "/dev/ttyACM0"
+remote_baudrate: 100000
+remote_timeout: 0.02
+remote_axis_deadzone: 50
+remote_active_threshold: 50
+remote_axis_full_scale: 660.0
+remote_max_vx: 0.8
+remote_max_vy: 0.3
+remote_max_yaw_rate: 0.5
+remote_invert_vx: true
+remote_invert_vy: false
+remote_invert_yaw: true
+remote_publish_inactive_zero: true
+remote_estop_latch: true
+remote_poll_hz: 50.0
+```
+
+如果遥控器串口不是 `/dev/ttyACM0`，需要修改：
+
+```yaml
+remote_port: "/dev/ttyUSB0"
+```
+
+或改成实际设备路径。
+
+## 4. 启动前检查
+
+### 4.1 确认串口存在
+
+```bash
+ls /dev/ttyACM* /dev/ttyUSB*
+```
+
+如果使用默认配置，应能看到：
+
+```bash
+/dev/ttyACM0
+```
+
+### 4.2 确认串口权限
+
+如果节点提示串口权限不足，可以临时执行：
+
+```bash
+sudo chmod 666 /dev/ttyACM0
+```
+
+更推荐的长期方式是把当前用户加入 `dialout` 组：
+
+```bash
+sudo usermod -aG dialout $USER
+```
+
+然后重新登录。
+
+### 4.3 确认 Python serial 依赖
+
+节点依赖 `pyserial`。如果系统没有安装：
+
+```bash
+sudo apt update
+sudo apt install -y python3-serial
+```
+
+## 5. 构建
+
+如果刚修改过代码或参数，建议重新构建相关包：
+
+```bash
+cd /path/to/sim2real_ros2
+source /opt/ros/humble/setup.bash
+colcon build --packages-select sim2real_runtime sim2real_bringup --symlink-install --merge-install
+```
+
+构建完成后 source 环境：
+
+```bash
+source install/setup.bash
+```
+
+确认可执行节点存在：
+
+```bash
+ros2 pkg executables sim2real_runtime
+```
+
+应包含：
+
+```text
+sim2real_runtime remote_uart_node.py
+```
+
+## 6. 推荐启动方式
+
+### 6.1 启动完整系统，不启动 Nav2
+
+这是你当前常用方式：
+
+```bash
+cd /path/to/sim2real_ros2
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
+```
+
+默认情况下，`launch_remote:=true`，所以上面命令会同时启动遥控器节点。
+
+等价完整写法：
+
+```bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=true
+```
+
+### 6.2 不启动遥控器
+
+如果只想用手动 `ros2 topic pub` 或其他上位机发 `/cmd_vel`，可以关闭遥控器节点：
+
+```bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=false
+```
+
+## 7. 单独启动遥控器节点
+
+如果系统已经在运行，只想单独测试遥控器节点：
+
+```bash
+cd /path/to/sim2real_ros2
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
+```
+
+如果要临时指定串口：
+
+```bash
+ros2 run sim2real_runtime remote_uart_node.py --ros-args \
+  --params-file src/sim2real_bringup/config/runtime.yaml \
+  -p remote_port:=/dev/ttyUSB0
+```
+
+## 8. 执行后会产生什么效果
+
+启动以下命令后：
+
+```bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
+```
+
+系统会产生以下效果。
+
+### 8.1 启动硬件桥接节点
+
+节点：
+
+```text
+/sim2real_hw_node
+```
+
+效果：
+
+1. 打开 `can0` 和 `can1`。
+2. 如果 `dry_run: false` 且 CAN 初始化成功，会使能 16 个 RobStride 电机。
+3. 设置电机 MIT 模式。
+4. 设置电机速度限制和力矩限制。
+5. 以 `200Hz` 运行硬件读写循环。
+6. 发布 `/runtime/state`。
+7. 订阅 `/runtime/target` 执行策略目标。
+
+### 8.2 启动策略运行节点
+
+节点：
+
+```text
+/sim2real_runtime_node
+```
+
+效果：
+
+1. 加载 ONNX 策略模型。
+2. 订阅 `/runtime/state`。
+3. 订阅 `/cmd_vel`。
+4. 订阅 `/safety/estop`。
+5. 执行启动站立流程：
+   - `boot_hold`
+   - `startup_soft_hold`
+   - `startup_hold`
+   - `runtime_zero_hold`
+   - `runtime_policy`
+6. 以 `50Hz` 发布 `/runtime/target`。
+
+### 8.3 启动遥控器节点
+
+节点：
+
+```text
+/sim2real_remote_uart_node
+```
+
+效果：
+
+1. 打开默认串口 `/dev/ttyACM0`。
+2. 以 `50Hz` 轮询 SBUS 数据。
+3. 遥控器摇杆居中时持续发布零速度：
+
+```text
+/cmd_vel:
+  linear.x = 0.0
+  linear.y = 0.0
+  angular.z = 0.0
+```
+
+4. 推动遥控器时发布非零速度，例如：
+
+```text
+/cmd_vel:
+  linear.x = vx
+  linear.y = vy
+  angular.z = yaw
+```
+
+5. 当 CH7 打到高位时发布：
+
+```text
+/safety/estop: true
+```
+
+由于当前 `remote_estop_latch: true`，急停是锁存式行为：一旦 CH7 高位触发，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。
+
+### 8.4 机器人行为效果
+
+正常启动后，机器人不会立即按策略行走，而是按阶段执行：
+
+1. 电机使能。
+2. 读取当前关节位置。
+3. 软保持当前姿态。
+4. 平滑过渡到默认站立姿态。
+5. 稳定后进入 runtime。
+6. 遥控器无输入时保持站立平衡，即 `runtime_zero_hold`。
+7. 遥控器有输入时进入策略控制，即 `runtime_policy`。
+
+也就是说：
+
+| 遥控器状态 | 机器人效果 |
+|---|---|
+| 摇杆居中 | 站立保持，不主动行走 |
+| CH2 前后推动 | 前进/后退 |
+| CH4 左右推动 | 横向移动 |
+| CH1 左右推动 | 原地转向 |
+| CH7 高位 | 软件急停，进入安全刹车 |
+
+## 9. 如何确认遥控器已经生效
+
+### 9.1 查看节点是否存在
+
+```bash
+ros2 node list
+```
+
+应看到：
+
+```text
+/sim2real_remote_uart_node
+/sim2real_runtime_node
+/sim2real_hw_node
+```
+
+### 9.2 查看 `/cmd_vel`
+
+```bash
+ros2 topic echo /cmd_vel
+```
+
+摇动遥控器时应看到 `linear.x`、`linear.y` 或 `angular.z` 变化。
+
+### 9.3 查看 `/safety/estop`
+
+```bash
+ros2 topic echo /safety/estop
+```
+
+CH7 高位时应看到：
+
+```yaml
+data: true
+```
+
+### 9.4 查看策略目标阶段
+
+```bash
+ros2 topic echo /runtime/target --field target_source
+```
+
+常见输出含义：
+
+| `target_source` | 含义 |
+|---|---|
+| `boot_hold` | 刚启动，保持初始姿态 |
+| `startup_soft_hold` | 启动软保持 |
+| `startup_hold` | 正在站立或站立后保持 |
+| `runtime_zero_hold` | 已进入 runtime，遥控器无有效输入 |
+| `runtime_policy` | 遥控器有输入，策略已经介入 |
+| `safety_brake` | 安全刹车 |
+| `timeout_hold` | 目标超时，硬件保持默认姿态 |
+
+### 9.5 查看完整目标状态
+
+```bash
+ros2 topic echo --once /runtime/target
+```
+
+重点关注字段：
+
+```yaml
+target_source:
+zero_command:
+runtime_released:
+release_alpha:
+command:
+raw_command:
+```
+
+如果遥控器摇杆有输入，通常会看到：
+
+```yaml
+target_source: runtime_policy
+zero_command: false
+runtime_released: true
+release_alpha: 1.0
+```
+
+## 10. 常见问题
+
+### 10.1 启动后提示无法打开串口
+
+可能原因：
+
+1. 串口路径不对。
+2. 权限不足。
+3. 设备没有插好。
+4. 设备被其他程序占用。
+
+检查：
+
+```bash
+ls /dev/ttyACM* /dev/ttyUSB*
+```
+
+修改 `runtime.yaml`：
+
+```yaml
+remote_port: "/dev/ttyUSB0"
+```
+
+### 10.2 `/cmd_vel` 没有变化
+
+检查：
+
+```bash
+ros2 node list
+ros2 topic echo /cmd_vel
+```
+
+如果节点存在但无变化，可能是：
+
+1. 遥控器没有输出 SBUS。
+2. 串口波特率不对。
+3. SBUS 接线错误。
+4. 遥控器通道未校准。
+5. 死区 `remote_axis_deadzone` 或 `remote_active_threshold` 太大。
+
+### 10.3 摇杆方向反了
+
+修改：
+
+```yaml
+remote_invert_vx: true
+remote_invert_vy: false
+remote_invert_yaw: true
+```
+
+例如前后方向反了，就切换：
+
+```yaml
+remote_invert_vx: false
+```
+
+### 10.4 急停后不恢复
+
+当前配置：
+
+```yaml
+remote_estop_latch: true
+```
+
+这表示急停锁存。触发后建议：
+
+1. 先确认机器人物理安全。
+2. 停止 launch。
+3. 将 CH7 打回安全位置。
+4. 重新启动系统。
+
+如果需要非锁存模式，可以改为：
+
+```yaml
+remote_estop_latch: false
+```
+
+但实机调试时更建议使用锁存模式。
+
+## 11. 快速验证命令清单
+
+```bash
+cd /path/to/sim2real_ros2
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
+```
+
+另开终端：
+
+```bash
+cd /path/to/sim2real_ros2
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+
+ros2 node list
+ros2 topic echo /cmd_vel
+ros2 topic echo /runtime/target --field target_source
+```
+
+如果只测遥控器，不启动电机系统：
+
+```bash
+ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
+```
+
+另开终端：
+
+```bash
+ros2 topic echo /cmd_vel
+ros2 topic echo /safety/estop
+```

05_software/real/sim2real_ros2/docs/WEB_DEBUG_USAGE.md

@@ -0,0 +1,246 @@
+# sim2real_ros2 Web UDP 调试说明
+
+本文档说明本次新增的最小 Web 调试链路。
+
+## 1. 架构
+
+```text
+Windows 本地浏览器/HTTP 服务
+        |
+        | UDP JSON
+        v
+Nano: sim2real_web_udp_bridge_node.py
+        |
+        | ROS 2 topics
+        v
+sim2real_cmd_mux_node.py -> /cmd_vel -> sim2real_runtime_node
+```
+
+Web 页面在 Windows 本地渲染，Nano 只运行轻量 UDP bridge 和 ROS2 节点。
+
+## 2. 新增 ROS2 节点
+
+### `remote_uart_node.py`
+
+遥控器节点现在发布：
+
+```text
+/cmd_vel_remote
+```
+
+不再直接发布 `/cmd_vel`。
+
+通道触发阈值改为：
+
+```yaml
+remote_axis_deadzone: 40
+remote_active_threshold: 40
+```
+
+只有通道归一化值绝对值大于 `40` 才认为是有效输入。
+
+### `cmd_mux_node.py`
+
+输入：
+
+```text
+/cmd_vel_remote
+/cmd_vel_web
+/cmd_vel_nav
+/control/mode
+/remote/enabled
+/web/enabled
+/nav/enabled
+/safety/estop
+```
+
+输出：
+
+```text
+/cmd_vel
+/control/mode_state
+/control/mux_status
+```
+
+控制模式：
+
+```text
+DISABLED
+REMOTE
+WEB
+NAV
+```
+
+急停 `/safety/estop=true` 会强制进入 `DISABLED`，并输出零速度。
+
+### `web_udp_bridge_node.py`
+
+Nano 端 UDP 监听：
+
+```text
+0.0.0.0:15000
+```
+
+发布：
+
+```text
+/cmd_vel_web
+/safety/estop
+/control/mode
+/web/enabled
+/remote/enabled
+/nav/enabled
+```
+
+订阅并回传状态：
+
+```text
+/runtime/state
+/runtime/target
+/cmd_vel
+/safety/estop
+/control/mode_state
+/control/mux_status
+```
+
+## 3. Nano 启动
+
+```bash
+cd /path/to/sim2real_ros2
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
+```
+
+默认会启动：
+
+```text
+sim2real_remote_uart_node
+sim2real_cmd_mux_node
+sim2real_web_udp_bridge_node
+```
+
+如果不想启动 Web UDP bridge：
+
+```bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_web_bridge:=false
+```
+
+## 4. Windows 本地 Web 启动
+
+把目录复制到 Windows 或通过共享目录访问：
+
+```text
+tools/win_web_debug
+```
+
+在 Windows 上安装 Python 3 后运行：
+
+```bash
+python server.py --nano-host <Nano_IP> --http-port 8088 --udp-port 15001
+```
+
+浏览器打开：
+
+```text
+http://127.0.0.1:8088
+```
+
+## 5. UDP 命令格式
+
+### 切换模式
+
+```json
+{"type":"mode","mode":"REMOTE"}
+```
+
+```json
+{"type":"mode","mode":"WEB"}
+```
+
+```json
+{"type":"mode","mode":"DISABLED"}
+```
+
+### Web 速度控制
+
+```json
+{
+  "type": "cmd_vel",
+  "linear": {"x": 0.2, "y": 0.0, "z": 0.0},
+  "angular": {"x": 0.0, "y": 0.0, "z": 0.1}
+}
+```
+
+Nano 端会再次限幅：
+
+```text
+vx  <= ±0.8 m/s
+vy  <= ±0.3 m/s
+yaw <= ±0.5 rad/s
+```
+
+### 零速度
+
+```json
+{"type":"zero"}
+```
+
+### 软急停
+
+```json
+{"type":"estop","data":true}
+```
+
+## 6. 安全保护
+
+当前最小版本已经包含：
+
+1. 遥控器误触发阈值：`40`。
+2. 遥控器/Web/Nav 互斥控制模式。
+3. `cmd_mux` 二次限幅。
+4. `cmd_mux` 加速度限制。
+5. Web UDP 超时自动发布零速度。
+6. 急停优先级最高。
+7. Web 页面切换到 `WEB` 模式需要确认。
+8. Web 松开虚拟摇杆会自动发送零速度。
+
+建议实机调试流程：
+
+1. 先点击 `DISABLED`。
+2. 确认 `/cmd_vel` 为零。
+3. 如果使用遥控器，点击 `REMOTE`。
+4. 如果使用 Web，点击 `WEB` 并确认周围安全。
+5. 一旦异常，立即点击 `软急停`。
+
+## 7. 验证命令
+
+查看最终输出速度：
+
+```bash
+ros2 topic echo /cmd_vel
+```
+
+查看遥控器输入：
+
+```bash
+ros2 topic echo /cmd_vel_remote
+```
+
+查看 Web 输入：
+
+```bash
+ros2 topic echo /cmd_vel_web
+```
+
+查看当前仲裁模式：
+
+```bash
+ros2 topic echo /control/mode_state
+```
+
+查看策略状态：
+
+```bash
+ros2 topic echo /runtime/target --field target_source
+```

05_software/real/sim2real_ros2/image/cam_in_ex.txt

@@ -0,0 +1,20 @@
+Tcl_0: [-0.009160, -0.999960, 0.000320, 0.032150, 
+        0.002390, -0.000340, -1.000000, -0.011850, 
+        0.999960, -0.009160, 0.002390, 0.005360, 
+        0.000000, 0.000000, 0.000000, 1.000000]
+cam_0: 
+   image_width: 1600
+   image_height: 1296
+   k2: 0.000656
+   k3: -0.028961
+   k4: 0.045390
+   k5: -0.064513
+   k6: 0.038735
+   k7: -0.009903
+   p1: 0.000000
+   p2: 0.000000
+   A11: 736.894262
+   A12: -0.161150
+   A22: 736.611354
+   u0: 806.125535
+   v0: 639.650710