diff --git a/.gitignore b/.gitignore index 0a9889c..7621fd7 100644 --- a/.gitignore +++ b/.gitignore @@ -29,6 +29,7 @@ log/ !05_software/real/sim2real/vendored/odin1_imu/build/ !05_software/real/sim2real/vendored/odin1_imu/build/libodin1_imu_bridge.so !05_software/real/sim2real/vendored/odin1_imu/lib/*.a +!05_software/real/sim2real_v2/vendored/odin1_imu/lib/*.a # Training outputs logs/ diff --git a/01_doc/version_history.md b/01_doc/version_history.md index 27d8346..dc703eb 100644 --- a/01_doc/version_history.md +++ b/01_doc/version_history.md @@ -14,6 +14,16 @@ | `v0.7.0` | MuJoCo 工具 | 姿态优化、IK 扫描、动力学、MPC 和 GUI 调试工具 | | `v0.8.0` | 后期 Sim2Sim | ONNX 回放、IK/路线检查工具和比赛最终 Rough 策略 | | `v0.8.1` | 导航打点工具 | 地图/航点编辑、路线迭代和抽样 PCD 补充包 | +| `v0.9.0` | Python Sim2Real v2 | 反馈新鲜度、Odin odom 诊断、Web 调试和安全监控增强 | + +> 原先临时归档为 `v0.9.0` 的最终 ROS 2/C++ 比赛部署已保存在 `backup/final-ros2-v0.9.0` 分支和 `backup-v0.9.0-ros2-final` 标签中，重排完成后将正式归入 `v1.0.0`。 + +## `v0.9.0` 的 Python Sim2Real v2 + +- 归档 `real/sim2real_v2` 真机部署版本，保持 `53D -> 16D` 策略观测和动作契约。 +- 增加电机反馈新鲜度判断、Odin odom 诊断、命令限加速度平滑和 Web 运行时诊断。 +- 保留 Python 策略运行时、ONNX/PT 模型、MJCF、Odin 接口、Web 工具和安全保护链路。 +- 排除运行日志、测试日志、临时 XML 和开发交接草稿；后续 ROS 2/C++ 版本另行归档。 ## `v0.4.0` 的模型变化 diff --git a/05_software/README.md b/05_software/README.md index 873632b..bdeb792 100644 --- a/05_software/README.md +++ b/05_software/README.md @@ -8,7 +8,8 @@ │ └─ rc_mjlab/ # 训练、MJCF、MuJoCo、Sim2Sim 和本地 mjlab 依赖 └─ real/ ├─ ik_real/ # IK 轨迹与早期真机控制 - └─ sim2real/ # 第一代 Python 策略真机部署 + ├─ sim2real/ # 第一代 Python 策略真机部署 + └─ sim2real_v2/ # Python Sim2Real v2 ``` ## 数据流 @@ -23,12 +24,12 @@ MJCF + mjlab task | +----> Sim2Sim 策略验证 | - +----> Python Sim2Real ----> 电机 / IMU + +----> Python Sim2Real / v2 ----> 电机 / 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`。 +`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`。 详细说明见： diff --git a/05_software/real/README.md b/05_software/real/README.md index 06461f6..b606c77 100644 --- a/05_software/real/README.md +++ b/05_software/real/README.md @@ -1,6 +1,6 @@ -# 第一代真机控制 +# 真机控制版本演进 -本目录保存 16DOF 轮足机器人的早期真机控制实现。 +本目录保存 16DOF 轮足机器人从早期 Python 闭环到 ROS 2 部署的真机控制演进。 ## `ik_real` @@ -20,6 +20,12 @@ 部署说明见 [`sim2real/README.md`](sim2real/README.md) 与 [`sim2real/DEPLOYMENT.md`](sim2real/DEPLOYMENT.md)。 +## `sim2real_v2` + +Python Sim2Real v2，保留 `53D -> 16D` 策略接口，并增加电机反馈新鲜度、Odin odom 诊断、命令平滑、Web 运行时诊断和安全监控工具。该版本对应重排主线的 `v0.9.0`。 + +部署说明见 [`sim2real_v2/README.md`](sim2real_v2/README.md) 与 [`sim2real_v2/DEPLOYMENT.md`](sim2real_v2/DEPLOYMENT.md)。 + ## 实机记录 [![第一代 Sim2Real 真机验证](../../06_assets/images/early_sim2real_preview.jpg)](../../06_assets/videos/early_sim2real.mp4) diff --git a/05_software/real/sim2real_v2/DEPLOYMENT.md b/05_software/real/sim2real_v2/DEPLOYMENT.md new file mode 100644 index 0000000..d22391e --- /dev/null +++ b/05_software/real/sim2real_v2/DEPLOYMENT.md @@ -0,0 +1,168 @@ +# 真机部署与验证指南 + +本文档面向当前目录 `shiji/sim2real (10)/sim2real`，用于减少上机排错时间。默认不修改已验证的电机映射、方向、零位和策略观测。 + +## 1. 前期环境 + +推荐在 Orin / Linux 上运行： + +```bash +cd sim2real +python -m pip install -r requirements-orin.txt +``` + +确认设备： + +```bash +ip link show can0 +ip link show can1 +ls -l /dev/ttyACM0 +``` + +确认 `config.yaml`： + +- `can1_port` 和 `can2_port` 对应实际 SocketCAN 设备。 +- `control_freq` 默认保持 `50`。 +- `remote.port` 默认 `/dev/ttyACM0`。 +- `policy.action_scale` 必须是 16 维。 +- 默认策略文件优先使用 `policies/model_rough.onnx`；如果只有 `.pt`，先用 `tools/export_onnx.py` 导出。 +- `controller.kp_leg/kd_leg/kd_wheel` 不要在未记录实验的情况下大改。 + +## 2. 无硬件/低风险检查 + +先做文件和模型契约检查： + +```bash +python tools/export_onnx.py --pt policies/model_rough.pt --onnx policies/model_rough.onnx +python tools/alignment_check.py --policy policies/model_rough.onnx --manifest deployment_manifest.yaml +python tools/standalone_check.py +python -m py_compile tools/logger.py interface/motor_driver.py interface/imu_client.py interface/real_io.py web/session.py +``` + +`alignment_check.py` 会同时检查： + +- 策略 obs/action 维度。 +- action scale。 +- default pose。 +- joint order。 +- wheel indices。 +- `config.yaml` 与 `deployment_manifest.yaml` 的控制频率和 command filter 是否一致。 +- ONNX 策略的 obs/action 维度是否仍为 `53D/16D`。 + +如果有 Node 环境，可检查前端语法： + +```bash +node --check web/static/app.js +``` + +## 3. 上电前检查 + +上电前确认： + +- 机器人架空或有可靠支撑。 +- 16 个电机 CAN 线和电源线固定。 +- Odin1 连接稳定，启动时机器人尽量静止，利于重力对齐。 +- Web 急停可见，遥控软急停通道可用。 +- CAN 设备名和遥控串口名与 `config.yaml` 一致。 + +## 4. Web 启动 + +```bash +python web/server.py --host 0.0.0.0 --port 8080 +``` + +浏览器打开： + +```text +http://:8080 +``` + +推荐先只看状态，不急着释放遥控。 + +## 5. 标准上机流程 + +1. 点击 `Connect`，确认 IMU/Odin 和 CAN 初始化正常。 +2. 点击 `Enable Motors`，确认 16 个电机都有反馈。 +3. 点击 `Startup`，从当前实测姿态过渡到站立。 +4. 进入 `STAND_HOLD` 后观察 IMU age、Motor Fresh、Loop Profile。 +5. 点击 `Start Runtime`，进入 50Hz 策略循环。 +6. 策略 release 后再点击遥控接管。 +7. 小幅给命令，先测试前后、转向，再测试组合动作。 + +## 6. 本轮新增诊断如何看 + +`Loop Profile`： + +- `read_state_ms` 高：优先查电机接收、CAN 队列、Odin 获取是否阻塞。 +- `policy_ms` 高：优先查策略推理和 CPU 负载。 +- `send_actions_ms` 高：优先查 CAN 发送和 USB-CAN 适配器。 +- `log_ms` 高：说明日志队列或磁盘仍可能有压力。 + +`Motor Fresh`： + +- 理想状态是 `16/16 (cnt 16, val 0)` 或接近。 +- `cnt` 高说明 `update_count` 正常增长，这是最可靠的电机反馈证据。 +- 如果机器人静止时 `val` 为 0 是正常现象，不应据此判断丢电机。 + +`Odin Odom`： + +- 显示 `STANDARD/HIGHFREQ/TF`、age、local x/y/yaw。 +- 当前只用于诊断和全局坐标显示，不参与策略输入。 +- odom 不可用时，当前 locomotion 仍应可以运行。 +- 如果显示 `JUMP`，说明 odom 局部位置或 yaw 出现突变，先不要把它用于闭环导航。 + +`Latest Target`： + +- 显示当前电机目标来源，例如 `runtime_policy`、`runtime_zero_hold`、`runtime_release_hold`。 +- `age` 应随 runtime 正常刷新；如果明显超过控制周期很多，说明目标更新链路卡住。 +- `d` 是相邻目标最大变化量，可用于观察停车/起步是否有目标突变。 + +`Obs / Action`： + +- `obs` 接近 `100` 时，说明观测可能接近 clip 边界。 +- `raw` 接近 `10` 时，说明策略输出可能接近 raw action 裁剪边界。 +- `scaled` 长期很大时，检查 action scale、目标限幅和 safety clip。 + +`cmd/raw cmd`： + +- `raw cmd` 是 Web/遥控原始输入。 +- `cmd` 是经过 `command_filter` 限加速度后的策略命令。 +- 如果机器人响应慢，先看两者差值是否由命令滤波造成。 + +## 7. 如果出现前后晃动 + +先不要直接改控制频率。按顺序排查： + +1. 看 `Loop Overruns` 是否增长。 +2. 看 `Loop Profile` 最慢阶段。 +3. 看 `imu_age` 是否超过 30-60ms。 +4. 看 `Motor Fresh` 是否掉到 16 以下。 +5. 看停止时 `cmd` 是否真的回到 0。 +6. 看 `runtime_released`、`release_alpha` 和 `track_err` 是否异常。 + +只有确认 50Hz 长期跑不稳时，才把 `control_freq: 40` 作为诊断实验，而不是默认方案。训练/部署频率不一致可能引入新的 sim2real gap。 + +## 8. 日志 + +每次运行会生成： + +- `state.csv`：高频状态流，后台线程写入。 +- `events.jsonl`：事件流，关键事件会即时 flush。 + +重点搜索： + +```bash +grep LOOP_OVERRUN web/logs/*/events.jsonl +grep SAFETY web/logs/*/events.jsonl +grep GUARD web/logs/*/events.jsonl +``` + +## 9. 当前不建议改动的内容 + +- 16 个电机映射、方向、零位。 +- 策略 53D 观测顺序和缩放。 +- 16D 动作顺序和 action scale。 +- 默认 `50Hz` 控制频率。 +- 已经真机跑通过的遥控方向配置。 + +这些内容只有在有新日志和明确现象时再改，避免把已验证链路打散。 diff --git a/05_software/real/sim2real_v2/FACTS_AND_ASSUMPTIONS.md b/05_software/real/sim2real_v2/FACTS_AND_ASSUMPTIONS.md new file mode 100644 index 0000000..a23e313 --- /dev/null +++ b/05_software/real/sim2real_v2/FACTS_AND_ASSUMPTIONS.md @@ -0,0 +1,49 @@ +# `FACTS_AND_ASSUMPTIONS` + +## 已确认 + +- 当前部署模型：默认优先使用 `sim2real/policies/model_rough.onnx` +- 当前源模型/fallback：`sim2real/policies/model_rough.pt` +- 源模型：`model_2000.pt` +- actor 输入：`53D` +- actor 输出：`16D` +- 当前 actor 不吃 `base_lin_vel` +- 当前 actor 不吃 `height_scan` + +## 当前观测顺序 + +1. `base_ang_vel * 0.25` +2. `projected_gravity` +3. `command` +4. `joint_pos_rel`（12） +5. `joint_vel_rel * 0.05`（12） +6. `wheel_vel * 0.05`（4） +7. `last_actions`（16） + +## 当前控制定义 + +- 控制频率：`50Hz` +- 腿缩放：`0.125 / 0.25` +- 轮缩放：`5.0` +- 腿 LPF：`5Hz` +- 轮 LPF：`15Hz` + +## 当前仍依赖现场一致的部分 + +- IMU 安装方向与上一版校正一致 +- 当前 MJCF / 电机参数对应这次重新训练后的模型 +- 电机零位、方向、接线已按当前硬件修正 + +## 本次实现边界 + +不再支持： + +- `crawl` 模型 +- 多策略切换 +- `318D` 历史输入 +- 旧版 `startup.start_pose` + +## 本次排查结论 + +代码应只围绕当前 rough 模型运行。 +如果后续模型结构再改，必须重新核对观测、动作缩放、控制频率和部署文档。 diff --git a/05_software/real/sim2real_v2/ORIN_NANO_DEPLOYMENT.md b/05_software/real/sim2real_v2/ORIN_NANO_DEPLOYMENT.md new file mode 100644 index 0000000..42104c6 --- /dev/null +++ b/05_software/real/sim2real_v2/ORIN_NANO_DEPLOYMENT.md @@ -0,0 +1,45 @@ +# `Orin Nano` 部署说明 + +## 是否必须转 ONNX + +不必须。 + +当前优先级仍然是： + +1. 先保证观测、动作、站立控制对齐 +2. 再测 `50Hz` 实际环路稳定性 +3. 最后才决定是否转 `ONNX/TensorRT` + +## 当前代码重点 + +- `stand_balance` 已加入 `main.py` 和 `web/session.py` +- 启动后先站稳，再允许策略接管 +- `PolicyRunner.step()` 仍保留零命令抑制开关，默认开启 + +## Orin 上先测什么 + +- 机器人能否在不启动策略时，仅靠 `startup + stand_balance` 稳定站住 +- `loop_dt_ms` +- `imu_age_ms` +- 电机 stale +- policy forward 耗时 + +## 纯 Python 部署命令 + +默认前提：当前目录就是 `sim2real/` + +```bash +python3 -m pip install -r requirements-orin.txt +python3 tools/alignment_check.py --policy policies/model_rough.pt --manifest deployment_manifest.yaml +python3 tools/standalone_check.py +python3 main.py --dry-run +python3 main.py +python3 web/server.py --host 0.0.0.0 --port 8080 +``` + +## 首轮实机建议 + +1. 先不启动策略 +2. 只验证 `startup -> stand_balance` +3. 站稳后再启动策略 +4. 只给很小的 `vx / vy / yaw` diff --git a/05_software/real/sim2real_v2/README.md b/05_software/real/sim2real_v2/README.md new file mode 100644 index 0000000..f0a0a83 --- /dev/null +++ b/05_software/real/sim2real_v2/README.md @@ -0,0 +1,99 @@ +# Python Sim2Real v2 + +本目录归档 `real/sim2real_v2` 版本，对应 Python 真机部署从第一代闭环继续演进后的版本节点。该阶段保持 `53D -> 16D` 策略契约，重点完善电机反馈判断、IMU/Odin 可观测性、Web 调试和运行时诊断；后续 ROS 2/C++ 版本另行归档。 + +## 当前控制链路 + +- 策略模型：默认优先使用 `policies/model_rough.onnx`，`.pt` 仅作为导出源和 fallback +- 策略输入：单帧 `53D` +- 策略输出：`16D` +- 控制频率：默认 `50Hz` +- 电机数量：16 个 RS02/RobStride 电机 +- 电机映射、方向、零位：以 `interface/motor_mapping.py` 为准，当前视为真机验证过，不在本轮改造中修改 +- Web 入口：`python web/server.py --host 0.0.0.0 --port 8080` + +策略观测顺序保持不变： + +1. `base_ang_vel * 0.25` +2. `projected_gravity` +3. `command` +4. `joint_pos[:12] - default_dof_pos[:12]` +5. `joint_vel[:12] * 0.05` +6. `wheel_vel[12:] * 0.05` +7. `last_actions` + +Odin odom 现在只作为诊断和全局状态显示接入，不进入策略观测，避免破坏已训练模型的输入契约。 + +## 本轮改造内容 + +- 日志写入改为后台线程，降低 `state.csv` 高频写入对 50Hz 控制循环的阻塞。 +- 电机 fresh/stale 判断优先使用驱动层 `update_count`，避免静止电机反馈正常但位置速度不变时被误判为丢电机。 +- `wait_feedback_ready()` 同步识别 `update_count`，降低使能后零位静止电机被误判 missing 的概率。 +- `IMUClient` 增加 `get_latest_odom()`，读取 Odin odom 的位置、姿态、线速度、角速度、类型和 age。 +- `RealIO.read_state()` 增加只读 `odom` 字段，但不改变策略观测。 +- 增加 `OdomTracker`，把 Odin raw odom 转成启动点局部坐标 `local x/y/yaw`，并做跳变检测。 +- 增加 `LatestTarget` 诊断，记录当前下发目标来源、age、目标变化量，为后续双 loop 解耦铺路。 +- Web runtime 状态增加 `loop_profile`、odom、电机 fresh 计数、每电机 update_count。 +- Web Diagnostics 面板增加 loop overrun、最慢阶段、电机 fresh 来源、Odin odom 状态。 +- 增加 `command_filter`，对 Web/遥控命令统一做限加速度平滑，降低起停冲击；Web 同时显示 raw cmd 和 filtered cmd。 +- 策略推理入口改为 ONNXRuntime 优先，默认查找 `policies/model_rough.onnx`；不存在时兼容回退到 `policies/model_rough.pt`。 +- ONNXRuntime 使用 CPU 单线程顺序执行，减少推理线程池和 motor/status/logger 线程抢占。 + +## 重点诊断字段 + +Web 中重点看这些项： + +- `loop_dt`：控制循环总耗时，50Hz 下目标约 `20ms`。 +- `Loop Overruns`：runtime 超时累计次数和最大超时。 +- `Loop Profile`：显示本轮最慢阶段，例如 `read_state_ms`、`policy_ms`、`send_actions_ms`。 +- `Latest Target`：显示目标来源、age 和本次目标最大变化量；age 异常增大说明目标更新链路卡住。 +- `Obs / Action`：显示观测绝对值最大值、raw action 最大值和 scaled action 最大值，用来发现输入爆炸或动作饱和。 +- `Motor Fresh`：格式为 `fresh/16 (cnt x, val y)`；`cnt` 表示通过 `update_count` 确认的新反馈数量。 +- `Odin Odom`：显示 odom 类型、age、local x/y/yaw；没有 odom 时不影响控制。 +- `cmd/raw cmd`：`cmd` 是进入策略的滤波后命令，`raw cmd` 是 Web/遥控原始命令。 +- 电机列表状态点：绿色代表 stale count 为 0，黄色代表短时未刷新，红色代表连续 stale。 + +## 当前仍需实机重点确认 + +- 如果继续出现 `LOOP_OVERRUN`，先看 Web 的 `Loop Profile`，不要直接调低控制频率。 +- 如果 `read_state_ms` 慢，重点排查 SocketCAN/CAN 队列、Odin bridge 或电机反馈处理。 +- 如果 `policy_ms` 慢，重点排查 ONNXRuntime 推理耗时、CPU 占用和是否有后台进程抢占。 +- 如果 `send_actions_ms` 慢，重点排查 CAN 发送阻塞或 USB-CAN 适配器。 +- 如果 `Obs / Action` 中 obs 接近 `100` 或 raw action 接近 `10`，说明策略输入/输出可能在饱和边界，需要优先检查观测缩放、IMU、关节速度和命令。 +- 如果 `Motor Fresh` 不是 16/16，但 `update_counts` 在增长，需要检查 Web stale 阈值而不是电机丢失。 +- 如果 odom age 长时间不更新，只影响全局坐标/诊断，不应影响当前策略控制。 + +## 启动命令 + +在 Orin / Linux 真机上： + +```bash +python -m pip install -r requirements-orin.txt +python tools/export_onnx.py --pt policies/model_rough.pt --onnx policies/model_rough.onnx +python tools/alignment_check.py --policy policies/model_rough.onnx --manifest deployment_manifest.yaml +python tools/standalone_check.py +python web/server.py --host 0.0.0.0 --port 8080 +``` + +Web 流程： + +1. Connect +2. Enable Motors +3. Startup +4. Start Runtime +5. 策略 release 后，如需要，点击遥控接管 +6. 实时观察 Diagnostics、Motors、Plots + +## 安全边界 + +当前保留原有保护链路： + +- Web 急停 +- 遥控软急停 +- runtime guard +- safety monitor +- NaN/Inf 检查 +- 电机 stale holdover +- damping brake + +这轮没有放宽安全边界，也没有调整电机限位、零位、方向、默认增益和策略动作缩放。 diff --git a/05_software/real/sim2real_v2/config.yaml b/05_software/real/sim2real_v2/config.yaml new file mode 100644 index 0000000..8977c1f --- /dev/null +++ b/05_software/real/sim2real_v2/config.yaml @@ -0,0 +1,113 @@ +can1_port: "can0" +can2_port: "can1" +motor_model: "rs-02" +debug: false + +control_freq: 50 +policy_freq: 50 +motor_freq: 200 +status_freq: 10 +policy_stale_warn_ms: 60.0 +policy_timeout_ms: 150.0 +imu_lib_path: null + +controller: + # Runtime policy gains should stay softer than stand/startup gains. This + # follows the same separation used by rl_sar fixed_kp/fixed_kd vs rl_kp/rl_kd + # and avoids high-stiffness zero-command oscillation after policy release. + kp_leg: 50.0 + kd_leg: 1.5 + hold_kp_leg: 80.0 + hold_kd_leg: 4.0 + kd_wheel: 1.0 + max_vx: 0.8 + max_vy: 0.3 + max_yaw_rate: 0.5 + +policy: + enable_zero_cmd_suppression: true + hold_zero_command_pose: true + command_release_s: 0.35 + require_active_command_to_release: true + zero_cmd_use_yaw_rate: false + clip_obs: 100.0 + action_scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0] + release_command_hold_s: 0.12 + release_posture_max_err: 0.35 + release_target_blend_s: 0.30 + +stand_balance: + enabled: true + height: 0.33 + kp_roll: 0.85 + pitch_compensation_enabled: false + kp_pitch: 0.25 + kd_roll_rate: 0.03 + kd_pitch_rate: 0.03 + pitch_deadband_deg: 2.0 + pitch_corr_clip: 0.04 + pitch_corr_filter_alpha: 0.10 + pitch_front_sign: -1.0 + lateral_lean_gain: 0.0 + hip_abduction_clip: 0.45 + hip_pitch_clip: [-1.0, 2.5] + knee_clip: [-2.6, -0.3] + stable_roll_deg: 6.0 + stable_pitch_deg: 8.0 + stable_gyro_deg_s: 45.0 + enter_hold_s: 1.0 + profile_h: [0.157, 0.248, 0.311, 0.366, 0.411, 0.448] + profile_hip: [1.5, 1.2, 1.0, 0.8, 0.6, 0.4] + profile_knee: [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9] + +startup: + enabled: true + wait_for_enter_before_rise: false + soft_hold_duration: 1.0 + ramp_kp_time: 1.0 + transition_time_min: 2.0 + transition_time_max: 6.0 + transition_seconds_per_rad: 1.5 + timeout_extra: 3.0 + imu_fresh_wait_s: 1.0 + hold_time: 1.0 + settle_pos_threshold: 0.30 + settle_vel_threshold: 0.6 + progress_log_interval: 0.5 + max_dev_warn: 1.5 + max_dev_abort: 3.0 + require_user_confirm: true + +safety: + enabled: true + max_target_offset: 0.6 + hard_target_offset: 1.2 + max_ang_vel: 10.0 + max_tilt_z: -0.3 + clip_to_brake: 0 + imu_age_warn_ms: 60.0 + imu_age_stop_ms: 200.0 + +remote: + enabled: true + port: "/dev/ttyACM0" + baudrate: 100000 + timeout: 0.02 + axis_deadzone: 50 + active_threshold: 50 + axis_full_scale: 660.0 + max_vx: 0.3 + max_vy: 0.3 + max_yaw_rate: 0.3 + invert_vx: true + invert_vy: false + invert_yaw: true + +command_filter: + enabled: true + max_vx_acc: 1.0 + max_vy_acc: 1.0 + max_yaw_acc: 1.5 + +log_dir: "logs" +log_every: 1 diff --git a/05_software/real/sim2real_v2/deployment_manifest.yaml b/05_software/real/sim2real_v2/deployment_manifest.yaml new file mode 100644 index 0000000..eea09e0 --- /dev/null +++ b/05_software/real/sim2real_v2/deployment_manifest.yaml @@ -0,0 +1,102 @@ +model: + path: "policies/model_rough.onnx" + source_pt: "policies/model_rough.pt" + backend: "onnxruntime" + obs_dim: 53 + action_dim: 16 + clip_obs: 100.0 + enable_zero_cmd_suppression: true + +observation: + terms: + - name: base_ang_vel + dim: 3 + scale: 0.25 + - name: projected_gravity + dim: 3 + - name: command + dim: 3 + - name: joint_pos_rel + dim: 12 + - name: joint_vel_rel + dim: 12 + scale: 0.05 + - name: wheel_vel + dim: 4 + scale: 0.05 + - name: last_actions + dim: 16 + +action: + joint_order: + - fl_hip_abduction + - fl_hip_pitch + - fl_knee + - fr_hip_abduction + - fr_hip_pitch + - fr_knee + - rl_hip_abduction + - rl_hip_pitch + - rl_knee + - rr_hip_abduction + - rr_hip_pitch + - rr_knee + - fl_wheel + - fr_wheel + - rl_wheel + - rr_wheel + wheel_indices: [12, 13, 14, 15] + scale: + - 0.125 + - 0.25 + - 0.25 + - 0.125 + - 0.25 + - 0.25 + - 0.125 + - 0.25 + - 0.25 + - 0.125 + - 0.25 + - 0.25 + - 5.0 + - 5.0 + - 5.0 + - 5.0 + default_dof_pos: + - 0.0 + - 0.9 + - -1.8 + - 0.0 + - 0.9 + - -1.8 + - 0.0 + - 0.9 + - -1.8 + - 0.0 + - 0.9 + - -1.8 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + +control: + control_freq_hz: 50 + runtime_kp_leg: 50.0 + runtime_kd_leg: 1.5 + hold_kp_leg: 80.0 + hold_kd_leg: 4.0 + kd_wheel: 1.0 + leg_lpf_hz: 5 + wheel_lpf_hz: 15 + command_filter: + enabled: true + max_vx_acc: 1.0 + max_vy_acc: 1.0 + max_yaw_acc: 1.5 + +safety: + zero_cmd_lin_thresh: 0.05 + zero_cmd_yaw_thresh: 0.05 + zero_yaw_rate_thresh: 0.10 diff --git a/05_software/real/sim2real_v2/input_dev/__init__.py b/05_software/real/sim2real_v2/input_dev/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real_v2/input_dev/keyboard.py b/05_software/real/sim2real_v2/input_dev/keyboard.py new file mode 100644 index 0000000..d4a271f --- /dev/null +++ b/05_software/real/sim2real_v2/input_dev/keyboard.py @@ -0,0 +1,89 @@ +"""键盘控制器 — 兼容 sim2sim/input_dev/keyboard.py 的接口与平滑参数。""" +import numpy as np + +try: + from pynput import keyboard + PYNPUT_AVAILABLE = True +except ImportError: + PYNPUT_AVAILABLE = False + keyboard = None # type: ignore + + +class KeyboardCommandController: + """方向键 + AD 键的键盘指令源。 + 指令: [vx, vy, yaw_rate]，平滑加减速；空格触发急停标志。 + """ + + def __init__(self, + max_x_vel: float = 0.8, + max_y_vel: float = 0.3, + max_yaw_vel: float = 0.5, + acc_step: float = 0.05, + dec_step: float = 0.1): + if not PYNPUT_AVAILABLE: + raise RuntimeError("pynput 不可用，无法使用键盘控制；改用其他输入源。") + + self.current_cmd = np.zeros(3, dtype=np.float32) + self.max_x_vel = max_x_vel + self.max_y_vel = max_y_vel + self.max_yaw_vel = max_yaw_vel + self.acc_step = acc_step + self.dec_step = dec_step + + self._pressed = set() + self._estop = False + self.listener = keyboard.Listener( + on_press=self._on_press, on_release=self._on_release + ) + + def start(self): + self.listener.start() + print("[Keyboard] 启动。↑↓ 前后, ←→ 转向, A/D 横移, SPACE 急停") + + def stop(self): + try: + self.listener.stop() + except Exception: + pass + + def _on_press(self, key): + self._pressed.add(key) + if key == keyboard.Key.space: + self._estop = True + + def _on_release(self, key): + self._pressed.discard(key) + + def is_estop_triggered(self) -> bool: + return self._estop + + def reset_estop(self): + self._estop = False + + def get_command(self) -> np.ndarray: + target = np.zeros(3, dtype=np.float32) + if keyboard.Key.up in self._pressed: + target[0] += self.max_x_vel + if keyboard.Key.down in self._pressed: + target[0] -= self.max_x_vel + if keyboard.Key.left in self._pressed: + target[2] += self.max_yaw_vel + if keyboard.Key.right in self._pressed: + target[2] -= self.max_yaw_vel + try: + if keyboard.KeyCode.from_char('a') in self._pressed: + target[1] += self.max_y_vel + if keyboard.KeyCode.from_char('d') in self._pressed: + target[1] -= self.max_y_vel + except Exception: + pass + + for i, max_v in enumerate((self.max_x_vel, self.max_y_vel, self.max_yaw_vel)): + step = self.acc_step if target[i] != 0 else self.dec_step + if i == 2: + step *= 2.0 + if self.current_cmd[i] < target[i]: + self.current_cmd[i] = min(self.current_cmd[i] + step, target[i]) + else: + self.current_cmd[i] = max(self.current_cmd[i] - step, target[i]) + return self.current_cmd.copy() diff --git a/05_software/real/sim2real_v2/input_dev/remote_uart.py b/05_software/real/sim2real_v2/input_dev/remote_uart.py new file mode 100644 index 0000000..4183f44 --- /dev/null +++ b/05_software/real/sim2real_v2/input_dev/remote_uart.py @@ -0,0 +1,256 @@ +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Optional + +import numpy as np +import serial + +SBUS_FRAME_SIZE = 25 +SBUS_RC_MID = 1024 +SBUS_AXIS_SCALE = 660.0 + +SWITCH_LOW = -1 +SWITCH_MID = 0 +SWITCH_HIGH = 1 + + +@dataclass +class RemoteSwitchState: + ch7: int = SWITCH_MID + + +@dataclass +class RemoteControlState: + ch1: int = 0 + ch2: int = 0 + ch3: int = 0 + ch4: int = 0 + switches: RemoteSwitchState = field(default_factory=RemoteSwitchState) + frame_ok: bool = False + + def active_axes(self, threshold: int = 50) -> dict[str, bool]: + return { + "ch1": abs(self.ch1) > threshold, + "ch2": abs(self.ch2) > threshold, + "ch3": abs(self.ch3) > threshold, + "ch4": abs(self.ch4) > threshold, + } + + @property + def estop_requested(self) -> bool: + return self.switches.ch7 == SWITCH_HIGH + + def as_dict(self) -> dict: + return { + "ch1": int(self.ch1), + "ch2": int(self.ch2), + "ch3": int(self.ch3), + "ch4": int(self.ch4), + "switches": {"ch7": int(self.switches.ch7)}, + "frame_ok": bool(self.frame_ok), + "estop_requested": bool(self.estop_requested), + } + + +class RemoteUartReceiver: + def __init__(self, port: str, baudrate: int = 100000, timeout: float = 0.02, axis_deadzone: int = 50): + self.port = port + self.baudrate = int(baudrate) + self.timeout = float(timeout) + self.axis_deadzone = int(axis_deadzone) + self.serial: Optional[serial.Serial] = None + self._buffer = bytearray() + self.state = RemoteControlState() + + def open(self) -> None: + if self.serial and self.serial.is_open: + return + self.serial = serial.Serial( + port=self.port, + baudrate=self.baudrate, + timeout=self.timeout, + bytesize=serial.EIGHTBITS, + parity=serial.PARITY_EVEN, + stopbits=serial.STOPBITS_TWO, + ) + + def close(self) -> None: + if self.serial and self.serial.is_open: + self.serial.close() + + def poll(self) -> RemoteControlState: + if not self.serial or not self.serial.is_open: + raise RuntimeError("remote uart is not open") + + waiting = self.serial.in_waiting + if waiting: + self._buffer.extend(self.serial.read(waiting)) + + while len(self._buffer) >= SBUS_FRAME_SIZE: + start_idx = self._buffer.find(0x0F) + if start_idx < 0: + self._buffer.clear() + break + if start_idx > 0: + del self._buffer[:start_idx] + if len(self._buffer) < SBUS_FRAME_SIZE: + break + frame = bytes(self._buffer[:SBUS_FRAME_SIZE]) + del self._buffer[:SBUS_FRAME_SIZE] + parsed = self._parse_frame(frame) + if parsed is not None: + self.state = parsed + return self.state + + def _parse_frame(self, frame: bytes) -> Optional[RemoteControlState]: + if len(frame) != SBUS_FRAME_SIZE or frame[0] != 0x0F: + return None + + channels = [0] * 16 + channels[0] = (frame[1] | (frame[2] << 8)) & 0x07FF + channels[1] = ((frame[2] >> 3) | (frame[3] << 5)) & 0x07FF + channels[2] = ((frame[3] >> 6) | (frame[4] << 2) | (frame[5] << 10)) & 0x07FF + channels[3] = ((frame[5] >> 1) | (frame[6] << 7)) & 0x07FF + channels[4] = ((frame[6] >> 4) | (frame[7] << 4)) & 0x07FF + channels[5] = ((frame[7] >> 7) | (frame[8] << 1) | (frame[9] << 9)) & 0x07FF + channels[6] = ((frame[9] >> 2) | (frame[10] << 6)) & 0x07FF + channels[7] = ((frame[10] >> 5) | (frame[11] << 3)) & 0x07FF + channels[8] = (frame[12] | (frame[13] << 8)) & 0x07FF + channels[9] = ((frame[13] >> 3) | (frame[14] << 5)) & 0x07FF + + if channels[0] < 100: + return None + + state = RemoteControlState( + ch1=self._normalize_axis(channels[0]), + ch2=self._normalize_axis(channels[1]), + ch3=self._normalize_axis(channels[3]), + ch4=self._normalize_axis(channels[2]), + switches=RemoteSwitchState(ch7=self._decode_switch(channels[6])), + frame_ok=True, + ) + if any(abs(value) > 800 for value in (state.ch1, state.ch2, state.ch3, state.ch4)): + return None + return state + + def _normalize_axis(self, value: int) -> int: + mapped = int(round((value - SBUS_RC_MID) * SBUS_AXIS_SCALE / 800.0)) + return 0 if abs(mapped) <= self.axis_deadzone else mapped + + @staticmethod + def _decode_switch(value: int) -> int: + if value < 500: + return SWITCH_LOW + if value > 1500: + return SWITCH_HIGH + return SWITCH_MID + + +class RemoteCommandMapper: + def __init__( + self, + *, + max_vx: float, + max_vy: float, + max_yaw: float, + active_threshold: int = 50, + axis_full_scale: float = SBUS_AXIS_SCALE, + invert_vx: bool = False, + invert_vy: bool = False, + invert_yaw: bool = False, + ): + self.max_vx = float(max_vx) + self.max_vy = float(max_vy) + self.max_yaw = float(max_yaw) + self.active_threshold = int(active_threshold) + self.axis_full_scale = max(float(axis_full_scale), 1.0) + self.invert_vx = bool(invert_vx) + self.invert_vy = bool(invert_vy) + self.invert_yaw = bool(invert_yaw) + + def map_command(self, state: RemoteControlState) -> np.ndarray: + vx = self._axis_to_velocity(state.ch2, self.max_vx, self.invert_vx) + vy = self._axis_to_velocity(state.ch4, self.max_vy, self.invert_vy) + yaw = self._axis_to_velocity(state.ch1, self.max_yaw, self.invert_yaw) + return np.array([vx, vy, yaw], dtype=np.float32) + + def is_command_active(self, state: RemoteControlState) -> bool: + return any(abs(value) > self.active_threshold for value in (state.ch1, state.ch2, state.ch4)) + + def _axis_to_velocity(self, raw_value: int, limit: float, invert: bool) -> float: + if abs(raw_value) <= self.active_threshold: + return 0.0 + scaled = max(-1.0, min(1.0, raw_value / self.axis_full_scale)) + if invert: + scaled = -scaled + return float(scaled * limit) + + +class RemoteCommandSource: + def __init__( + self, + *, + port: str, + max_vx: float, + max_vy: float, + max_yaw: float, + baudrate: int = 100000, + timeout: float = 0.02, + axis_deadzone: int = 50, + active_threshold: int = 50, + axis_full_scale: float = SBUS_AXIS_SCALE, + invert_vx: bool = False, + invert_vy: bool = False, + invert_yaw: bool = False, + ): + self.receiver = RemoteUartReceiver( + port=port, + baudrate=baudrate, + timeout=timeout, + axis_deadzone=axis_deadzone, + ) + self.mapper = RemoteCommandMapper( + max_vx=max_vx, + max_vy=max_vy, + max_yaw=max_yaw, + active_threshold=active_threshold, + axis_full_scale=axis_full_scale, + invert_vx=invert_vx, + invert_vy=invert_vy, + invert_yaw=invert_yaw, + ) + self.last_state = RemoteControlState() + self.last_command = np.zeros(3, dtype=np.float32) + + @property + def port(self) -> str: + return self.receiver.port + + def open(self) -> None: + self.receiver.open() + + def close(self) -> None: + self.receiver.close() + + def poll(self) -> RemoteControlState: + self.last_state = self.receiver.poll() + self.last_command = self.mapper.map_command(self.last_state) + return self.last_state + + def get_command(self) -> np.ndarray: + return self.last_command.copy() + + def is_command_active(self) -> bool: + return self.mapper.is_command_active(self.last_state) + + def get_status(self) -> dict: + status = self.last_state.as_dict() + status.update( + { + "port": self.port, + "cmd": self.last_command.tolist(), + "command_active": bool(self.is_command_active()), + } + ) + return status diff --git a/05_software/real/sim2real_v2/interface/__init__.py b/05_software/real/sim2real_v2/interface/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real_v2/interface/imu_client.py b/05_software/real/sim2real_v2/interface/imu_client.py new file mode 100644 index 0000000..d2c12b4 --- /dev/null +++ b/05_software/real/sim2real_v2/interface/imu_client.py @@ -0,0 +1,229 @@ +"""Odin1 IMU 客户端封装。 + +核心改动相对 sim_rl/odin1/python/odin1_imu.py： +- 自动加载默认 .so 路径，调用方只需要 IMUClient(lib_path=...) +- 启动后做一次"重力对齐" — 用静止时的加速度计读数初始化 Mahony 滤波器， + 把首步姿态偏差从可能的 5°+ 降到 0.3° 内。这是方法论 D4 的关键一步。 +- 数据老化检测：若 imu_age_ms > stale_threshold 则报警（不阻塞）。 +""" +import sys +import time +from pathlib import Path +from typing import Dict, Optional + +import numpy as np + + +class IMUClient: + """Odin1 IMU 包装。 + + Args: + lib_path: libodin1_imu_bridge.so 的绝对路径；None 则按方法论 1.2 中 + 约定的相对位置寻找。 + gravity_align_samples: 启动时取多少帧加速度计平均值用于姿态初始化 + stale_threshold_ms: 单帧数据超过该 age 视为陈旧 + """ + + def __init__(self, lib_path: Optional[str] = None, gravity_align_samples: int = 50, + stale_threshold_ms: float = 50.0, dry_run: bool = False): + self._mock_mode = dry_run + self._initial_gravity = None + self._last_seq = -1 + self._last_fresh_time = 0.0 + self._last_odom_stamp = -1 + self._last_odom_fresh_time = 0.0 + + if self._mock_mode: + print("[IMUClient] 启动 Mock IMU 模式 (不加载物理 IMU 驱动)。") + self._client = None + return + + # 优先级 1: vendored/odin1_imu（独立部署模式） + # 优先级 2: ../../odin1/odin1/python（开发模式，即 sim_rl/odin1/odin1/python） + sim2real_root = Path(__file__).resolve().parents[1] + candidates = [ + sim2real_root / "vendored" / "odin1_imu", + sim2real_root.parents[1] / "odin1" / "odin1" / "python", + ] + for cand in candidates: + if cand.exists() and str(cand) not in sys.path: + sys.path.insert(0, str(cand)) + break + try: + from odin1_imu import Odin1ImuClient # type: ignore + # lib_path 默认查找：vendored/odin1_imu/build/libodin1_imu_bridge.so → 开发路径 + if lib_path is None: + so_candidates = [ + sim2real_root / "vendored" / "odin1_imu" / "build" / "libodin1_imu_bridge.so", + sim2real_root / "vendored" / "odin1_imu" / "libodin1_imu_bridge.so", + sim2real_root.parents[1] / "odin1" / "odin1" / "build" / "libodin1_imu_bridge.so", + ] + for so in so_candidates: + if so.exists(): + lib_path = str(so) + break + + self._client = Odin1ImuClient(lib_path=lib_path) + self._gravity_align_samples = gravity_align_samples + self._stale_threshold_ms = stale_threshold_ms + except Exception as e: + print(f"[IMUClient] 错误: 无法初始化实机 IMU 驱动 ({type(e).__name__}: {e})。实机部署下拒绝启动。") + raise + + def version(self) -> str: + if self._mock_mode: + return "MockIMU-v1.0" + return self._client.version() + + def start(self, timeout_ms: int = 8000): + """启动 IMU 流，并采集若干帧用于重力对齐。""" + if self._mock_mode: + self._initial_gravity = np.array([0.0, 0.0, 9.81], dtype=np.float32) + self._last_fresh_time = time.time() + self._last_odom_fresh_time = time.time() + return + self._client.start(timeout_ms=timeout_ms) + self._wait_for_stream() + self._initial_gravity = self._collect_gravity_samples() + self._last_fresh_time = time.time() + + def stop(self): + if self._mock_mode: + return + try: + self._client.stop() + except Exception: + pass + + @property + def initial_gravity(self) -> Optional[np.ndarray]: + """启动后的初始重力向量（机身坐标系），用于初始化 Mahony 四元数。""" + return self._initial_gravity + + def get_latest(self): + """返回 (gyro[3], accel[3], age_ms, fresh)；fresh=False 表示无新数据。""" + if self._mock_mode: + now = time.time() + age_ms = float(getattr(self, "_debug_mock_age_ms", 0.0)) + fresh = True + return (np.zeros(3, dtype=np.float32), + np.array([0.0, 0.0, 9.81], dtype=np.float32), + age_ms, fresh) + + sample = self._client.get_latest() + if sample is None: + return (np.zeros(3, dtype=np.float32), + np.array([0.0, 0.0, 9.81], dtype=np.float32), + -1.0, False) + gyro = np.array([sample.gyro_x, sample.gyro_y, sample.gyro_z], dtype=np.float32) + accel = np.array([sample.accel_x, sample.accel_y, sample.accel_z], dtype=np.float32) + # 用 stamp_ns 判断是否有新数据，因为 sequence 字段在 C++ 中可能没有赋值，导致永远为 0 + stamp = getattr(sample, "stamp_ns", 0) + now = time.time() + if stamp != self._last_seq: + self._last_seq = stamp + self._last_fresh_time = now + fresh = True + else: + fresh = False + age_ms = (now - self._last_fresh_time) * 1000.0 + return gyro, accel, age_ms, fresh + + def get_latest_odom(self) -> Optional[Dict[str, object]]: + """Return latest Odin odom for diagnostics only; policy observations stay unchanged.""" + if self._mock_mode: + now = time.time() + # 模拟一个围绕 (0, 0) 的圆形轨迹，用于测试 Web UI Canvas 绘图 + theta = now * 0.2 + x = 0.5 * np.cos(theta) + y = 0.5 * np.sin(theta) + return { + "type": "STANDARD", + "stamp_ns": int(now * 1e9), + "fresh": True, + "age_ms": 0.0, + "pos": [float(x), float(y), 0.33], + "quat_wxyz": [1.0, 0.0, 0.0, 0.0], + "linear_vel": [0.0, 0.0, 0.0], + "angular_vel": [0.0, 0.0, 0.2], + } + + getter = getattr(self._client, "odom_get_latest", None) + if getter is None: + return None + try: + sample = getter() + except Exception: + return None + if sample is None: + return None + + stamp = int(getattr(sample, "stamp_ns", 0)) + now = time.time() + if stamp != self._last_odom_stamp: + self._last_odom_stamp = stamp + self._last_odom_fresh_time = now + fresh = True + else: + fresh = False + age_ms = (now - self._last_odom_fresh_time) * 1000.0 if self._last_odom_fresh_time else -1.0 + return { + "type": _odom_type_name(int(getattr(sample, "type", -1))), + "stamp_ns": stamp, + "fresh": fresh, + "age_ms": age_ms, + "pos": [ + float(getattr(sample, "pos_x", 0.0)), + float(getattr(sample, "pos_y", 0.0)), + float(getattr(sample, "pos_z", 0.0)), + ], + "quat_wxyz": [ + float(getattr(sample, "orient_w", 1.0)), + float(getattr(sample, "orient_x", 0.0)), + float(getattr(sample, "orient_y", 0.0)), + float(getattr(sample, "orient_z", 0.0)), + ], + "linear_vel": [ + float(getattr(sample, "linear_vel_x", 0.0)), + float(getattr(sample, "linear_vel_y", 0.0)), + float(getattr(sample, "linear_vel_z", 0.0)), + ], + "angular_vel": [ + float(getattr(sample, "angular_vel_x", 0.0)), + float(getattr(sample, "angular_vel_y", 0.0)), + float(getattr(sample, "angular_vel_z", 0.0)), + ], + } + + # ---- 内部方法 ---- + def _wait_for_stream(self, timeout: float = 3.0): + deadline = time.time() + timeout + while time.time() < deadline: + if self._client.wait_for_data(timeout_ms=200): + # 有数据进来后清空一次队列以保证后续 get_latest 拿到的都是最新 + while self._client.pop_sample() is not None: + pass + return + raise RuntimeError("IMU 启动超时，未收到任何样本") + + def _collect_gravity_samples(self) -> np.ndarray: + accels = [] + for _ in range(self._gravity_align_samples): + sample = self._client.pop_sample() + if sample is None: + if not self._client.wait_for_data(timeout_ms=100): + continue + sample = self._client.pop_sample() + if sample is None: + continue + accels.append([sample.accel_x, sample.accel_y, sample.accel_z]) + if not accels: + print("[IMU] 警告: 重力对齐期间未收到样本，使用默认重力 [0,0,-9.81]") + return np.array([0.0, 0.0, -9.81], dtype=np.float32) + gravity = np.mean(accels, axis=0).astype(np.float32) + print(f"[IMU] 重力对齐完成: g_body = {gravity}") + return gravity + + +def _odom_type_name(value: int) -> str: + return {0: "STANDARD", 1: "HIGHFREQ", 2: "TF"}.get(value, f"UNKNOWN_{value}") diff --git a/05_software/real/sim2real_v2/interface/motor_driver.py b/05_software/real/sim2real_v2/interface/motor_driver.py new file mode 100644 index 0000000..0535e11 --- /dev/null +++ b/05_software/real/sim2real_v2/interface/motor_driver.py @@ -0,0 +1,364 @@ +"""RobStride 电机驱动包装。 + +职责： +- 封装 ik_real 中 RobStrideDriver 的 enable/disable/clear/control_mit 调用 +- **真实的丢包检测**：旧版用「value=0 启发式」会误判（电机回机械零位时也是 0）。 + 新方案： + 1. 调用 process_messages 前快照所有电机的 (pos, vel, torque) + 2. 调用后比较：状态变了 → 这一帧有新反馈；状态完全没变 → 累计 stale_count + 3. stale_count 超过阈值才沿用上一帧（方法论 3.4.2） + 仍然不完美（电机长时间静止确实会有连续多帧 state 不变），但比 0 启发式可靠。 +- 通过 driver_factory 由调用方注入：远程 Linux 主机用 RobStrideDriver， + 本地 Windows 调试可用 Mock。 +""" +from dataclasses import dataclass +import threading +from typing import Callable, Dict, List, Optional, Tuple + +import numpy as np + +from interface.motor_mapping import MotorMapping + + +@dataclass +class MotorReading: + position: float + velocity: float + torque: float = 0.0 + fresh: bool = False # True 表示本帧驱动板有新反馈 + + +class HardwareIO: + """统一的电机+IMU总线接口（不含策略），主控调用这一层。 + + Args: + driver_factory: () -> (drv1, drv2)，由调用方注入；返回的对象需要满足： + connect()/disconnect()/disable(name)/enable(name)/clear_warnings(name) + add_motor(name, mid, model)/process_messages() + control_mit(name, q, dq, kp, kd, tau) + .motors: dict[name -> motor], motor.state.position / .velocity / .torque + config: yaml 解析后的字典 + """ + + def __init__(self, driver_factory: Callable[[str, str, bool], Tuple[object, object]], + motor_model: str, can1_port: str, can2_port: str, debug: bool = False, + stale_frames_to_holdover: int = 2): + self.mapper = MotorMapping() + drv1, drv2 = driver_factory(can1_port, can2_port, debug) + self.driver_can1 = drv1 + self.driver_can2 = drv2 + self.motor_model = motor_model + self.stale_frames_to_holdover = stale_frames_to_holdover + + # 上一帧反馈（按 (bus, can_id) 索引），用于丢包兜底 + self._last_pos: Dict[Tuple[int, int], float] = {} + self._last_vel: Dict[Tuple[int, int], float] = {} + self._last_torque: Dict[Tuple[int, int], float] = {} + self._last_real_pos: Dict[Tuple[int, int], float] = {} + # 每个电机连续多少帧没收到新反馈 + self._stale_counts: Dict[Tuple[int, int], int] = {} + # 第一次必须读到才能解锁，避免初始化时直接用零位发送大力矩 + self._initialized = False + + self.lock = threading.Lock() + + # 累计诊断 + self.holdover_total = 0 # 累计被沿用上一帧的次数 + + # ---- 总线管理 ---- + def connect(self): + self.driver_can1.connect() + self.driver_can2.connect() + for jk in self.mapper.SIM_JOINT_ORDER: + leg, joint = jk + bus, mid = self.mapper.CAN_ID_MAP[jk] + name = f"{leg}_{joint}" + drv = self.driver_can1 if bus == 1 else self.driver_can2 + drv.add_motor(name, mid, self.motor_model) + self._stale_counts[(bus, mid)] = 0 + + def disconnect(self): + try: + self.driver_can1.disconnect() + finally: + self.driver_can2.disconnect() + + def enable_all(self): + for drv in (self.driver_can1, self.driver_can2): + for name in drv.motors: + drv.clear_warnings(name) + drv.enable(name) + + def disable_all(self): + for drv in (self.driver_can1, self.driver_can2): + for name in drv.motors: + drv.disable(name) + + # ---- 状态读取 ---- + def _snapshot_state(self) -> Dict[Tuple[int, int], Tuple[float, float, float, int, float, int, int]]: + """快照所有电机的 (pos, vel, torque, update_count, temperature, fault_code, mode_state)，process_messages 前后比较即可判 fresh。""" + snap: Dict[Tuple[int, int], Tuple[float, float, float, int, float, int, int]] = {} + for drv_idx, drv in enumerate((self.driver_can1, self.driver_can2)): + bus = drv_idx + 1 + for name, motor in drv.motors.items(): + parts = name.split("_", 1) + if len(parts) != 2: + continue + key = (parts[0], parts[1]) + if key not in self.mapper.CAN_ID_MAP: + continue + _, mid = self.mapper.CAN_ID_MAP[key] + s = motor.state + snap[(bus, mid)] = ( + s.position, + s.velocity, + s.torque, + getattr(s, "update_count", 0), + getattr(s, "temperature", 0.0), + getattr(s, "fault_code", 0), + getattr(s, "mode_state", 0) + ) + return snap + + def read_state(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray, Dict[str, object]]: + """返回 (sim_joint_pos[16], sim_joint_vel[16], sim_joint_torque[16], debug_info)。""" + with self.lock: + # 1) 抓取上一次的状态作为「pre」快照（基线） + pre = self._snapshot_state() + + # 2) 拉取本帧反馈 + self.driver_can1.process_messages() + self.driver_can2.process_messages() + + # 3) 抓取「post」快照 + post = self._snapshot_state() + + # 4) 比较：state 元组变了 → 本帧有新反馈，stale_count 清零；否则 stale_count++ + per_motor_fresh: Dict[Tuple[int, int], bool] = {} + fresh_by_update_count = 0 + fresh_by_value_change = 0 + update_counts: Dict[Tuple[int, int], int] = {} + for key in post: + pre_fields = pre.get(key) + post_fields = post[key] + pre_count = pre_fields[3] if pre_fields is not None and len(pre_fields) >= 4 else 0 + post_count = post_fields[3] if len(post_fields) >= 4 else 0 + update_counts[key] = int(post_count) + if post_count > pre_count: + fresh = True + fresh_by_update_count += 1 + elif pre_fields is not None and post_fields[:3] != pre_fields[:3]: + fresh = True + fresh_by_value_change += 1 + else: + fresh = False + per_motor_fresh[key] = fresh + if fresh: + self._stale_counts[key] = 0 + else: + self._stale_counts[key] += 1 + + # 5) 取出本帧 pos/vel；若该电机连续多帧没刷新，沿用上一帧（方法论 3.4.2） + real_pos: Dict[Tuple[int, int], float] = {} + real_vel: Dict[Tuple[int, int], float] = {} + real_torque: Dict[Tuple[int, int], float] = {} + real_temp: Dict[Tuple[int, int], float] = {} + real_fault: Dict[Tuple[int, int], int] = {} + real_mode: Dict[Tuple[int, int], int] = {} + holdover_this_frame = 0 + for key, snap_val in post.items(): + pos, vel, tor, _, temp, fault, mode = snap_val + real_temp[key] = temp + real_fault[key] = fault + real_mode[key] = mode + if (not per_motor_fresh[key]) and self._stale_counts[key] >= self.stale_frames_to_holdover: + # 长时间不刷新视作丢包：沿用上一帧 + if key in self._last_pos: + real_pos[key] = self._last_pos[key] + real_vel[key] = self._last_vel[key] + real_torque[key] = self._last_torque[key] + holdover_this_frame += 1 + else: + real_pos[key] = pos + real_vel[key] = vel + real_torque[key] = tor + else: + real_pos[key] = pos + real_vel[key] = vel + real_torque[key] = tor + + self.holdover_total += holdover_this_frame + # 缓存本帧（即便部分是 holdover 也缓存） + self._last_pos = real_pos.copy() + self._last_vel = real_vel.copy() + self._last_torque = real_torque.copy() + self._last_real_pos = real_pos.copy() + if not self._initialized: + self._initialized = True + + cur_pos = self.mapper.real_to_sim(real_pos) + cur_vel = self.mapper.real_vel_to_sim(real_vel) + cur_torque = self.mapper.real_vel_to_sim(real_torque) + + # 诊断信息 + stale_max = max(self._stale_counts.values()) if self._stale_counts else 0 + n_stale_motors = sum(1 for c in self._stale_counts.values() + if c >= self.stale_frames_to_holdover) + # 按 SIM_JOINT_ORDER 排列的每个电机连续丢帧数 + per_motor_stale = [ + self._stale_counts.get(self.mapper.CAN_ID_MAP[jk], 99) + for jk in self.mapper.SIM_JOINT_ORDER + ] + return cur_pos, cur_vel, cur_torque, { + "holdover_this_frame": holdover_this_frame, + "stale_max": stale_max, + "n_stale_motors": n_stale_motors, + "fresh_count": sum(1 for v in per_motor_fresh.values() if v), + "per_motor_stale": per_motor_stale, + "fresh_by_update_count": fresh_by_update_count, + "fresh_by_value_change": fresh_by_value_change, + "update_counts": [ + update_counts.get(self.mapper.CAN_ID_MAP[jk], 0) + for jk in self.mapper.SIM_JOINT_ORDER + ], + "temperatures": [ + real_temp.get(self.mapper.CAN_ID_MAP[jk], 0.0) + for jk in self.mapper.SIM_JOINT_ORDER + ], + "fault_codes": [ + real_fault.get(self.mapper.CAN_ID_MAP[jk], 0) + for jk in self.mapper.SIM_JOINT_ORDER + ], + "mode_states": [ + real_mode.get(self.mapper.CAN_ID_MAP[jk], 0) + for jk in self.mapper.SIM_JOINT_ORDER + ], + } + + def passive_poll(self): + """发送全 0 (0刚度0阻尼0力矩) 的 MIT 指令给所有电机。 + 目的：在 ENABLED 状态下，不产生力矩地索要反馈（因为 RobStride 在 MIT 模式下必须有指令才反馈）。""" + with self.lock: + for jk in self.mapper.SIM_JOINT_ORDER: + bus, mid = self.mapper.CAN_ID_MAP[jk] + name = f"{jk[0]}_{jk[1]}" + drv = self.driver_can1 if bus == 1 else self.driver_can2 + if name in drv.motors: + drv.control_mit(name, 0.0, 0.0, 0.0, 0.0, 0.0) + + # ---- 控制下发 ---- + def send_control(self, target_angles: np.ndarray, kp_leg: float, kd_leg: float, + kd_wheel: float): + """与 sim2sim 的 PD 模型对齐： + - 腿: position 控制，目标角度由 target_angles[:12] 给出，kp/kd 来自配置 + - 轮: velocity 控制，目标速度由 target_angles[12:] 给出，kd 阻尼 + """ + with self.lock: + if target_angles.shape != (16,): + raise ValueError("target_angles must be (16,)") + + real_targets = self.mapper.sim_to_real( + target_angles.astype(np.float32), + current_real_pos=self._last_real_pos, + ) + + # 轮毂速度目标暂且用 0，如果 target_angles 里包含了速度，就在 policy 那里处理， + # 这里的 target_angles 是 pose 目标，轮毂作为连续旋转关节其实位置控制没有意义。 + # 为了兼容旧代码，这里构造一个 16 维的 velocity array，只有后 4 个是目标（如果当作速度的话）。 + vel_targets = np.zeros(16, dtype=np.float32) + vel_targets[12:] = target_angles[12:].astype(np.float32) + real_wheel = self.mapper.sim_vel_to_real(vel_targets) + + for jk in self.mapper.SIM_JOINT_ORDER: + leg, joint = jk + bus, mid = self.mapper.CAN_ID_MAP[jk] + name = f"{leg}_{joint}" + drv = self.driver_can1 if bus == 1 else self.driver_can2 + if name not in drv.motors: + continue + + if joint == "wheel": + v = real_wheel[(bus, mid)] + drv.control_mit(name, 0.0, v, 0.0, kd_wheel, 0.0) + else: + q = real_targets[(bus, mid)] + drv.control_mit(name, q, 0.0, kp_leg, kd_leg, 0.0) + + def damping_brake(self, kd_leg: float, kd_wheel: float): + """急停模式：所有关节卸载刚度，仅保留阻尼。 + 对应 270_SimToReal 方法论 97.11 Level 2 "刹车"。 + """ + with self.lock: + for jk in self.mapper.SIM_JOINT_ORDER: + leg, joint = jk + bus, _ = self.mapper.CAN_ID_MAP[jk] + name = f"{leg}_{joint}" + drv = self.driver_can1 if bus == 1 else self.driver_can2 + if name not in drv.motors: + continue + kd = kd_wheel if joint == "wheel" else kd_leg + drv.control_mit(name, 0.0, 0.0, 0.0, kd, 0.0) + + def wait_feedback_ready(self, max_attempts: int = 20, + poll_interval: float = 0.05) -> Tuple[bool, list]: + """enable 后调用：尝试 max_attempts 次读总线，等所有 16 个电机 + 都至少给出一帧反馈。 + 返回 (all_ready, missing_motors)；missing_motors 是 (bus, mid, name) 列表。 + """ + import time + seen: Dict[Tuple[int, int], bool] = { + self.mapper.CAN_ID_MAP[jk]: False for jk in self.mapper.SIM_JOINT_ORDER + } + # 用第一次读到的 (pos, vel, torque) 三元组的"非零"或"已变化"作为反馈到达的判据。 + # 启动瞬间所有 motor.state 默认全 0，要么收到反馈让其变化，要么收到反馈但值确实是 0。 + # 退化情况下电机静止时 vel=0 且 pos=机械零位也=0，那种情况只能等多帧确认。 + snap_prev = self._snapshot_state() + for attempt in range(max_attempts): + with self.lock: + self.driver_can1.process_messages() + self.driver_can2.process_messages() + snap_cur = self._snapshot_state() + for key, fields_cur in snap_cur.items(): + if seen[key]: + continue + fields_prev = snap_prev.get(key) + # 任一字段不为 0 → 一定有反馈（因为初始值都是 0） + if any(v != 0.0 for v in fields_cur): + seen[key] = True + # 与上一次快照不同 → 一定有反馈（即便都很小） + elif fields_prev is not None and (fields_cur[3] > fields_prev[3] or fields_cur != fields_prev): + seen[key] = True + snap_prev = snap_cur + if all(seen.values()): + return True, [] + time.sleep(poll_interval) + + # 超时：列出仍未反馈的电机 + missing = [] + rev_can = {v: k for k, v in self.mapper.CAN_ID_MAP.items()} + for key, ok in seen.items(): + if not ok: + leg, joint = rev_can[key] + missing.append((key[0], key[1], f"{leg}_{joint}")) + return False, missing + + def read_measured_pose(self) -> np.ndarray: + """返回 (16,) 当前实测 sim 坐标系下的关节位置。 + 会先 process_messages 一次保证拿到本帧。 + """ + self.driver_can1.process_messages() + self.driver_can2.process_messages() + real_pos: Dict[Tuple[int, int], float] = {} + for drv_idx, drv in enumerate((self.driver_can1, self.driver_can2)): + bus = drv_idx + 1 + for name, motor in drv.motors.items(): + parts = name.split("_", 1) + if len(parts) != 2: + continue + key = (parts[0], parts[1]) + if key not in self.mapper.CAN_ID_MAP: + continue + _, mid = self.mapper.CAN_ID_MAP[key] + real_pos[(bus, mid)] = motor.state.position + self._last_real_pos = real_pos.copy() + return self.mapper.real_to_sim(real_pos) diff --git a/05_software/real/sim2real_v2/interface/motor_mapping.py b/05_software/real/sim2real_v2/interface/motor_mapping.py new file mode 100644 index 0000000..b1d17d2 --- /dev/null +++ b/05_software/real/sim2real_v2/interface/motor_mapping.py @@ -0,0 +1,123 @@ +"""仿真→实机电机映射。 + +数据来源：sim_rl/ik_real/sim_to_real_deploy_beifen.py 和 +sim_rl/sim2real/motor_mapping.py 中的 sign / offset / can_id 表（已在实机上验证）。 +关节顺序与 rc_mjlab/sim2sim 完全一致：[12 个腿关节] + [4 个轮子]。 +""" +from typing import Dict, Tuple + +import numpy as np + + +class MotorMapping: + TWO_PI = float(2.0 * np.pi) + LEG_NAMES = ("fl", "fr", "rl", "rr") + JOINT_NAMES = ("hip_abduction", "hip_pitch", "knee", "wheel") + + SIM_JOINT_ORDER = ( + ("fl", "hip_abduction"), ("fl", "hip_pitch"), ("fl", "knee"), + ("fr", "hip_abduction"), ("fr", "hip_pitch"), ("fr", "knee"), + ("rl", "hip_abduction"), ("rl", "hip_pitch"), ("rl", "knee"), + ("rr", "hip_abduction"), ("rr", "hip_pitch"), ("rr", "knee"), + ("fl", "wheel"), ("fr", "wheel"), ("rl", "wheel"), ("rr", "wheel"), + ) + SIM_INDEX_MAP = {jk: i for i, jk in enumerate(SIM_JOINT_ORDER)} + + CAN_ID_MAP: Dict[Tuple[str, str], Tuple[int, int]] = { + ("fl", "hip_abduction"): (1, 1), ("fl", "hip_pitch"): (1, 2), + ("fl", "knee"): (1, 3), ("fl", "wheel"): (1, 4), + ("fr", "hip_abduction"): (1, 5), ("fr", "hip_pitch"): (1, 6), + ("fr", "knee"): (1, 7), ("fr", "wheel"): (1, 8), + ("rl", "hip_abduction"): (2, 1), ("rl", "hip_pitch"): (2, 2), + ("rl", "knee"): (2, 3), ("rl", "wheel"): (2, 4), + ("rr", "hip_abduction"): (2, 5), ("rr", "hip_pitch"): (2, 6), + ("rr", "knee"): (2, 7), ("rr", "wheel"): (2, 8), + } + + DIRECTION_MAP: Dict[Tuple[str, str], int] = { + ("fl", "hip_abduction"): -1, ("fl", "hip_pitch"): -1, + ("fl", "knee"): -1, ("fl", "wheel"): -1, + ("fr", "hip_abduction"): -1, ("fr", "hip_pitch"): 1, + ("fr", "knee"): 1, ("fr", "wheel"): 1, + ("rl", "hip_abduction"): 1, ("rl", "hip_pitch"): -1, + ("rl", "knee"): -1, ("rl", "wheel"): -1, + ("rr", "hip_abduction"): 1, ("rr", "hip_pitch"): 1, + ("rr", "knee"): 1, ("rr", "wheel"): 1, + } + + ZERO_OFFSET_MAP: Dict[Tuple[str, str], float] = { + ("fl", "hip_abduction"): 0.003, ("fl", "hip_pitch"): 0.030, + ("fl", "knee"): 0.028, ("fl", "wheel"): 0.000, + ("fr", "hip_abduction"): 0.004, ("fr", "hip_pitch"): 0.038, + ("fr", "knee"): 0.011, ("fr", "wheel"): 0.000, + ("rl", "hip_abduction"): 0.019, ("rl", "hip_pitch"): -0.034, + ("rl", "knee"): 0.025, ("rl", "wheel"): 0.000, + ("rr", "hip_abduction"): -0.001, ("rr", "hip_pitch"): 0.039, + ("rr", "knee"): 0.018, ("rr", "wheel"): 0.000, + } + + def __init__(self): + self.num_motors = len(self.SIM_JOINT_ORDER) + self._sign = np.array([self.DIRECTION_MAP[jk] for jk in self.SIM_JOINT_ORDER], dtype=np.float32) + self._offset = np.array([self.ZERO_OFFSET_MAP[jk] for jk in self.SIM_JOINT_ORDER], dtype=np.float32) + self._default_pose = np.array( + [0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.9, -1.8, 0.0, 0.0, 0.0, 0.0], + dtype=np.float32, + ) + + @classmethod + def _nearest_periodic(cls, value: float, reference: float) -> float: + return float(reference + ((value - reference + np.pi) % cls.TWO_PI - np.pi)) + + def canonicalize_sim(self, sim_angles: np.ndarray, reference: np.ndarray | None = None) -> np.ndarray: + """Map leg joint angles onto the 2*pi branch nearest the deployment reference pose.""" + out = np.asarray(sim_angles, dtype=np.float32).copy() + ref = self._default_pose if reference is None else np.asarray(reference, dtype=np.float32) + for i, (_, joint) in enumerate(self.SIM_JOINT_ORDER[:12]): + out[i] = self._nearest_periodic(float(out[i]), float(ref[i])) + return out + + def sim_to_real( + self, + sim_angles: np.ndarray, + current_real_pos: Dict[Tuple[int, int], float] | None = None, + ) -> Dict[Tuple[int, int], float]: + if len(sim_angles) != 16: + raise ValueError(f"expected 16 sim angles, got {len(sim_angles)}") + out: Dict[Tuple[int, int], float] = {} + for i, jk in enumerate(self.SIM_JOINT_ORDER): + real = float(self._sign[i] * sim_angles[i] + self._offset[i]) + can_key = self.CAN_ID_MAP[jk] + if current_real_pos is not None and i < 12 and can_key in current_real_pos: + real = self._nearest_periodic(real, float(current_real_pos[can_key])) + out[can_key] = real + return out + + def sim_vel_to_real(self, sim_vels: np.ndarray) -> Dict[Tuple[int, int], float]: + # 速度只受方向影响，不应用 offset。 + out: Dict[Tuple[int, int], float] = {} + for i, jk in enumerate(self.SIM_JOINT_ORDER): + out[self.CAN_ID_MAP[jk]] = float(self._sign[i] * sim_vels[i]) + return out + + def real_to_sim(self, real_pos: Dict[Tuple[int, int], float]) -> np.ndarray: + out = np.zeros(16, dtype=np.float32) + for i, jk in enumerate(self.SIM_JOINT_ORDER): + v = real_pos.get(self.CAN_ID_MAP[jk]) + if v is None: + continue + out[i] = (v - self._offset[i]) / self._sign[i] + return self.canonicalize_sim(out) + + def real_vel_to_sim(self, real_vel: Dict[Tuple[int, int], float]) -> np.ndarray: + out = np.zeros(16, dtype=np.float32) + for i, jk in enumerate(self.SIM_JOINT_ORDER): + v = real_vel.get(self.CAN_ID_MAP[jk]) + if v is None: + continue + out[i] = v / self._sign[i] + return out + + def joint_name_at(self, idx: int) -> str: + leg, joint = self.SIM_JOINT_ORDER[idx] + return f"{leg}_{joint}_joint" diff --git a/05_software/real/sim2real_v2/interface/real_io.py b/05_software/real/sim2real_v2/interface/real_io.py new file mode 100644 index 0000000..38d8418 --- /dev/null +++ b/05_software/real/sim2real_v2/interface/real_io.py @@ -0,0 +1,216 @@ +import time +from typing import Callable, Dict, Tuple + +import numpy as np + +from interface.imu_client import IMUClient +from interface.motor_driver import HardwareIO +from tools.math_utils import LowPassFilter, MahonyFilter, get_gravity_orientation + + +def _quat_yaw_wxyz(quat) -> float: + w, x, y, z = [float(v) for v in quat] + return float(np.arctan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))) + + +def _wrap_pi(angle: float) -> float: + return float((angle + np.pi) % (2.0 * np.pi) - np.pi) + + +class OdomTracker: + def __init__(self, jump_distance_m: float = 0.5, jump_yaw_rad: float = 0.8): + self.jump_distance_m = float(jump_distance_m) + self.jump_yaw_rad = float(jump_yaw_rad) + self.origin_pos = None + self.origin_yaw = 0.0 + self.last_local_pos = None + self.last_local_yaw = 0.0 + + def reset(self): + self.origin_pos = None + self.origin_yaw = 0.0 + self.last_local_pos = None + self.last_local_yaw = 0.0 + + def update(self, odom): + if odom is None: + return None + pos = np.asarray(odom.get("pos", [0.0, 0.0, 0.0]), dtype=np.float32) + yaw = _quat_yaw_wxyz(odom.get("quat_wxyz", [1.0, 0.0, 0.0, 0.0])) + if self.origin_pos is None: + self.origin_pos = pos.copy() + self.origin_yaw = yaw + local_pos = pos - self.origin_pos + local_yaw = _wrap_pi(yaw - self.origin_yaw) + jump_detected = False + jump_distance = 0.0 + jump_yaw = 0.0 + if self.last_local_pos is not None: + jump_distance = float(np.linalg.norm(local_pos[:2] - self.last_local_pos[:2])) + jump_yaw = abs(_wrap_pi(local_yaw - self.last_local_yaw)) + jump_detected = jump_distance > self.jump_distance_m or jump_yaw > self.jump_yaw_rad + self.last_local_pos = local_pos.copy() + self.last_local_yaw = local_yaw + tracked = dict(odom) + tracked.update( + { + "local_pos": local_pos.tolist(), + "local_yaw": local_yaw, + "jump_detected": bool(jump_detected), + "jump_distance_m": jump_distance, + "jump_yaw_rad": jump_yaw, + } + ) + return tracked + + +class RealIO: + def __init__( + self, + driver_factory: Callable[[str, str, bool], Tuple[object, object]], + motor_model: str, + can1_port: str, + can2_port: str, + imu_lib_path: str, + control_dt: float = 0.02, + motor_dt: float = 0.005, + kp_leg: float = 80.0, + kd_leg: float = 2.5, + hold_kp_leg: float | None = None, + hold_kd_leg: float | None = None, + kd_wheel: float = 2.0, + debug: bool = False, + dry_run: bool = False, + ): + self.control_dt = control_dt + self.motor_dt = motor_dt + self.kp_leg = kp_leg + self.kd_leg = kd_leg + self.hold_kp_leg = kp_leg if hold_kp_leg is None else float(hold_kp_leg) + self.hold_kd_leg = kd_leg if hold_kd_leg is None else float(hold_kd_leg) + self.kd_wheel = kd_wheel + + print("[RealIO] 初始化电机驱动...") + self.hw = HardwareIO(driver_factory, motor_model, can1_port, can2_port, debug) + print("[RealIO] 初始化 IMU...") + self.imu = IMUClient(lib_path=imu_lib_path, dry_run=dry_run) + + # 使用 motor_dt 初始化滤波器，因为它们都在 200Hz 电机控制循环中更新 + self.imu_filter = MahonyFilter(kp=2.0, ki=0.0, dt=motor_dt) + self.quat_wxyz = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32) + + self.lpf_legs = LowPassFilter(cutoff_freq=5.0, dt=motor_dt, dim=12) + self.lpf_wheels = LowPassFilter(cutoff_freq=15.0, dt=motor_dt, dim=4) + + self._last_imu_age_ms = -1.0 + self._last_imu_fresh = False + self.odom_tracker = OdomTracker() + self._last_read_time = None + + def connect(self, imu_timeout_ms: int = 8000): + self.hw.connect() + self.imu.start(timeout_ms=imu_timeout_ms) + if self.imu.initial_gravity is not None: + self.imu_filter.reset_with_accel(self.imu.initial_gravity) + self.quat_wxyz = self.imu_filter.q.copy() + self.odom_tracker.reset() + self._last_read_time = None + + def disconnect(self): + try: + self.hw.disable_all() + finally: + self.imu.stop() + self.hw.disconnect() + + def enable_motors(self): + self.hw.enable_all() + + def disable_motors(self): + self.hw.disable_all() + + def damping_brake(self): + self.hw.damping_brake(self.kd_leg, self.kd_wheel) + + def wait_feedback_ready(self, max_attempts: int = 20, poll_interval: float = 0.05): + return self.hw.wait_feedback_ready(max_attempts=max_attempts, poll_interval=poll_interval) + + def read_measured_pose(self) -> np.ndarray: + return self.hw.read_measured_pose() + + def read_state(self) -> Dict[str, object]: + joint_pos, joint_vel, joint_torque, motor_diag = self.hw.read_state() + gyro, accel, age_ms, fresh = self.imu.get_latest() + odom = self.odom_tracker.update(self.imu.get_latest_odom()) + self._last_imu_age_ms = age_ms + self._last_imu_fresh = fresh + + # 动态测量 dt，以适应 POLL (5Hz) 与 RUNTIME (200Hz) 的不同频率切换 + t_now = time.perf_counter() + if self._last_read_time is not None: + dt = t_now - self._last_read_time + if dt <= 0.0 or dt > 0.5: + dt = self.motor_dt + else: + dt = self.motor_dt + self._last_read_time = t_now + + self.quat_wxyz = self.imu_filter.update(accel, gyro, dt=dt) + projected_gravity = get_gravity_orientation(self.quat_wxyz) + + return { + "joint_pos": joint_pos, + "joint_vel": joint_vel, + "joint_torque": joint_torque, + "imu_gyro": gyro, + "imu_accel": accel, + "quat_wxyz": self.quat_wxyz.copy(), + "projected_gravity": projected_gravity, + "imu_age_ms": age_ms, + "imu_fresh": fresh, + "odom": odom, + "motor_stale": motor_diag, + } + + def get_obs_policy( + self, + state: Dict[str, object], + command: np.ndarray, + default_dof_pos: np.ndarray, + last_actions_raw: np.ndarray, + ) -> np.ndarray: + gyro = state["imu_gyro"] + joint_pos = state["joint_pos"] + joint_vel = state["joint_vel"] + projected_gravity = state["projected_gravity"] + + base_ang_vel = (gyro * 0.25).astype(np.float32) + joint_pos_rel = (joint_pos[:12] - default_dof_pos[:12]).astype(np.float32) + joint_vel_leg = (joint_vel[:12] * 0.05).astype(np.float32) + wheel_vel = (joint_vel[12:] * 0.05).astype(np.float32) + + return np.concatenate( + [ + base_ang_vel, + projected_gravity, + command.astype(np.float32), + joint_pos_rel, + joint_vel_leg, + wheel_vel, + last_actions_raw, + ] + ).astype(np.float32) + + def send_actions(self, scaled_actions: np.ndarray, default_dof_pos: np.ndarray): + act = (scaled_actions + default_dof_pos).astype(np.float32) + act = np.clip(act, -100.0, 100.0) + act[:12] = self.lpf_legs.filter(act[:12]) + act[12:] = self.lpf_wheels.filter(act[12:]) + self.hw.send_control(act, self.kp_leg, self.kd_leg, self.kd_wheel) + return act + + def hold_pose(self, sim_target_pose: np.ndarray, kp_scale: float = 1.0): + target = np.clip(sim_target_pose.astype(np.float32), -100.0, 100.0) + kp_scale = float(np.clip(kp_scale, 0.0, 1.0)) + self.hw.send_control(target, self.hold_kp_leg * kp_scale, self.hold_kd_leg, self.kd_wheel) + return target diff --git a/05_software/real/sim2real_v2/main.py b/05_software/real/sim2real_v2/main.py new file mode 100644 index 0000000..a0524eb --- /dev/null +++ b/05_software/real/sim2real_v2/main.py @@ -0,0 +1,727 @@ +"""CLI entrypoint for current sim2real deployment.""" + +import argparse +import os +import sys +import threading +import time +from pathlib import Path + +import numpy as np +import yaml + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +from input_dev.keyboard import KeyboardCommandController +from interface.real_io import RealIO +from policy.policy_runner import PolicyRunner, resolve_policy_path +from safety.runtime_guard import GuardLevel, RuntimeGuard +from safety.safety_monitor import SafetyLevel, SafetyMonitor +from startup.pose_initializer import PoseInitFailed, PoseInitializer, STAND_POSE +from startup.stand_balance import StandBalanceController +from tools.logger import LogBundle +from tools.math_utils import get_gravity_orientation + +JOINT_LABELS = LogBundle.JOINT_LABELS + + +def make_real_driver_factory(): + sim2real_root = Path(__file__).resolve().parent + workspace_root = sim2real_root.parent.parent + for path in (workspace_root,): + path_str = str(path) + if path_str not in sys.path and path.exists(): + sys.path.append(path_str) + + from dm_socket.sim2real_factory import driver_factory + + return driver_factory + + +def make_dry_driver_factory(): + class MockMotor: + def __init__(self): + class State: + position = 0.0 + velocity = 0.0 + torque = 0.0 + + self.state = State() + + class MockDriver: + def __init__(self, port, debug): + self.port = port + self.motors = {} + + def connect(self): ... + def disconnect(self): ... + def add_motor(self, name, motor_id, model): self.motors[name] = MockMotor() + def enable(self, name): ... + def disable(self, name): ... + def clear_warnings(self, name): ... + def process_messages(self): ... + def control_mit(self, *args, **kwargs): ... + + def factory(can1_port, can2_port, debug): + return MockDriver(can1_port, debug), MockDriver(can2_port, debug) + + return factory + + +def _sleep_to(next_exec: float) -> float: + slack = next_exec - time.perf_counter() + if slack > 0: + time.sleep(slack) + return next_exec + 0.0 + return time.perf_counter() + + +def build_action_diag( + *, + joint_pos: np.ndarray, + default_pose: np.ndarray, + raw: np.ndarray, + scaled: np.ndarray, + tentative: np.ndarray, + cmd: np.ndarray, + zero_command: bool, + runtime_released: bool, + release_alpha: float, + safety_details: dict | None = None, +) -> dict: + details = dict(safety_details or {}) + joint_indices = list(details.get("joint_indices", [])) + pos_err = tentative - joint_pos + leg_offset = tentative[:12] - default_pose[:12] + diag = { + "joint_indices": joint_indices, + "joint_names": [JOINT_LABELS[i] for i in joint_indices if 0 <= i < len(JOINT_LABELS)], + "cmd": cmd.tolist(), + "zero_command": bool(zero_command), + "runtime_released": bool(runtime_released), + "release_alpha": float(release_alpha), + "max_raw": float(np.max(np.abs(raw))) if raw.size else 0.0, + "max_scaled": float(np.max(np.abs(scaled[:12]))) if scaled.size else 0.0, + "max_target": float(np.max(np.abs(tentative[:12]))) if tentative.size else 0.0, + } + if joint_indices: + primary = int(joint_indices[0]) + diag.update( + { + "primary_joint_index": primary, + "primary_joint_name": JOINT_LABELS[primary], + "primary_target": float(tentative[primary]), + "primary_default": float(default_pose[primary]), + "primary_measured": float(joint_pos[primary]), + "primary_pos_err": float(pos_err[primary]), + "primary_raw": float(raw[primary]), + "primary_scaled": float(scaled[primary]), + } + ) + if primary < 12: + diag["primary_leg_offset"] = float(leg_offset[primary]) + details.update(diag) + return details + + +def policy_release_cfg(cfg: dict) -> dict[str, float]: + policy_cfg = cfg.get("policy", {}) + return { + "command_hold_s": max(float(policy_cfg.get("release_command_hold_s", 0.12)), 0.0), + "posture_max_err": max(float(policy_cfg.get("release_posture_max_err", 0.35)), 0.0), + "target_blend_s": max(float(policy_cfg.get("release_target_blend_s", 0.30)), 1e-3), + } + + +def compute_release_metrics(runner: PolicyRunner, state: dict, hold_target: np.ndarray, cmd: np.ndarray) -> dict: + joint_pos = np.asarray(state["joint_pos"], dtype=np.float32) + default_pose = np.asarray(runner.default_dof_pos, dtype=np.float32) + hold_target = np.asarray(hold_target, dtype=np.float32) + planar_cmd, yaw_cmd = runner.command_activation_metrics(cmd) + return { + "planar_cmd": float(planar_cmd), + "yaw_cmd": float(yaw_cmd), + "max_hold_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))), + "max_default_err": float(np.max(np.abs(joint_pos[:12] - default_pose[:12]))), + "max_hold_default_gap": float(np.max(np.abs(hold_target[:12] - default_pose[:12]))), + } + + +def blend_runtime_target( + runner: PolicyRunner, + hold_target: np.ndarray, + policy_target: np.ndarray, + release_alpha: float, + target_blend_s: float, + control_dt: float, +) -> np.ndarray: + blend = min(1.0, release_alpha * (runner.command_release_s / max(target_blend_s, control_dt))) + return ((1.0 - blend) * hold_target + blend * policy_target).astype(np.float32) + + +def compute_target_error_metrics( + state: dict, + hold_target: np.ndarray, + policy_target: np.ndarray, +) -> dict[str, float]: + joint_pos = np.asarray(state["joint_pos"], dtype=np.float32) + hold_target = np.asarray(hold_target, dtype=np.float32) + policy_target = np.asarray(policy_target, dtype=np.float32) + return { + "hold_target_max_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))), + "policy_target_max_err": float(np.max(np.abs(joint_pos[:12] - policy_target[:12]))), + "hold_policy_max_gap": float(np.max(np.abs(hold_target[:12] - policy_target[:12]))), + } + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--config", default=str(Path(__file__).parent / "config.yaml")) + parser.add_argument("--policy", default=None) + parser.add_argument("--dry-run", action="store_true") + args = parser.parse_args() + + with open(args.config, "r", encoding="utf-8") as file_obj: + cfg = yaml.safe_load(file_obj) + + sim2real_root = Path(__file__).resolve().parent + policy_path = resolve_policy_path(args.policy, sim2real_root) + if not policy_path.exists(): + print(f"[Main] policy not found: {policy_path}") + sys.exit(1) + + control_dt = 1.0 / float(cfg["control_freq"]) + driver_factory = make_dry_driver_factory() if args.dry_run else make_real_driver_factory() + + logger = LogBundle(cfg["log_dir"]) + logger.event( + "CONFIG_LOADED", + config_path=args.config, + policy=str(policy_path), + dry_run=args.dry_run, + control_freq=cfg["control_freq"], + motor_model=cfg["motor_model"], + ) + + io = RealIO( + driver_factory=driver_factory, + motor_model=cfg["motor_model"], + can1_port=cfg["can1_port"], + can2_port=cfg["can2_port"], + imu_lib_path=cfg.get("imu_lib_path"), + control_dt=control_dt, + kp_leg=cfg["controller"]["kp_leg"], + kd_leg=cfg["controller"]["kd_leg"], + hold_kp_leg=cfg["controller"].get("hold_kp_leg", cfg["controller"]["kp_leg"]), + hold_kd_leg=cfg["controller"].get("hold_kd_leg", cfg["controller"]["kd_leg"]), + kd_wheel=cfg["controller"]["kd_wheel"], + debug=cfg.get("debug", False), + ) + runner = PolicyRunner( + policy_path, + enable_zero_cmd_suppression=cfg.get("policy", {}).get("enable_zero_cmd_suppression", True), + hold_zero_command_pose=cfg.get("policy", {}).get("hold_zero_command_pose", True), + command_release_s=cfg.get("policy", {}).get("command_release_s", 0.35), + action_scale=np.asarray( + cfg.get("policy", {}).get( + "action_scale", + [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0], + ), + dtype=np.float32, + ), + zero_cmd_use_yaw_rate=cfg.get("policy", {}).get("zero_cmd_use_yaw_rate", False), + clip_obs=cfg.get("policy", {}).get("clip_obs", 100.0), + ) + require_active_command = cfg.get("policy", {}).get("require_active_command_to_release", True) + keyboard = KeyboardCommandController( + max_x_vel=cfg["controller"]["max_vx"], + max_y_vel=cfg["controller"]["max_vy"], + max_yaw_vel=cfg["controller"]["max_yaw_rate"], + ) + safety = SafetyMonitor( + max_target_offset=cfg["safety"]["max_target_offset"], + max_ang_vel=cfg["safety"]["max_ang_vel"], + max_tilt_z=cfg["safety"]["max_tilt_z"], + clip_to_brake=cfg["safety"].get("clip_to_brake", 0), + hard_target_offset=cfg["safety"].get("hard_target_offset", 1.2), + ) + safety.reset() + guard = RuntimeGuard( + max_ang_vel=cfg["safety"]["max_ang_vel"], + max_tilt_z=cfg["safety"]["max_tilt_z"], + imu_age_warn_ms=cfg["safety"].get("imu_age_warn_ms", 60.0), + imu_age_stop_ms=cfg["safety"].get("imu_age_stop_ms", 200.0), + ) + initializer = PoseInitializer( + io, + control_dt=control_dt, + transition_time_min=cfg["startup"].get("transition_time_min", 2.0), + transition_time_max=cfg["startup"].get("transition_time_max", 6.0), + transition_seconds_per_rad=cfg["startup"].get("transition_seconds_per_rad", 1.5), + hold_time=cfg["startup"]["hold_time"], + settle_pos_threshold=cfg["startup"]["settle_pos_threshold"], + settle_vel_threshold=cfg["startup"]["settle_vel_threshold"], + timeout_extra=cfg["startup"].get("timeout_extra", 3.0), + imu_fresh_wait_s=cfg["startup"].get("imu_fresh_wait_s", 1.0), + progress_log_interval=cfg["startup"]["progress_log_interval"], + ramp_kp_time=cfg["startup"].get("ramp_kp_time", 1.0), + soft_hold_duration=cfg["startup"].get("soft_hold_duration", 1.0), + max_dev_warn=cfg["startup"].get("max_dev_warn", 1.5), + max_dev_abort=cfg["startup"].get("max_dev_abort", 3.0), + ) + initializer.attach(logger=logger, guard=guard, keyboard=keyboard) + stand_balance = StandBalanceController(cfg.get("stand_balance", {}), control_dt=control_dt) + + print("\n[Main] connecting hardware...") + keyboard.start() + try: + io.connect() + logger.event("CAN_IMU_CONNECTED", initial_gravity=io.imu.initial_gravity) + except Exception as exc: + logger.event("HARDWARE_CONNECT_FAILED", error=str(exc)) + keyboard.stop() + logger.close() + raise + + try: + io.enable_motors() + logger.event("MOTORS_ENABLED") + time.sleep(0.5) + + target_pose = initializer.transition_to_stand_from_current(target_pose=STAND_POSE) if cfg["startup"]["enabled"] else STAND_POSE.copy() + + if stand_balance.enabled: + logger.event("STAND_BALANCE_BEGIN") + print("[Main] waiting for stand-balance to settle...") + stand_balance.reset() + next_exec = time.perf_counter() + while True: + state = io.read_state() + target_pose = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) + io.hold_pose(target_pose, kp_scale=1.0) + debug = stand_balance.last_debug + if stand_balance.is_stable(): + logger.event( + "STAND_BALANCE_STABLE", + roll_deg=float(np.degrees(debug.roll)), + pitch_deg=float(np.degrees(debug.pitch)), + pitch_corr=float(debug.pitch_corr), + pitch_compensation_enabled=bool(debug.pitch_compensation_enabled), + ) + break + next_exec += control_dt + next_exec = _sleep_to(next_exec) + logger.event("STAND_BALANCE_END") + + if cfg["startup"]["require_user_confirm"]: + print("[Main] standing complete. Press Enter to release policy control...") + done = threading.Event() + + def _wait(): + try: + input() + except EOFError: + pass + done.set() + + threading.Thread(target=_wait, daemon=True).start() + if not initializer.hold_until_user_confirm(target_pose, done): + raise PoseInitFailed("WAIT_USER interrupted") + + print("[Main] priming current observation...") + logger.event("PRIME_BEGIN") + zero_cmd = np.zeros(3, dtype=np.float32) + next_exec = time.perf_counter() + for index in range(1): + if stand_balance.enabled: + state = io.read_state() + target_pose = stand_balance.compute_target(state, zero_cmd) + io.hold_pose(target_pose, kp_scale=1.0) + else: + io.hold_pose(target_pose, kp_scale=1.0) + state = io.read_state() + obs = io.get_obs_policy(state, zero_cmd, runner.default_dof_pos, runner.last_actions) + if index == 0: + runner.reset(prime_obs=obs) + logger.state( + phase="PRIME", + joint_pos=state["joint_pos"], + joint_vel=state["joint_vel"], + joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)), + target_pose=target_pose, + raw_action=None, + gyro=state["imu_gyro"], + accel=state["imu_accel"], + quat=state["quat_wxyz"], + proj_gravity=state["projected_gravity"], + command=zero_cmd, + imu_age_ms=float(state["imu_age_ms"]), + loop_dt_ms=0.0, + kp_scale=1.0, + ) + next_exec += control_dt + next_exec = _sleep_to(next_exec) + logger.event("PRIME_END") + + print("[Main] entering 50Hz control loop... (space = estop)") + logger.event("RUNTIME_BEGIN") + next_exec = time.perf_counter() + loop_count = 0 + last_print = next_exec + log_every = int(cfg.get("log_every", 1)) + recent_dt_ms = [] + runtime_released = not require_active_command + release_cfg = policy_release_cfg(cfg) + release_active_time = 0.0 + + while True: + loop_t0 = time.perf_counter() + cmd = keyboard.get_command() + state = io.read_state() + obs = io.get_obs_policy(state, cmd, runner.default_dof_pos, runner.last_actions) + zero_command = runner._is_zero_command(cmd, state["imu_gyro"]) + + obs_nan = bool(np.any(np.isnan(obs)) or np.any(np.isinf(obs))) + if obs_nan: + logger.event("OBS_NAN", obs_max=float(np.nanmax(obs))) + io.damping_brake() + break + + if not runtime_released and zero_command: + raw = np.zeros(16, dtype=np.float32) + scaled = np.zeros(16, dtype=np.float32) + target_hold = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if stand_balance.enabled else runner.default_dof_pos.copy() + actual_target = io.hold_pose(target_hold, kp_scale=1.0) + policy_target = runner.default_dof_pos.copy() + release_metrics = compute_release_metrics(runner, state, target_hold, cmd) + target_metrics = compute_target_error_metrics(state, target_hold, policy_target) + release_active_time = 0.0 + safety_decision = SafetyMonitor().check( + target_pose=target_hold, + default_pose=runner.default_dof_pos, + imu_gyro=state["imu_gyro"], + projected_gravity=state["projected_gravity"], + estop_triggered=keyboard.is_estop_triggered(), + ) + guard_decision = guard.check( + imu_gyro=state["imu_gyro"], + projected_gravity=state["projected_gravity"], + imu_age_ms=float(state["imu_age_ms"]), + estop_triggered=keyboard.is_estop_triggered(), + extra_nan_arrays=(target_hold,), + ) + else: + target_hold = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if stand_balance.enabled else runner.default_dof_pos.copy() + release_metrics = compute_release_metrics(runner, state, target_hold, cmd) + if not runtime_released: + release_active_time += control_dt if runner.is_command_active(cmd) else 0.0 + active_ready = release_active_time >= release_cfg["command_hold_s"] + posture_ready = release_metrics["max_hold_err"] <= release_cfg["posture_max_err"] + if active_ready and posture_ready: + runtime_released = True + logger.event( + "RUNTIME_COMMAND_RELEASED", + cmd=cmd.tolist(), + active_hold_s=release_active_time, + max_hold_err=release_metrics["max_hold_err"], + max_default_err=release_metrics["max_default_err"], + max_hold_default_gap=release_metrics["max_hold_default_gap"], + ) + else: + reasons = [] + if not active_ready: + reasons.append(f"cmd_hold<{release_cfg['command_hold_s']:.2f}s") + if not posture_ready: + reasons.append(f"hold_err>{release_cfg['posture_max_err']:.3f}") + logger.event( + "RUNTIME_RELEASE_BLOCKED", + reason=",".join(reasons), + cmd=cmd.tolist(), + active_hold_s=release_active_time, + max_hold_err=release_metrics["max_hold_err"], + max_default_err=release_metrics["max_default_err"], + max_hold_default_gap=release_metrics["max_hold_default_gap"], + ) + raw = np.zeros(16, dtype=np.float32) + scaled = np.zeros(16, dtype=np.float32) + actual_target = io.hold_pose(target_hold, kp_scale=1.0) + policy_target = runner.default_dof_pos.copy() + target_metrics = compute_target_error_metrics(state, target_hold, policy_target) + safety_decision = SafetyMonitor().check( + target_pose=target_hold, + default_pose=runner.default_dof_pos, + imu_gyro=state["imu_gyro"], + projected_gravity=state["projected_gravity"], + estop_triggered=keyboard.is_estop_triggered(), + ) + guard_decision = guard.check( + imu_gyro=state["imu_gyro"], + projected_gravity=state["projected_gravity"], + imu_age_ms=float(state["imu_age_ms"]), + estop_triggered=keyboard.is_estop_triggered(), + extra_nan_arrays=(target_hold,), + ) + loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0 + if log_every and (loop_count % log_every == 0): + motor_diag = state.get("motor_stale", {}) + logger.state( + phase="RUNTIME", + joint_pos=state["joint_pos"], + joint_vel=state["joint_vel"], + joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)), + target_pose=actual_target, + raw_action=raw, + gyro=state["imu_gyro"], + accel=state["imu_accel"], + quat=state["quat_wxyz"], + proj_gravity=state["projected_gravity"], + command=cmd, + imu_age_ms=float(state["imu_age_ms"]), + loop_dt_ms=loop_dt_ms, + safety_level=int(safety_decision.level), + guard_level=int(guard_decision.level), + holdover=int(motor_diag.get("holdover_this_frame", 0)), + stale_max=int(motor_diag.get("stale_max", 0)), + fresh_count=int(motor_diag.get("fresh_count", 16)), + kp_scale=1.0, + nan_flag=0, + kp_leg_cmd=float(io.kp_leg), + kd_leg_cmd=float(io.kd_leg), + kd_wheel_cmd=float(io.kd_wheel), + runtime_release_alpha=0.0, + runtime_release_hold_s=release_active_time, + runtime_blend_ratio=0.0, + hold_target_max_err=target_metrics["hold_target_max_err"], + policy_target_max_err=target_metrics["policy_target_max_err"], + hold_policy_max_gap=target_metrics["hold_policy_max_gap"], + target_source="runtime_hold", + clip_primary_joint="", + safety_reason=f"release_blocked:{','.join(reasons)}", + guard_reason=guard_decision.reason, + ) + next_exec += control_dt + next_exec = _sleep_to(next_exec) + loop_count += 1 + continue + scaled, raw = runner.step(obs) + act_nan = bool(np.any(np.isnan(raw)) or np.any(np.isinf(raw))) + if act_nan: + logger.event("ACTION_NAN") + io.damping_brake() + break + + policy_target = (scaled + runner.default_dof_pos).astype(np.float32) + tentative = blend_runtime_target( + runner, + target_hold, + policy_target, + float(getattr(runner, "_command_release_alpha", 0.0)), + release_cfg["target_blend_s"], + control_dt, + ) + scaled = tentative - runner.default_dof_pos + target_metrics = compute_target_error_metrics(state, target_hold, policy_target) + runtime_blend_ratio = min( + 1.0, + float(getattr(runner, "_command_release_alpha", 0.0)) + * (runner.command_release_s / max(release_cfg["target_blend_s"], control_dt)), + ) + projected_gravity = get_gravity_orientation(state["quat_wxyz"]) + + guard_decision = guard.check( + imu_gyro=state["imu_gyro"], + projected_gravity=projected_gravity, + imu_age_ms=float(state["imu_age_ms"]), + estop_triggered=keyboard.is_estop_triggered(), + extra_nan_arrays=(raw, tentative), + ) + if guard_decision.level == GuardLevel.STOP: + logger.event("GUARD_STOP", phase="RUNTIME", reason=guard_decision.reason) + io.damping_brake() + break + + safety_decision = safety.check( + target_pose=tentative, + default_pose=runner.default_dof_pos, + imu_gyro=state["imu_gyro"], + projected_gravity=projected_gravity, + estop_triggered=keyboard.is_estop_triggered(), + ) + if safety_decision.level == SafetyLevel.ESTOP: + logger.event("SAFETY_ESTOP", reason=safety_decision.message) + io.damping_brake() + break + if safety_decision.level == SafetyLevel.BRAKE: + safety_diag = build_action_diag( + joint_pos=state["joint_pos"], + default_pose=runner.default_dof_pos, + raw=raw, + scaled=scaled, + tentative=tentative, + cmd=cmd, + zero_command=zero_command, + runtime_released=runtime_released, + release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)), + safety_details=safety_decision.details, + ) + logger.event( + "SAFETY_BRAKE", + reason=safety_decision.message, + details=safety_diag, + primary_joint=safety_diag.get("primary_joint_name"), + primary_offset=safety_diag.get("primary_leg_offset"), + primary_target=safety_diag.get("primary_target"), + primary_measured=safety_diag.get("primary_measured"), + primary_raw=safety_diag.get("primary_raw"), + primary_scaled=safety_diag.get("primary_scaled"), + cmd=cmd.tolist(), + release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)), + ) + io.damping_brake() + break + if safety_decision.level == SafetyLevel.CLIP and safety_decision.clipped_target is not None: + scaled = safety_decision.clipped_target - runner.default_dof_pos + safety_diag = build_action_diag( + joint_pos=state["joint_pos"], + default_pose=runner.default_dof_pos, + raw=raw, + scaled=scaled, + tentative=tentative, + cmd=cmd, + zero_command=zero_command, + runtime_released=runtime_released, + release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)), + safety_details=safety_decision.details, + ) + logger.event( + "SAFETY_CLIP", + reason=safety_decision.message, + details=safety_diag, + primary_joint=safety_diag.get("primary_joint_name"), + primary_offset=safety_diag.get("primary_leg_offset"), + primary_target=safety_diag.get("primary_target"), + primary_measured=safety_diag.get("primary_measured"), + primary_raw=safety_diag.get("primary_raw"), + primary_scaled=safety_diag.get("primary_scaled"), + max_raw=float(np.max(np.abs(raw))), + cmd=cmd.tolist(), + release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)), + ) + + actual_target = io.send_actions(scaled, runner.default_dof_pos) + loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0 + + if log_every and (loop_count % log_every == 0): + motor_diag = state.get("motor_stale", {}) + logger.state( + phase="RUNTIME", + joint_pos=state["joint_pos"], + joint_vel=state["joint_vel"], + joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)), + target_pose=actual_target, + raw_action=raw, + gyro=state["imu_gyro"], + accel=state["imu_accel"], + quat=state["quat_wxyz"], + proj_gravity=projected_gravity, + command=cmd, + imu_age_ms=float(state["imu_age_ms"]), + loop_dt_ms=loop_dt_ms, + safety_level=int(safety_decision.level), + guard_level=int(guard_decision.level), + holdover=int(motor_diag.get("holdover_this_frame", 0)), + stale_max=int(motor_diag.get("stale_max", 0)), + fresh_count=int(motor_diag.get("fresh_count", 16)), + kp_scale=1.0, + nan_flag=int(obs_nan or act_nan), + kp_leg_cmd=float(io.kp_leg), + kd_leg_cmd=float(io.kd_leg), + kd_wheel_cmd=float(io.kd_wheel), + runtime_release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)), + runtime_release_hold_s=release_active_time, + runtime_blend_ratio=runtime_blend_ratio, + hold_target_max_err=target_metrics["hold_target_max_err"], + policy_target_max_err=target_metrics["policy_target_max_err"], + hold_policy_max_gap=target_metrics["hold_policy_max_gap"], + target_source="runtime_blend" if runtime_blend_ratio < 0.999 else "runtime_policy", + clip_primary_joint=str((safety_decision.details or {}).get("primary_joint_name", "")), + clip_primary_target=float((safety_decision.details or {}).get("primary_target", 0.0) or 0.0), + clip_primary_measured=float((safety_decision.details or {}).get("primary_measured", 0.0) or 0.0), + clip_primary_default=float((safety_decision.details or {}).get("primary_default", 0.0) or 0.0), + clip_primary_pos_err=float((safety_decision.details or {}).get("primary_pos_err", 0.0) or 0.0), + clip_primary_raw=float((safety_decision.details or {}).get("primary_raw", 0.0) or 0.0), + clip_primary_scaled=float((safety_decision.details or {}).get("primary_scaled", 0.0) or 0.0), + safety_reason=( + f"{safety_decision.message};zero_cmd={int(zero_command)};" + f"released={int(runtime_released)};alpha={getattr(runner, '_command_release_alpha', 0.0):.2f};" + f"max_raw={float(np.max(np.abs(raw))):.2f};" + f"clip={((safety_decision.details or {}).get('joint_indices', []))}" + ), + guard_reason=guard_decision.reason, + ) + + next_exec += control_dt + slack = next_exec - time.perf_counter() + if slack > 0: + coarse = slack - 0.002 + if coarse > 0: + time.sleep(coarse) + while time.perf_counter() < next_exec: + pass + elif slack < -control_dt: + logger.event("LOOP_OVERRUN", over_ms=-slack * 1000.0) + next_exec = time.perf_counter() + + recent_dt_ms.append(loop_dt_ms) + if len(recent_dt_ms) > 50: + recent_dt_ms.pop(0) + if len(recent_dt_ms) == 50: + median_dt = float(np.median(recent_dt_ms)) + if median_dt > 22.0: + logger.event("SLOW_LOOP_TREND", median_dt_ms=median_dt) + recent_dt_ms.clear() + + loop_count += 1 + if time.perf_counter() - last_print > 1.0: + print( + f"[Loop] cmd=[{cmd[0]:+.2f},{cmd[1]:+.2f},{cmd[2]:+.2f}] " + f"|raw|={float(np.max(np.abs(raw))):.2f} " + f"zero={int(zero_command)} rel={int(runtime_released)} " + f"alpha={getattr(runner, '_command_release_alpha', 0.0):.2f} " + f"imu_age={state['imu_age_ms']:.1f}ms " + f"holdover={io.hw.holdover_total} " + f"safety={int(safety_decision.level)}" + ) + last_print = time.perf_counter() + + except PoseInitFailed as exc: + print(f"[Main] startup aborted: {exc}") + logger.event("POSE_INIT_FAILED", error=str(exc)) + except KeyboardInterrupt: + print("\n[Main] Ctrl+C received, stopping...") + logger.event("KEYBOARD_INTERRUPT") + except Exception as exc: + import traceback + + print(f"\n[Main] exception: {exc}") + traceback.print_exc() + logger.event("UNEXPECTED_ERROR", error=str(exc), traceback=traceback.format_exc()) + finally: + print("[Main] cleaning up...") + try: + io.damping_brake() + time.sleep(0.05) + logger.event("DAMPING_BRAKE_APPLIED") + except Exception as exc: + logger.event("DAMPING_BRAKE_FAILED", error=str(exc)) + try: + io.disconnect() + logger.event("HARDWARE_DISCONNECTED") + finally: + keyboard.stop() + logger.close() + os._exit(0) + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real_v2/mjcf/meshes/base_link.STL b/05_software/real/sim2real_v2/mjcf/meshes/base_link.STL new file mode 100644 index 0000000..015035f Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/base_link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/fl_hip_abduction_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/fl_hip_abduction_Link.STL new file mode 100644 index 0000000..971a78e Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/fl_hip_abduction_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/fl_hip_pitch_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/fl_hip_pitch_Link.STL new file mode 100644 index 0000000..e1803e0 Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/fl_hip_pitch_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/fl_knee_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/fl_knee_Link.STL new file mode 100644 index 0000000..cc5f40c Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/fl_knee_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/fl_wheel_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/fl_wheel_Link.STL new file mode 100644 index 0000000..c3e124b Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/fl_wheel_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/fr_hip_abduction_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/fr_hip_abduction_Link.STL new file mode 100644 index 0000000..8a15920 Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/fr_hip_abduction_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/fr_hip_pitch_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/fr_hip_pitch_Link.STL new file mode 100644 index 0000000..909e5ae Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/fr_hip_pitch_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/fr_knee_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/fr_knee_Link.STL new file mode 100644 index 0000000..802f2eb Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/fr_knee_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/fr_wheel_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/fr_wheel_Link.STL new file mode 100644 index 0000000..9c17db1 Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/fr_wheel_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/rl_hip_abduction_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/rl_hip_abduction_Link.STL new file mode 100644 index 0000000..0680c00 Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/rl_hip_abduction_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/rl_hip_pitch_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/rl_hip_pitch_Link.STL new file mode 100644 index 0000000..ab8ffa7 Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/rl_hip_pitch_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/rl_knee_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/rl_knee_Link.STL new file mode 100644 index 0000000..5d64c75 Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/rl_knee_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/rl_wheel_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/rl_wheel_Link.STL new file mode 100644 index 0000000..5bab538 Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/rl_wheel_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/rr_hip_abduction_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/rr_hip_abduction_Link.STL new file mode 100644 index 0000000..4013ccf Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/rr_hip_abduction_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/rr_hip_pitch_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/rr_hip_pitch_Link.STL new file mode 100644 index 0000000..5f81a5f Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/rr_hip_pitch_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/rr_knee_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/rr_knee_Link.STL new file mode 100644 index 0000000..28dccab Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/rr_knee_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/meshes/rr_wheel_Link.STL b/05_software/real/sim2real_v2/mjcf/meshes/rr_wheel_Link.STL new file mode 100644 index 0000000..a9d9ecb Binary files /dev/null and b/05_software/real/sim2real_v2/mjcf/meshes/rr_wheel_Link.STL differ diff --git a/05_software/real/sim2real_v2/mjcf/scene.xml b/05_software/real/sim2real_v2/mjcf/scene.xml new file mode 100644 index 0000000..155ffb2 --- /dev/null +++ b/05_software/real/sim2real_v2/mjcf/scene.xml @@ -0,0 +1,22 @@ + + + + diff --git a/05_software/real/sim2real_v2/mjcf/wheelleg.xml b/05_software/real/sim2real_v2/mjcf/wheelleg.xml new file mode 100644 index 0000000..960a3cb --- /dev/null +++ b/05_software/real/sim2real_v2/mjcf/wheelleg.xml @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/05_software/real/sim2real_v2/policies/model_rough.onnx b/05_software/real/sim2real_v2/policies/model_rough.onnx new file mode 100644 index 0000000..357b1a8 Binary files /dev/null and b/05_software/real/sim2real_v2/policies/model_rough.onnx differ diff --git a/05_software/real/sim2real_v2/policies/model_rough.pt b/05_software/real/sim2real_v2/policies/model_rough.pt new file mode 100644 index 0000000..f0be4c3 Binary files /dev/null and b/05_software/real/sim2real_v2/policies/model_rough.pt differ diff --git a/05_software/real/sim2real_v2/policy/__init__.py b/05_software/real/sim2real_v2/policy/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real_v2/policy/policy_runner.py b/05_software/real/sim2real_v2/policy/policy_runner.py new file mode 100644 index 0000000..06fb75e --- /dev/null +++ b/05_software/real/sim2real_v2/policy/policy_runner.py @@ -0,0 +1,243 @@ +from pathlib import Path + +import numpy as np +import torch +import torch.nn as nn + + +def resolve_policy_path(policy_path: Path | str | None, root: Path | None = None) -> Path: + """Prefer ONNX for deployment while keeping the PT file as source/fallback.""" + root = root or Path(__file__).resolve().parents[1] + if policy_path is None: + onnx_path = root / "policies" / "model_rough.onnx" + pt_path = root / "policies" / "model_rough.pt" + return onnx_path if onnx_path.exists() else pt_path + + path = Path(policy_path) + if not path.is_absolute(): + path = root / path + if path.suffix.lower() == ".pt": + onnx_path = path.with_suffix(".onnx") + if onnx_path.exists(): + return onnx_path + return path + + +class PolicyMLP(nn.Module): + def __init__(self, obs_dim: int, action_dim: int): + super().__init__() + self.register_buffer("obs_mean", torch.zeros(obs_dim)) + self.register_buffer("obs_std", torch.ones(obs_dim)) + self.net = nn.Sequential( + nn.Linear(obs_dim, 512), + nn.ELU(), + nn.Linear(512, 256), + nn.ELU(), + nn.Linear(256, 128), + nn.ELU(), + nn.Linear(128, action_dim), + ) + + def forward(self, x: torch.Tensor) -> torch.Tensor: + x = (x - self.obs_mean) / torch.clamp(self.obs_std, min=1e-6) + return self.net(x) + + +class OnnxPolicy: + def __init__(self, model_path: Path): + try: + import onnxruntime as ort + except ImportError as exc: + raise ImportError( + "onnxruntime is required for ONNX policy inference. " + "Install it on Orin with `python -m pip install onnxruntime`." + ) from exc + + opts = ort.SessionOptions() + opts.intra_op_num_threads = 1 + opts.inter_op_num_threads = 1 + opts.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL + self.session = ort.InferenceSession( + str(model_path), + sess_options=opts, + providers=["CPUExecutionProvider"], + ) + self.input_name = self.session.get_inputs()[0].name + self.output_name = self.session.get_outputs()[0].name + input_shape = self.session.get_inputs()[0].shape + output_shape = self.session.get_outputs()[0].shape + self.expected_obs_dim = int(input_shape[1]) if len(input_shape) >= 2 and isinstance(input_shape[1], int) else 53 + self.expected_action_dim = int(output_shape[1]) if len(output_shape) >= 2 and isinstance(output_shape[1], int) else 16 + self.backend = "onnxruntime" + + self.obs_mean = torch.zeros(self.expected_obs_dim) + self.obs_std = torch.ones(self.expected_obs_dim) + + def __call__(self, x: torch.Tensor) -> torch.Tensor: + obs = x.detach().cpu().numpy().astype(np.float32, copy=False) + action = self.session.run([self.output_name], {self.input_name: obs})[0] + return torch.from_numpy(np.asarray(action, dtype=np.float32)).to(x.device) + + +def load_policy(model_path: Path, device: torch.device): + if model_path.suffix.lower() == ".onnx": + return OnnxPolicy(model_path) + + checkpoint = torch.load(model_path, map_location=device, weights_only=False) + state_dict = checkpoint["actor_state_dict"] + + input_key = "mlp.0.weight" if "mlp.0.weight" in state_dict else "net.0.weight" + output_key = "mlp.6.weight" if "mlp.6.weight" in state_dict else "net.6.weight" + obs_dim = int(state_dict[input_key].shape[1]) + action_dim = int(state_dict[output_key].shape[0]) + + model = PolicyMLP(obs_dim=obs_dim, action_dim=action_dim) + remapped_state_dict: dict[str, torch.Tensor] = {} + for key, value in state_dict.items(): + if key.startswith("mlp."): + remapped_state_dict[key.replace("mlp.", "net.")] = value + elif key.startswith("net."): + remapped_state_dict[key] = value + elif key == "obs_normalizer._mean": + remapped_state_dict["obs_mean"] = value.squeeze() + elif key == "obs_normalizer._var": + remapped_state_dict["obs_std"] = torch.sqrt(value.squeeze() + 1e-5) + + model.load_state_dict(remapped_state_dict, strict=False) + model.eval() + model.to(device) + model.expected_obs_dim = obs_dim + model.expected_action_dim = action_dim + model.backend = "torch" + return model + + +class PolicyRunner: + BASE_OBS_DIM = 53 + DEFAULT_STAND_POSE = np.array( + [ + 0.0, 0.9, -1.8, + 0.0, 0.9, -1.8, + 0.0, 0.9, -1.8, + 0.0, 0.9, -1.8, + 0.0, 0.0, 0.0, 0.0, + ], + dtype=np.float32, + ) + + def __init__( + self, + policy_path: Path, + device: torch.device | None = None, + enable_zero_cmd_suppression: bool = True, + hold_zero_command_pose: bool = True, + command_release_s: float = 0.35, + action_scale: np.ndarray | None = None, + zero_cmd_use_yaw_rate: bool = True, + clip_obs: float = 100.0, + ): + self.device = device or torch.device("cuda" if torch.cuda.is_available() else "cpu") + self.policy_path = Path(policy_path) + self.enable_zero_cmd_suppression = bool(enable_zero_cmd_suppression) + self.hold_zero_command_pose = bool(hold_zero_command_pose) + self.command_release_s = max(float(command_release_s), 1e-3) + self.clip_obs = max(float(clip_obs), 0.0) + self.policy_path = resolve_policy_path(self.policy_path) + if not self.policy_path.exists(): + raise FileNotFoundError(f"policy file not found: {self.policy_path}") + print(f"[PolicyRunner] device={self.device}, policy={self.policy_path}") + self.policy = load_policy(self.policy_path, self.device) + if self.policy.expected_obs_dim != self.BASE_OBS_DIM: + raise ValueError( + f"Unsupported policy obs dim {self.policy.expected_obs_dim}. " + f"Current sim2real only supports {self.BASE_OBS_DIM}-D actor observations." + ) + + self.default_dof_pos = self.DEFAULT_STAND_POSE.copy() + self.last_actions = np.zeros(16, dtype=np.float32) + + self.action_scale = np.asarray( + action_scale + if action_scale is not None + else [ + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 5.0, 5.0, 5.0, 5.0, + ], + dtype=np.float32, + ) + if self.action_scale.shape != (16,): + raise ValueError(f"action_scale must be shape (16,), got {self.action_scale.shape}") + + self.zero_cmd_lin_thresh = 0.05 + self.zero_cmd_yaw_thresh = 0.05 + self.zero_yaw_rate_thresh = 0.10 + self.zero_cmd_use_yaw_rate = bool(zero_cmd_use_yaw_rate) + self._command_release_alpha = 0.0 + print( + f"[PolicyRunner] obs_dim={self.policy.expected_obs_dim}, " + f"base_obs_dim={self.BASE_OBS_DIM}, history=1, " + f"action_dim={self.policy.expected_action_dim}, " + f"backend={getattr(self.policy, 'backend', 'unknown')}, " + f"clip_obs={self.clip_obs:.1f}, " + f"zero_cmd_suppression={self.enable_zero_cmd_suppression}, " + f"hold_zero_command_pose={self.hold_zero_command_pose}" + ) + + def reset(self, prime_obs: np.ndarray | None = None) -> None: + self.last_actions = np.zeros(16, dtype=np.float32) + self._command_release_alpha = 0.0 + + def _is_zero_command(self, command: np.ndarray, base_ang_vel: np.ndarray) -> bool: + cmd_is_zero = ( + np.linalg.norm(command[:2]) < self.zero_cmd_lin_thresh + and abs(command[2]) < self.zero_cmd_yaw_thresh + ) + if not self.zero_cmd_use_yaw_rate: + return cmd_is_zero + return cmd_is_zero and abs(base_ang_vel[2]) < self.zero_yaw_rate_thresh + + def command_activation_metrics(self, command: np.ndarray) -> tuple[float, float]: + command = np.asarray(command, dtype=np.float32) + planar = float(np.linalg.norm(command[:2])) + yaw = float(abs(command[2])) + return planar, yaw + + def is_command_active(self, command: np.ndarray) -> bool: + planar, yaw = self.command_activation_metrics(command) + return planar >= self.zero_cmd_lin_thresh or yaw >= self.zero_cmd_yaw_thresh + + def step(self, obs: np.ndarray, dt: float = 0.02) -> tuple[np.ndarray, np.ndarray]: + obs = np.asarray(obs, dtype=np.float32) + expected_obs_dim = int(self.policy.expected_obs_dim) + if obs.shape[0] != expected_obs_dim: + raise ValueError( + f"Observation dim mismatch: got {obs.shape[0]}, expected {expected_obs_dim}." + ) + if self.clip_obs > 0.0: + obs = np.clip(obs, -self.clip_obs, self.clip_obs).astype(np.float32, copy=False) + + obs_tensor = torch.tensor(obs, dtype=torch.float32, device=self.device).unsqueeze(0) + with torch.no_grad(): + raw_actions = self.policy(obs_tensor).squeeze(0).cpu().numpy() + + raw_actions = np.clip(raw_actions, -10.0, 10.0).astype(np.float32) + command = obs[6:9] + base_ang_vel = obs[0:3] / 0.25 + zero_command = self._is_zero_command(command, base_ang_vel) + if zero_command: + self._command_release_alpha = 0.0 + if self.hold_zero_command_pose: + raw_actions[:] = 0.0 + elif self.enable_zero_cmd_suppression: + raw_actions[12:16] = 0.0 + raw_actions[:12] *= 0.5 + else: + self._command_release_alpha = min(1.0, self._command_release_alpha + dt / self.command_release_s) + raw_actions *= self._command_release_alpha + + self.last_actions = raw_actions.copy() + scaled_actions = raw_actions * self.action_scale + return scaled_actions, raw_actions diff --git a/05_software/real/sim2real_v2/requirements-orin.txt b/05_software/real/sim2real_v2/requirements-orin.txt new file mode 100644 index 0000000..20b988b --- /dev/null +++ b/05_software/real/sim2real_v2/requirements-orin.txt @@ -0,0 +1,8 @@ +numpy +PyYAML +onnxruntime +pyserial + +# Optional: +# torch # only needed to export/check .pt policies +# pynput # only needed for CLI keyboard control diff --git a/05_software/real/sim2real_v2/safety/__init__.py b/05_software/real/sim2real_v2/safety/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real_v2/safety/runtime_guard.py b/05_software/real/sim2real_v2/safety/runtime_guard.py new file mode 100644 index 0000000..5ffecd1 --- /dev/null +++ b/05_software/real/sim2real_v2/safety/runtime_guard.py @@ -0,0 +1,78 @@ +"""通用运行期守护：每个控制周期调用一次，无副作用，只做检查。 + +设计原则： +- 守护函数本身不下发动作、不打印（除非 verbose），只返回判定 +- 调用方决定收到 GuardStop 时怎么办（damping_brake 或 raise） +- 起立期 / 等待期 / 主循环都共用同一组检查 +""" +from dataclasses import dataclass +from enum import IntEnum +from typing import Optional + +import numpy as np + + +class GuardLevel(IntEnum): + OK = 0 + WARN = 1 # 仅记录，不停 + STOP = 2 # 主调方应立刻 damping_brake + 退出当前阶段 + + +@dataclass +class GuardDecision: + level: GuardLevel + reason: str # 触发时人类可读说明，OK 时为空 + + +class RuntimeGuard: + """启动/起立/主循环共用的安全守护。 + + 不监控目标位置范围（那是 SafetyMonitor 的职责）。这里只关心 + 机身整体状态：是否倾倒、是否翻滚、是否检测到 NaN、用户是否按急停。 + """ + + def __init__(self, + max_ang_vel: float = 12.0, + max_tilt_z: float = -0.30, + imu_age_warn_ms: float = 60.0, + imu_age_stop_ms: float = 200.0): + self.max_ang_vel = max_ang_vel + self.max_tilt_z = max_tilt_z + self.imu_age_warn_ms = imu_age_warn_ms + self.imu_age_stop_ms = imu_age_stop_ms + + def check(self, + imu_gyro: np.ndarray, + projected_gravity: np.ndarray, + imu_age_ms: float, + estop_triggered: bool, + extra_nan_arrays: tuple = ()) -> GuardDecision: + # 1) 用户急停 + if estop_triggered: + return GuardDecision(GuardLevel.STOP, "user E-stop") + + # 2) NaN 检查（任意输入数组中出现 NaN） + for arr in (imu_gyro, projected_gravity, *extra_nan_arrays): + if arr is None: + continue + if np.any(np.isnan(arr)) or np.any(np.isinf(arr)): + return GuardDecision(GuardLevel.STOP, "NaN/Inf detected in observation/action") + + # 3) IMU 数据陈旧 + if imu_age_ms > self.imu_age_stop_ms: + return GuardDecision(GuardLevel.STOP, f"IMU stale {imu_age_ms:.0f}ms") + warned_imu = imu_age_ms > self.imu_age_warn_ms + + # 4) 倾倒 + if projected_gravity[2] > self.max_tilt_z: + return GuardDecision(GuardLevel.STOP, + f"tilt: g_z={projected_gravity[2]:.3f}") + + # 5) 角速度爆表 + ang_norm = float(np.linalg.norm(imu_gyro)) + if ang_norm > self.max_ang_vel: + return GuardDecision(GuardLevel.STOP, f"ang_vel overflow: |w|={ang_norm:.2f}") + + if warned_imu: + return GuardDecision(GuardLevel.WARN, f"IMU age {imu_age_ms:.0f}ms") + return GuardDecision(GuardLevel.OK, "") diff --git a/05_software/real/sim2real_v2/safety/safety_monitor.py b/05_software/real/sim2real_v2/safety/safety_monitor.py new file mode 100644 index 0000000..fc7832e --- /dev/null +++ b/05_software/real/sim2real_v2/safety/safety_monitor.py @@ -0,0 +1,116 @@ +"""三级安全监控（对应方法论 97.11）。 + +Level 0: 正常 +Level 1: 限幅（位置/速度异常）— 截断目标位置幅值，记录连续触发次数 +Level 2: 刹车（连续限幅 N 次 / IMU 角速度过大 / 倾倒）— 卸载刚度只留阻尼 +Level 3: 急停（用户触发）— 让上层断电 + +设计原则：监控只判定，不直接关电机；返回 SafetyDecision 由上层决策。 +""" +from dataclasses import dataclass +from enum import IntEnum +from typing import Any, Optional + +import numpy as np + + +class SafetyLevel(IntEnum): + NORMAL = 0 + CLIP = 1 + BRAKE = 2 + ESTOP = 3 + + +@dataclass +class SafetyDecision: + level: SafetyLevel + message: str + clipped_target: Optional[np.ndarray] + details: Optional[dict[str, Any]] = None + + +class SafetyMonitor: + """安全监控（按 50Hz 控制频率调用）。 + + Args: + max_target_offset: 单关节相对默认位姿的最大偏离 (rad) + max_ang_vel: IMU 角速度模 (rad/s) + max_tilt_rad: 机身重力 z 轴投影低于该值认为已严重倾倒 + clip_to_brake: 连续 clip 多少帧升级为刹车 + """ + + def __init__(self, + max_target_offset: float = 0.6, + max_ang_vel: float = 10.0, + max_tilt_z: float = -0.3, + clip_to_brake: int = 0, + hard_target_offset: float = 1.2): + self.max_target_offset = max_target_offset + self.max_ang_vel = max_ang_vel + self.max_tilt_z = max_tilt_z # projected_gravity z 应当 ~ -1，明显小于 -0.3 视作倾倒 + self.clip_to_brake = clip_to_brake + self.hard_target_offset = hard_target_offset + self.consecutive_clips = 0 + + def check(self, + target_pose: np.ndarray, + default_pose: np.ndarray, + imu_gyro: np.ndarray, + projected_gravity: np.ndarray, + estop_triggered: bool) -> SafetyDecision: + if estop_triggered: + return SafetyDecision(SafetyLevel.ESTOP, "user E-stop", None, None) + + # 倾倒（projected_gravity[2] 应在 -1 附近，越接近 0 越倾斜） + if projected_gravity[2] > self.max_tilt_z: + return SafetyDecision( + SafetyLevel.BRAKE, + f"tilt detected: g_z={projected_gravity[2]:.3f}", + None, + {"g_z": float(projected_gravity[2])}, + ) + + # 角速度爆表（猛烈翻滚） + if np.linalg.norm(imu_gyro) > self.max_ang_vel: + return SafetyDecision( + SafetyLevel.BRAKE, + f"angular velocity overflow: |w|={np.linalg.norm(imu_gyro):.2f}", + None, + {"ang_vel_norm": float(np.linalg.norm(imu_gyro))}, + ) + + # 目标位置偏离过大 → 截断到允许范围 + offset_leg = target_pose[:12] - default_pose[:12] + clipped_offset = np.clip(offset_leg, -self.max_target_offset, self.max_target_offset) + if not np.allclose(offset_leg, clipped_offset): + self.consecutive_clips += 1 + clipped = target_pose.copy() + clipped[:12] = default_pose[:12] + clipped_offset + exceeded = np.where(np.abs(offset_leg) > self.max_target_offset)[0].tolist() + max_offset = float(np.max(np.abs(offset_leg))) + details = { + "joint_indices": exceeded, + "max_leg_offset": max_offset, + "consecutive_clips": int(self.consecutive_clips), + } + if self.hard_target_offset > 0.0 and max_offset > self.hard_target_offset: + return SafetyDecision( + SafetyLevel.BRAKE, + f"target leg offset exceeds hard limit: {max_offset:.3f}", + clipped, + details, + ) + if self.clip_to_brake > 0 and self.consecutive_clips >= self.clip_to_brake: + return SafetyDecision( + SafetyLevel.BRAKE, + f"clipped {self.consecutive_clips} frames in a row", + clipped, + details, + ) + return SafetyDecision(SafetyLevel.CLIP, "target leg offset out of range", clipped, details) + + self.consecutive_clips = 0 + return SafetyDecision(SafetyLevel.NORMAL, "", None, None) + + def reset(self): + self.consecutive_clips = 0 diff --git a/05_software/real/sim2real_v2/startup/__init__.py b/05_software/real/sim2real_v2/startup/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real_v2/startup/pose_initializer.py b/05_software/real/sim2real_v2/startup/pose_initializer.py new file mode 100644 index 0000000..7bec9c7 --- /dev/null +++ b/05_software/real/sim2real_v2/startup/pose_initializer.py @@ -0,0 +1,351 @@ +"""起立姿态初始化器（实测起点版本）。 + +设计： + - 不再假设机器人的物理起始姿态（不再有 CRAWL_POSE / GROUND_POSE 起点） + - enable 后从 io.read_measured_pose() 读 16 关节实测，直接作为插值起点 + - 余弦插值到 STAND_POSE，transition_time 根据最大偏差自适应 + - 全程 RuntimeGuard 守护（空格急停/倾倒/翻滚/NaN/IMU 陈旧） + - 50Hz 写 LogBundle CSV（phase 字段标识阶段） + +Phase 流程： + STARTUP_SOFT_HOLD — 软起步保持实测姿态，kp 从 0.125 渐升到 1.0 + STARTUP_TRANSITION — 实测起点 → STAND 余弦插值 + STARTUP_HOLD_AFTER — 站稳后保持 1 秒 +""" +import time +from typing import Optional + +import numpy as np + +from safety.runtime_guard import GuardLevel, RuntimeGuard +from tools.logger import LogBundle +from tools.math_utils import get_gravity_orientation + + +# 仅作为目标姿态使用（训练侧 default_dof_pos） +STAND_POSE = np.array([ + 0.0, 0.9, -1.8, + 0.0, 0.9, -1.8, + 0.0, 0.9, -1.8, + 0.0, 0.9, -1.8, + 0.0, 0.0, 0.0, 0.0, +], dtype=np.float32) + + +def _periodic_leg_delta(src: np.ndarray, dst: np.ndarray) -> np.ndarray: + """Return shortest signed leg-joint delta from src to dst.""" + delta = np.asarray(dst[:12], dtype=np.float32) - np.asarray(src[:12], dtype=np.float32) + return ((delta + np.pi) % (2.0 * np.pi) - np.pi).astype(np.float32) + + +class PoseInitFailed(RuntimeError): + """起立流程触发安全停止。main.py 捕获后立即 damping_brake。""" + + +class PoseInitializer: + def __init__(self, real_io, control_dt: float = 0.02, + transition_time_min: float = 2.0, + transition_time_max: float = 6.0, + transition_seconds_per_rad: float = 1.5, + hold_time: float = 1.0, + settle_pos_threshold: float = 0.12, + settle_vel_threshold: float = 0.6, + timeout_extra: float = 3.0, + imu_fresh_wait_s: float = 1.0, + progress_log_interval: float = 0.5, + ramp_kp_time: float = 1.0, + soft_hold_duration: float = 1.0, + max_dev_warn: float = 1.5, + max_dev_abort: float = 3.0): + """ + Args: + transition_time_min/max/_per_rad: 自适应公式 + t = clip(min, max, max_dev * seconds_per_rad) + timeout_extra: 起立超时 = transition_time + timeout_extra + soft_hold_duration: 起立前先在实测姿态保持几秒，期间 kp ramp-up + max_dev_warn: 最大偏差超过此值打警告（仅日志） + max_dev_abort: 最大偏差超过此值直接 PoseInitFailed（拒绝起立） + """ + self.io = real_io + self.control_dt = control_dt + self.transition_time_min = transition_time_min + self.transition_time_max = transition_time_max + self.transition_seconds_per_rad = transition_seconds_per_rad + self.hold_time = hold_time + self.settle_pos_threshold = settle_pos_threshold + self.settle_vel_threshold = settle_vel_threshold + self.timeout_extra = timeout_extra + self.imu_fresh_wait_s = max(float(imu_fresh_wait_s), 0.0) + self.progress_log_interval = progress_log_interval + self.ramp_kp_time = ramp_kp_time + self.soft_hold_duration = soft_hold_duration + self.max_dev_warn = max_dev_warn + self.max_dev_abort = max_dev_abort + + self.logger: Optional[LogBundle] = None + self.guard: Optional[RuntimeGuard] = None + self.keyboard = None + + def attach(self, logger: LogBundle, guard: RuntimeGuard, keyboard): + self.logger = logger + self.guard = guard + self.keyboard = keyboard + + # ---- 通用每周期工作 ---- + def _tick(self, phase: str, sim_target: np.ndarray, kp_scale: float, next_exec: float): + """读状态 → guard 检查 → 写日志 → 锁帧。返回 (state_dict, next_exec)。 + 若 guard.STOP，立即抛 PoseInitFailed。""" + loop_t0 = time.perf_counter() + + state = self.io.read_state() + proj_g = get_gravity_orientation(state["quat_wxyz"]) + + guard_dec = None + if self.guard is not None: + estop = bool(self.keyboard and self.keyboard.is_estop_triggered()) + guard_dec = self.guard.check( + imu_gyro=state["imu_gyro"], + projected_gravity=proj_g, + imu_age_ms=float(state["imu_age_ms"]), + estop_triggered=estop, + extra_nan_arrays=(sim_target, state["joint_pos"], state["joint_vel"]), + ) + + if self.logger is not None: + motor_diag = state.get("motor_stale", {}) + self.logger.state( + phase=phase, + joint_pos=state["joint_pos"], + joint_vel=state["joint_vel"], + joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)), + target_pose=sim_target, + raw_action=None, + gyro=state["imu_gyro"], + accel=state["imu_accel"], + quat=state["quat_wxyz"], + proj_gravity=proj_g, + command=np.zeros(3, dtype=np.float32), + imu_age_ms=float(state["imu_age_ms"]), + loop_dt_ms=(time.perf_counter() - loop_t0) * 1000.0, + safety_level=0, + guard_level=int(guard_dec.level) if guard_dec else 0, + holdover=int(motor_diag.get("holdover_this_frame", 0)), + stale_max=int(motor_diag.get("stale_max", 0)), + fresh_count=int(motor_diag.get("fresh_count", 16)), + kp_scale=kp_scale, + nan_flag=int(np.any(np.isnan(state["joint_pos"]))), + kp_leg_cmd=float(getattr(self.io, "hold_kp_leg", self.io.kp_leg) * kp_scale), + kd_leg_cmd=float(getattr(self.io, "hold_kd_leg", self.io.kd_leg)), + kd_wheel_cmd=float(self.io.kd_wheel), + target_source="startup_hold", + guard_reason=guard_dec.reason if guard_dec else "", + ) + + if guard_dec is not None and guard_dec.level == GuardLevel.STOP: + if self.logger: + self.logger.event("GUARD_STOP", phase=phase, reason=guard_dec.reason) + raise PoseInitFailed(f"[{phase}] {guard_dec.reason}") + + next_exec += self.control_dt + slack = next_exec - time.perf_counter() + if slack > 0: + coarse = slack - 0.002 + if coarse > 0: + time.sleep(coarse) + micro_slack = next_exec - time.perf_counter() + if micro_slack > 0: + time.sleep(min(micro_slack, 0.001)) + else: + next_exec = time.perf_counter() + return state, next_exec + + # ---- 主入口：从实测姿态起立到 STAND ---- + def transition_to_stand_from_current(self, + target_pose: Optional[np.ndarray] = None + ) -> np.ndarray: + """完整起立流程： + 1. 读实测起点 + 2. 偏差检查（warn / abort） + 3. SOFT_HOLD：保持实测姿态 + kp ramp-up + 4. TRANSITION：余弦插值到 target，transition_time 自适应 + 5. HOLD_AFTER：保持 1 秒 + 返回最终 target_pose（供主循环使用）。 + """ + if target_pose is None: + target_pose = STAND_POSE.copy() + target_pose = target_pose.astype(np.float32).copy() + target_pose[12:] = 0.0 + + # === 1. 读实测起点（要求电机反馈完整）=== + ok, missing = self.io.wait_feedback_ready(max_attempts=20, poll_interval=0.05) + if not ok: + msg = f"feedback incomplete: {len(missing)} motors no response: {missing[:4]}" + if self.logger: + self.logger.event("STARTUP_NO_FEEDBACK", + missing=[m[2] for m in missing]) + raise PoseInitFailed(msg) + + last_imu_age = -1.0 + imu_deadline = time.perf_counter() + self.imu_fresh_wait_s + while self.imu_fresh_wait_s > 0.0 and time.perf_counter() < imu_deadline: + state = self.io.read_state() + last_imu_age = float(state.get("imu_age_ms", 1e9)) + if last_imu_age <= 60.0: + break + time.sleep(self.control_dt) + else: + if self.imu_fresh_wait_s > 0.0 and self.logger: + self.logger.event("STARTUP_IMU_STALE_WARN", imu_age_ms=last_imu_age) + + start_pose = self.io.read_measured_pose().astype(np.float32).copy() + start_pose[12:] = 0.0 # 轮子起点固定为 0 速度 + + # === 2. 偏差检查 === + startup_delta = _periodic_leg_delta(start_pose, target_pose) + diff = np.abs(startup_delta) + max_dev = float(np.max(diff)) + max_dev_joint = int(np.argmax(diff)) + transition_time = float(np.clip( + max_dev * self.transition_seconds_per_rad, + self.transition_time_min, self.transition_time_max + )) + timeout = transition_time + self.timeout_extra + + if self.logger: + self.logger.event( + "STARTUP_PLAN", + start_pose_leg=start_pose[:12].tolist(), + target_pose_leg=target_pose[:12].tolist(), + max_dev=max_dev, + max_dev_joint_idx=max_dev_joint, + transition_time=transition_time, + timeout=timeout, + ) + print(f"[PoseInit] 实测起点最大偏差 {max_dev:.3f} rad (关节 idx={max_dev_joint}); " + f"transition_time={transition_time:.2f}s") + + if max_dev > self.max_dev_abort: + raise PoseInitFailed( + f"实测起点偏差过大 ({max_dev:.2f} rad > abort 阈值 " + f"{self.max_dev_abort})；请检查电机是否在合理姿势" + ) + if max_dev > self.max_dev_warn: + print(f"[PoseInit] WARNING 偏差 {max_dev:.2f} rad > {self.max_dev_warn}; " + f"起立可能比较剧烈") + if self.logger: + self.logger.event("STARTUP_LARGE_DEV", max_dev=max_dev) + + # === 3. SOFT_HOLD：实测姿态 + kp ramp-up === + if self.logger: + self.logger.event("STARTUP_SOFT_HOLD_BEGIN", + duration=self.soft_hold_duration, + ramp_kp_time=self.ramp_kp_time, + ramp_kp_min=0.125) + n = max(1, int(self.soft_hold_duration / max(self.control_dt, 1e-3))) + next_exec = time.perf_counter() + t0 = next_exec + ramp_min = 0.125 + for i in range(n): + elapsed = time.perf_counter() - t0 + if elapsed < self.ramp_kp_time: + kp_scale = ramp_min + (1.0 - ramp_min) * (elapsed / self.ramp_kp_time) + else: + kp_scale = 1.0 + self.io.hold_pose(start_pose, kp_scale=kp_scale) + _s, next_exec = self._tick("STARTUP_SOFT_HOLD", start_pose, kp_scale, next_exec) + if self.logger: + self.logger.event("STARTUP_SOFT_HOLD_END") + + # === 4. TRANSITION：余弦插值 === + if self.logger: + self.logger.event("STARTUP_TRANSITION_BEGIN", + transition_time=transition_time, timeout=timeout) + print(f"[PoseInit] 起立: transition={transition_time:.2f}s, " + f"hold={self.hold_time}s, timeout={timeout:.2f}s") + + t0 = time.perf_counter() + last_log = t0 + reached = False + hold_start: Optional[float] = None + next_exec = t0 + + while True: + now = time.perf_counter() + elapsed = now - t0 + phase = min(1.0, elapsed / max(transition_time, 1e-3)) + + if elapsed > timeout: + if self.logger: + self.logger.event("STARTUP_TIMEOUT", elapsed=elapsed) + raise PoseInitFailed( + f"transition timeout after {elapsed:.2f}s, target not reached" + ) + + blend = 0.5 - 0.5 * np.cos(np.pi * phase) + blended = start_pose.astype(np.float32).copy() + blended[:12] = start_pose[:12] + blend * startup_delta + blended[12:] = 0.0 + self.io.hold_pose(blended, kp_scale=1.0) + state, next_exec = self._tick("STARTUP_TRANSITION", blended, 1.0, next_exec) + + joint_pos = state["joint_pos"] + joint_vel = state["joint_vel"] + pos_err = float(np.max(np.abs(_periodic_leg_delta(joint_pos, target_pose)))) + vel_err = float(np.max(np.abs(joint_vel[:12]))) + + if now - last_log >= self.progress_log_interval: + msg = (f"[PoseInit] phase={phase*100:5.1f}% | " + f"max_pos_err={pos_err:.3f} | max_vel={vel_err:.3f}") + print(msg) + if self.logger: + self.logger.event("STARTUP_PROGRESS", + phase=phase, pos_err=pos_err, vel_err=vel_err) + last_log = now + + if (phase >= 1.0 + and pos_err <= self.settle_pos_threshold + and vel_err <= self.settle_vel_threshold): + if not reached: + reached = True + hold_start = now + if self.logger: + self.logger.event("STARTUP_REACHED", + pos_err=pos_err, vel_err=vel_err) + print(f"[PoseInit] 已到位，保持 {self.hold_time:.2f}s") + elif hold_start is not None and now - hold_start >= self.hold_time: + break + elif phase >= 1.0: + reached = False + hold_start = None + + # === 5. HOLD_AFTER === + if self.logger: + self.logger.event("STARTUP_HOLD_AFTER_BEGIN", duration=self.hold_time) + n_hold = max(1, int(self.hold_time / max(self.control_dt, 1e-3))) + next_exec = time.perf_counter() + for _ in range(n_hold): + self.io.hold_pose(target_pose, kp_scale=1.0) + _s, next_exec = self._tick("STARTUP_HOLD_AFTER", target_pose, 1.0, next_exec) + + if self.logger: + self.logger.event("STARTUP_TRANSITION_END") + print("[PoseInit] 默认站姿初始化完成") + return target_pose + + # ---- 等用户回车（外部调用，期间持续保持） ---- + def hold_until_user_confirm(self, target_pose: np.ndarray, evt) -> bool: + """阻塞循环到 evt.is_set()，期间持续 PD 保持站姿、跑 guard、写日志。 + 返回 True 正常确认，False 因 guard.STOP 中止。""" + if self.logger: + self.logger.event("WAIT_USER_BEGIN") + next_exec = time.perf_counter() + while not evt.is_set(): + self.io.hold_pose(target_pose, kp_scale=1.0) + try: + _s, next_exec = self._tick("WAIT_USER", target_pose, 1.0, next_exec) + except PoseInitFailed as e: + print(f"[PoseInit] WAIT_USER 期间触发停止: {e}") + return False + if self.logger: + self.logger.event("WAIT_USER_END") + return True diff --git a/05_software/real/sim2real_v2/startup/stand_balance.py b/05_software/real/sim2real_v2/startup/stand_balance.py new file mode 100644 index 0000000..f956781 --- /dev/null +++ b/05_software/real/sim2real_v2/startup/stand_balance.py @@ -0,0 +1,141 @@ +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any, Dict + +import numpy as np + + +@dataclass +class StandBalanceDebug: + roll: float + pitch: float + roll_rate: float + pitch_rate: float + hip_base: float + knee_base: float + roll_corr: float + pitch_corr: float + pitch_compensation_enabled: bool + target: list[float] + stable: bool + + +class StandBalanceController: + def __init__(self, cfg: Dict[str, Any], control_dt: float): + self.enabled = bool(cfg.get("enabled", True)) + self.control_dt = float(control_dt) + self.height = float(cfg.get("height", 0.33)) + self.kp_roll = float(cfg.get("kp_roll", 0.85)) + self.pitch_compensation_enabled = bool(cfg.get("pitch_compensation_enabled", True)) + self.kp_pitch = float(cfg.get("kp_pitch", 0.70)) + self.kd_roll_rate = float(cfg.get("kd_roll_rate", 0.03)) + self.kd_pitch_rate = float(cfg.get("kd_pitch_rate", 0.025)) + self.pitch_deadband = float(np.radians(cfg.get("pitch_deadband_deg", 0.0))) + self.pitch_corr_clip = float(cfg.get("pitch_corr_clip", 0.12)) + self.pitch_corr_filter_alpha = float(np.clip(cfg.get("pitch_corr_filter_alpha", 1.0), 0.0, 1.0)) + self.pitch_front_sign = float(cfg.get("pitch_front_sign", -1.0)) + self.lateral_lean_gain = float(cfg.get("lateral_lean_gain", 0.0)) + self.hip_abduction_clip = float(cfg.get("hip_abduction_clip", 0.45)) + self.hip_pitch_clip = tuple(cfg.get("hip_pitch_clip", [-1.0, 2.5])) + self.knee_clip = tuple(cfg.get("knee_clip", [-2.6, -0.3])) + self.stable_roll_deg = float(cfg.get("stable_roll_deg", 6.0)) + self.stable_pitch_deg = float(cfg.get("stable_pitch_deg", 8.0)) + self.stable_gyro_deg_s = float(cfg.get("stable_gyro_deg_s", 45.0)) + self.enter_hold_s = float(cfg.get("enter_hold_s", 1.0)) + + self.profile_h = np.asarray( + cfg.get("profile_h", [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]), + dtype=np.float32, + ) + self.profile_hip = np.asarray( + cfg.get("profile_hip", [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]), + dtype=np.float32, + ) + self.profile_knee = np.asarray( + cfg.get("profile_knee", [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]), + dtype=np.float32, + ) + self._stable_time = 0.0 + self._pitch_corr_filtered = 0.0 + self._last_debug = StandBalanceDebug(0.0, 0.0, 0.0, 0.0, 0.9, -1.8, 0.0, 0.0, False, [], False) + + @property + def last_debug(self) -> StandBalanceDebug: + return self._last_debug + + def reset(self) -> None: + self._stable_time = 0.0 + self._pitch_corr_filtered = 0.0 + + def _estimate_roll_pitch(self, projected_gravity: np.ndarray) -> tuple[float, float]: + gx, gy, gz = [float(v) for v in projected_gravity] + roll = float(np.arctan2(-gy, max(1e-6, -gz))) + pitch = float(np.arctan2(gx, np.sqrt(max(1e-6, gy * gy + gz * gz)))) + return roll, pitch + + def _base_leg_pose(self) -> tuple[float, float]: + h_clamp = float(np.clip(self.height, float(self.profile_h[0]), float(self.profile_h[-1]))) + hip = float(np.interp(h_clamp, self.profile_h, self.profile_hip)) + knee = float(np.interp(h_clamp, self.profile_h, self.profile_knee)) + return hip, knee + + def compute_target(self, state: Dict[str, Any], command: np.ndarray | None = None) -> np.ndarray: + projected_gravity = np.asarray(state["projected_gravity"], dtype=np.float32) + imu_gyro = np.asarray(state["imu_gyro"], dtype=np.float32) + cmd = np.zeros(3, dtype=np.float32) if command is None else np.asarray(command, dtype=np.float32) + + hip_base, knee_base = self._base_leg_pose() + roll, pitch = self._estimate_roll_pitch(projected_gravity) + roll_rate = float(imu_gyro[0]) + pitch_rate = float(imu_gyro[1]) + + roll_corr = -self.kp_roll * roll - self.kd_roll_rate * roll_rate + if self.pitch_compensation_enabled: + pitch_for_ctrl = 0.0 if abs(pitch) < self.pitch_deadband else pitch + pitch_corr_raw = -self.kp_pitch * pitch_for_ctrl - self.kd_pitch_rate * pitch_rate + pitch_corr_raw = float(np.clip(pitch_corr_raw, -self.pitch_corr_clip, self.pitch_corr_clip)) + alpha = self.pitch_corr_filter_alpha + pitch_corr = (1.0 - alpha) * self._pitch_corr_filtered + alpha * pitch_corr_raw + self._pitch_corr_filtered = pitch_corr + else: + pitch_corr = 0.0 + self._pitch_corr_filtered = 0.0 + lateral_lean = self.lateral_lean_gain * float(cmd[1]) + + target = np.zeros(16, dtype=np.float32) + for leg_idx in range(4): + side = 1.0 if leg_idx in (0, 2) else -1.0 + fore_aft = self.pitch_front_sign if leg_idx in (0, 1) else -self.pitch_front_sign + target[leg_idx * 3 + 0] = float( + np.clip(side * roll_corr + lateral_lean, -self.hip_abduction_clip, self.hip_abduction_clip) + ) + target[leg_idx * 3 + 1] = float( + np.clip(hip_base + fore_aft * pitch_corr, self.hip_pitch_clip[0], self.hip_pitch_clip[1]) + ) + target[leg_idx * 3 + 2] = float(np.clip(knee_base, self.knee_clip[0], self.knee_clip[1])) + target[12:] = 0.0 + + stable = ( + abs(np.degrees(roll)) <= self.stable_roll_deg + and abs(np.degrees(pitch)) <= self.stable_pitch_deg + and max(abs(np.degrees(roll_rate)), abs(np.degrees(pitch_rate))) <= self.stable_gyro_deg_s + ) + self._stable_time = self._stable_time + self.control_dt if stable else 0.0 + self._last_debug = StandBalanceDebug( + roll=roll, + pitch=pitch, + roll_rate=roll_rate, + pitch_rate=pitch_rate, + hip_base=hip_base, + knee_base=knee_base, + roll_corr=roll_corr, + pitch_corr=pitch_corr, + pitch_compensation_enabled=self.pitch_compensation_enabled, + target=target.tolist(), + stable=stable, + ) + return target + + def is_stable(self) -> bool: + return self._stable_time >= self.enter_hold_s diff --git a/05_software/real/sim2real_v2/tools/__init__.py b/05_software/real/sim2real_v2/tools/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real_v2/tools/alignment_check.py b/05_software/real/sim2real_v2/tools/alignment_check.py new file mode 100644 index 0000000..626dfa9 --- /dev/null +++ b/05_software/real/sim2real_v2/tools/alignment_check.py @@ -0,0 +1,244 @@ +"""Offline deployment alignment check for the current 53-D rough policy.""" + +import argparse +from pathlib import Path +import sys + +import numpy as np +import torch +import yaml + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +from interface.motor_mapping import MotorMapping # noqa: E402 +from policy.policy_runner import PolicyRunner # noqa: E402 + + +def _load_manifest(manifest_path: Path) -> dict: + with open(manifest_path, "r", encoding="utf-8") as f: + return yaml.safe_load(f) + + +def _load_config() -> dict: + config_path = Path(__file__).resolve().parents[1] / "config.yaml" + with open(config_path, "r", encoding="utf-8") as f: + return yaml.safe_load(f) + + +def check(policy_path: Path, manifest_path: Path | None = None) -> int: + issues: list[tuple[str, str]] = [] + manifest = _load_manifest(manifest_path) if manifest_path is not None else None + config = _load_config() + + expected = ( + ("fl", "hip_abduction"), ("fl", "hip_pitch"), ("fl", "knee"), + ("fr", "hip_abduction"), ("fr", "hip_pitch"), ("fr", "knee"), + ("rl", "hip_abduction"), ("rl", "hip_pitch"), ("rl", "knee"), + ("rr", "hip_abduction"), ("rr", "hip_pitch"), ("rr", "knee"), + ("fl", "wheel"), ("fr", "wheel"), ("rl", "wheel"), ("rr", "wheel"), + ) + if MotorMapping.SIM_JOINT_ORDER != expected: + issues.append(("joint_order", "MotorMapping.SIM_JOINT_ORDER mismatch")) + else: + print("[Check] joint order: PASS") + expected_joint_names = [f"{leg}_{joint}" for leg, joint in expected] + + manifest_enable_zero_cmd = True + if manifest is not None: + manifest_enable_zero_cmd = bool( + manifest.get("model", {}).get("enable_zero_cmd_suppression", True) + ) + + runner = PolicyRunner( + policy_path, + device=torch.device("cpu"), + enable_zero_cmd_suppression=manifest_enable_zero_cmd, + clip_obs=float(config.get("policy", {}).get("clip_obs", 100.0)), + ) + obs_mean = np.asarray(runner.policy.obs_mean.detach().cpu().numpy(), dtype=np.float32) + obs_std = np.asarray(runner.policy.obs_std.detach().cpu().numpy(), dtype=np.float32) + if np.allclose(obs_mean, 0.0) and np.allclose(obs_std, 1.0): + print("[Check] obs normalizer: PASS (identity)") + else: + print( + f"[Check] obs normalizer: PASS " + f"(mean range=[{obs_mean.min():.3f},{obs_mean.max():.3f}], " + f"std range=[{obs_std.min():.3f},{obs_std.max():.3f}])" + ) + if (obs_std < 1e-6).any(): + issues.append( + ( + "normalizer_zero_std", + f"obs_std has near-zero entries: {np.where(obs_std < 1e-6)[0].tolist()}", + ) + ) + + raw_zero = np.zeros(runner.BASE_OBS_DIM, dtype=np.float32) + runner.reset(prime_obs=raw_zero) + _, raw = runner.step(raw_zero) + if np.max(np.abs(raw)) > 5.0: + issues.append( + ( + "output_range", + f"raw action too large under zero obs: {np.max(np.abs(raw)):.3f}", + ) + ) + else: + print(f"[Check] zero-obs output range: PASS (max|raw|={np.max(np.abs(raw)):.3f})") + + expected_default = np.array([0.0, 0.9, -1.8] * 4 + [0.0] * 4, dtype=np.float32) + if not np.allclose(runner.default_dof_pos, expected_default): + issues.append(("default_pose_mismatch", f"default_dof_pos mismatch: {runner.default_dof_pos}")) + else: + print("[Check] default_dof_pos: PASS") + + if runner.BASE_OBS_DIM != 53: + issues.append(("obs_dim", f"base obs dim {runner.BASE_OBS_DIM} != 53")) + else: + print("[Check] actor obs dim: PASS (53)") + + if manifest is not None: + declared_model = manifest.get("model", {}) + declared_action = manifest.get("action", {}) + declared_safety = manifest.get("safety", {}) + declared_control = manifest.get("control", {}) + + if int(declared_model.get("obs_dim", -1)) != runner.policy.expected_obs_dim: + issues.append( + ( + "manifest_obs_dim", + f"manifest obs_dim {declared_model.get('obs_dim')} != policy {runner.policy.expected_obs_dim}", + ) + ) + else: + print("[Check] manifest obs_dim: PASS") + + if int(declared_model.get("action_dim", -1)) != runner.policy.expected_action_dim: + issues.append( + ( + "manifest_action_dim", + f"manifest action_dim {declared_model.get('action_dim')} != policy {runner.policy.expected_action_dim}", + ) + ) + else: + print("[Check] manifest action_dim: PASS") + + declared_clip_obs = float(declared_model.get("clip_obs", -1.0)) + config_clip_obs = float(config.get("policy", {}).get("clip_obs", -2.0)) + if declared_clip_obs != runner.clip_obs or config_clip_obs != runner.clip_obs: + issues.append( + ( + "clip_obs", + f"clip_obs mismatch: manifest={declared_clip_obs}, config={config_clip_obs}, runner={runner.clip_obs}", + ) + ) + else: + print("[Check] clip_obs: PASS") + + declared_default = np.asarray(declared_action.get("default_dof_pos", []), dtype=np.float32) + if declared_default.shape != runner.default_dof_pos.shape or not np.allclose( + declared_default, runner.default_dof_pos + ): + issues.append(("manifest_default_pose", "manifest default_dof_pos mismatch")) + else: + print("[Check] manifest default_dof_pos: PASS") + + declared_scale = np.asarray(declared_action.get("scale", []), dtype=np.float32) + if declared_scale.shape != runner.action_scale.shape or not np.allclose( + declared_scale, runner.action_scale + ): + issues.append(("manifest_action_scale", "manifest action scale mismatch")) + else: + print("[Check] manifest action scale: PASS") + + config_scale = np.asarray(config.get("policy", {}).get("action_scale", []), dtype=np.float32) + if config_scale.shape != runner.action_scale.shape or not np.allclose(config_scale, runner.action_scale): + issues.append(("config_action_scale", "config policy.action_scale mismatch")) + else: + print("[Check] config action scale: PASS") + + declared_joint_order = list(declared_action.get("joint_order", [])) + if declared_joint_order != expected_joint_names: + issues.append(("manifest_joint_order", "manifest action.joint_order mismatch")) + else: + print("[Check] manifest joint order: PASS") + + declared_wheel_indices = list(declared_action.get("wheel_indices", [])) + if declared_wheel_indices != [12, 13, 14, 15]: + issues.append(("manifest_wheel_indices", "manifest wheel_indices must be [12,13,14,15]")) + else: + print("[Check] manifest wheel indices: PASS") + + if float(declared_safety.get("zero_cmd_lin_thresh", -1.0)) != runner.zero_cmd_lin_thresh: + issues.append(("manifest_zero_cmd_lin_thresh", "manifest zero_cmd_lin_thresh mismatch")) + if float(declared_safety.get("zero_cmd_yaw_thresh", -1.0)) != runner.zero_cmd_yaw_thresh: + issues.append(("manifest_zero_cmd_yaw_thresh", "manifest zero_cmd_yaw_thresh mismatch")) + if float(declared_safety.get("zero_yaw_rate_thresh", -1.0)) != runner.zero_yaw_rate_thresh: + issues.append(("manifest_zero_yaw_rate_thresh", "manifest zero_yaw_rate_thresh mismatch")) + if bool(declared_model.get("enable_zero_cmd_suppression", True)) != runner.enable_zero_cmd_suppression: + issues.append(("manifest_zero_cmd_switch", "manifest zero-command suppression switch mismatch")) + else: + print("[Check] manifest zero-command suppression: PASS") + + if int(declared_control.get("control_freq_hz", -1)) != 50: + issues.append(("manifest_control_freq", "manifest control_freq_hz must be 50")) + else: + print("[Check] manifest control freq: PASS") + + if int(config.get("control_freq", -1)) != int(declared_control.get("control_freq_hz", -2)): + issues.append(("config_control_freq", "config control_freq != manifest control_freq_hz")) + else: + print("[Check] config control freq: PASS") + + controller_cfg = config.get("controller", {}) or {} + gain_pairs = ( + ("runtime_kp_leg", "kp_leg"), + ("runtime_kd_leg", "kd_leg"), + ("hold_kp_leg", "hold_kp_leg"), + ("hold_kd_leg", "hold_kd_leg"), + ("kd_wheel", "kd_wheel"), + ) + for manifest_key, config_key in gain_pairs: + manifest_value = float(declared_control.get(manifest_key, -9999.0)) + config_value = float(controller_cfg.get(config_key, -9998.0)) + if not np.isclose(manifest_value, config_value): + issues.append( + ( + "control_gains", + f"{manifest_key}/{config_key} mismatch: manifest={manifest_value}, config={config_value}", + ) + ) + if not any(tag == "control_gains" for tag, _ in issues): + print("[Check] control gains: PASS") + + manifest_filter = declared_control.get("command_filter", {}) or {} + config_filter = config.get("command_filter", {}) or {} + for key in ("enabled", "max_vx_acc", "max_vy_acc", "max_yaw_acc"): + if manifest_filter.get(key) != config_filter.get(key): + issues.append(("command_filter", f"command_filter.{key} mismatch")) + if not any(tag == "command_filter" for tag, _ in issues): + print("[Check] command filter config: PASS") + + if issues: + print("\n" + "=" * 60) + print(f"Alignment check failed: {len(issues)} issue(s)") + for tag, msg in issues: + print(f" [{tag}] {msg}") + return 1 + + print("\n" + "=" * 60) + print("All offline alignment checks passed.") + return 0 + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--policy", type=str, required=True, help="Path to policy .onnx or .pt") + parser.add_argument("--manifest", type=str, default=None, help="Optional deployment manifest yaml") + args = parser.parse_args() + manifest = Path(args.manifest) if args.manifest else None + sys.exit(check(Path(args.policy), manifest)) + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real_v2/tools/analyze_log.py b/05_software/real/sim2real_v2/tools/analyze_log.py new file mode 100644 index 0000000..a214bcf --- /dev/null +++ b/05_software/real/sim2real_v2/tools/analyze_log.py @@ -0,0 +1,138 @@ +"""Summarize real-run logs for startup/stand/runtime diagnosis.""" + +from __future__ import annotations + +import argparse +import csv +import json +import math +from pathlib import Path +import statistics + +LEG_JOINTS = ( + "fl_hip_abd", "fl_hip_pitch", "fl_knee", + "fr_hip_abd", "fr_hip_pitch", "fr_knee", + "rl_hip_abd", "rl_hip_pitch", "rl_knee", + "rr_hip_abd", "rr_hip_pitch", "rr_knee", +) + + +def _f(row: dict[str, str], key: str, default: float = 0.0) -> float: + try: + return float(row.get(key, default)) + except Exception: + return default + + +def _stats(values: list[float]) -> str: + if not values: + return "--" + return ( + f"mean={statistics.mean(values):.3f} " + f"std={statistics.pstdev(values):.3f} " + f"min={min(values):.3f} max={max(values):.3f}" + ) + + +def _pitch_deg(row: dict[str, str]) -> float: + gx = _f(row, "pgrav_x") + gy = _f(row, "pgrav_y") + gz = _f(row, "pgrav_z") + return math.degrees(math.atan2(gx, math.sqrt(max(1e-9, gy * gy + gz * gz)))) + + +def summarize(log_dir: Path) -> int: + state_path = log_dir / "state.csv" + events_path = log_dir / "events.jsonl" + if not state_path.exists(): + print(f"[Analyze] missing {state_path}") + return 1 + + if events_path.exists(): + print("[Analyze] key events:") + for line in events_path.read_text(encoding="utf-8", errors="replace").splitlines(): + try: + ev = json.loads(line) + except Exception: + continue + if ev.get("kind") in { + "STARTUP_PLAN", + "STARTUP_REACHED", + "STAND_BALANCE_STABLE", + "RUNTIME_BEGIN", + "POLICY_TARGET_STALE", + "POLICY_TIMEOUT", + "SAFETY_BRAKE", + "GUARD_STOP", + "POSE_INIT_FAILED", + }: + detail = {k: v for k, v in ev.items() if k not in ("t", "t_rel")} + print(f" t={ev.get('t_rel', 0):.2f}s {detail}") + + with state_path.open(newline="", encoding="utf-8") as f: + rows = list(csv.DictReader(f)) + print(f"\n[Analyze] state rows: {len(rows)}") + global_notes: list[str] = [] + for phase in sorted({r.get("phase", "") for r in rows}): + phase_rows = [r for r in rows if r.get("phase") == phase] + if not phase_rows: + continue + pitch = [_pitch_deg(r) for r in phase_rows] + loop = [_f(r, "loop_dt_ms") for r in phase_rows] + imu = [_f(r, "imu_age_ms") for r in phase_rows] + print(f"\n[Phase] {phase} n={len(phase_rows)} t={phase_rows[0].get('t_rel')}..{phase_rows[-1].get('t_rel')}") + print(f" pitch_deg {_stats(pitch)}") + print(f" loop_ms {_stats(loop)}") + print(f" imu_age {_stats(imu)}") + if "stand_pitch_corr" in phase_rows[0]: + stand_pitch = [_f(r, "stand_pitch_deg") for r in phase_rows] + stand_corr = [_f(r, "stand_pitch_corr") for r in phase_rows] + stand_enabled = [_f(r, "stand_pitch_comp_enabled") for r in phase_rows] + print(f" stand_pitch_deg {_stats(stand_pitch)}") + print(f" stand_pitch_corr {_stats(stand_corr)} enabled_mean={statistics.mean(stand_enabled):.3f}") + for joint in ("fl_hip_pitch", "fr_hip_pitch", "rl_hip_pitch", "rr_hip_pitch", "fl_knee", "fr_knee", "rl_knee", "rr_knee"): + pos_key = f"{joint}_pos" + tgt_key = f"{joint}_tgt" + tau_key = f"{joint}_tau" + if pos_key in phase_rows[0] and tgt_key in phase_rows[0]: + err = [_f(r, pos_key) - _f(r, tgt_key) for r in phase_rows] + tau = [_f(r, tau_key) for r in phase_rows] if tau_key in phase_rows[0] else [] + print(f" {joint}_err {_stats(err)} tau {_stats(tau)}") + high_tau = [] + high_err = [] + for joint in LEG_JOINTS: + tau_key = f"{joint}_tau" + pos_key = f"{joint}_pos" + tgt_key = f"{joint}_tgt" + if tau_key in phase_rows[0]: + tau_abs_mean = statistics.mean(abs(_f(r, tau_key)) for r in phase_rows) + if tau_abs_mean > 6.0: + high_tau.append((joint, tau_abs_mean)) + if pos_key in phase_rows[0] and tgt_key in phase_rows[0]: + err_abs_mean = statistics.mean(abs(_f(r, pos_key) - _f(r, tgt_key)) for r in phase_rows) + if err_abs_mean > 0.08: + high_err.append((joint, err_abs_mean)) + if high_tau: + text = ", ".join(f"{name}:{value:.2f}Nm" for name, value in sorted(high_tau, key=lambda x: -x[1])[:4]) + print(f" high_tau_mean {text}") + global_notes.append(f"{phase}: high mean torque -> {text}") + if high_err: + text = ", ".join(f"{name}:{value:.3f}rad" for name, value in sorted(high_err, key=lambda x: -x[1])[:4]) + print(f" high_err_mean {text}") + global_notes.append(f"{phase}: high tracking error -> {text}") + if global_notes: + print("\n[Analyze] notes:") + for note in global_notes: + print(f" - {note}") + return 0 + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("log_dir", type=str) + args = parser.parse_args() + return summarize(Path(args.log_dir)) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/05_software/real/sim2real_v2/tools/calibrate_offsets.py b/05_software/real/sim2real_v2/tools/calibrate_offsets.py new file mode 100644 index 0000000..662132e --- /dev/null +++ b/05_software/real/sim2real_v2/tools/calibrate_offsets.py @@ -0,0 +1,163 @@ +"""零位偏移标定向导。 + +用途：把机器人摆到 sim2sim/训练侧的 stand 默认姿态（人工摆好）， +跑这个脚本，它会读 16 个电机的当前位置，反算每个电机的 ZERO_OFFSET。 + +关键公式（与 motor_mapping.py 一致）： + real = sign * sim + offset +当 sim = stand_default 时： + offset = real - sign * stand_default + +⚠️ 使用前置条件： +1. 已运行过 motor_driver_direction_test 类的脚本，确认每个电机的 sign 是对的； + sign 错的话本工具会算出错误的 offset 看起来很对，但发动作时机器人会反向冲撞 +2. 机器人物理上摆到 stand 姿态：四条腿微弯曲、轮子接地、机身水平 +3. 电机已 enable 并清除告警 + +输出：把打印出来的 ZERO_OFFSET_MAP 字段直接覆盖 motor_mapping.py 中的对应字典。 +""" +import argparse +import sys +import time +from pathlib import Path + +import numpy as np + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +from interface.motor_mapping import MotorMapping # noqa: E402 +from policy.policy_runner import PolicyRunner # noqa: E402 + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--can1-port", default="/dev/can1") + parser.add_argument("--can2-port", default="/dev/can2") + parser.add_argument("--motor-model", default="rs-02") + parser.add_argument("--samples", type=int, default=100, + help="平均采样帧数（去抖动）") + parser.add_argument("--target-pose", default="stand", choices=["stand", "crawl"], + help="标定时机器人摆的物理姿态") + parser.add_argument("--no-enable", action="store_true", + help="不主动 enable 电机（仅读取，适合手动转关节标定）") + args = parser.parse_args() + + # 真机驱动注入（路径优先级与 main.py 一致：vendored/drivers > /home/rc2/...） + sim2real_root = Path(__file__).resolve().parents[1] + for path in (sim2real_root / "vendored", + "/home/rc2/work/rcwork/control", + "/home/rc2/work/rcwork"): + sp = str(path) + if sp not in sys.path and Path(path).exists(): + sys.path.append(sp) + from drivers.motor_driver import RobStrideDriver # type: ignore + + mapper = MotorMapping() + drv1 = RobStrideDriver(args.can1_port, debug=False) + drv2 = RobStrideDriver(args.can2_port, debug=False) + drv1.connect() + drv2.connect() + + for jk in mapper.SIM_JOINT_ORDER: + leg, joint = jk + bus, mid = mapper.CAN_ID_MAP[jk] + name = f"{leg}_{joint}" + (drv1 if bus == 1 else drv2).add_motor(name, mid, args.motor_model) + + if not args.no_enable: + print("[Calib] Enable 电机以读取状态...（已就位则可加 --no-enable 跳过）") + for drv in (drv1, drv2): + for name in drv.motors: + drv.clear_warnings(name) + drv.enable(name) + time.sleep(0.5) + + # 选择标定姿态 + if args.target_pose == "stand": + sim_pose = PolicyRunner.DEFAULT_STAND_POSE.copy() # [0,0.9,-1.8] x4 + zeros + else: + sim_pose = np.array([ + 0.4, 1.65, -2.55, -0.4, 1.65, -2.55, + 0.4, 1.65, -2.55, -0.4, 1.65, -2.55, + 0.0, 0.0, 0.0, 0.0, + ], dtype=np.float32) + + print(f"\n[Calib] 请把机器人物理摆成 {args.target_pose.upper()} 姿态：") + if args.target_pose == "stand": + print(" 四条腿髋外展=0, 髋俯仰=0.9rad(~52°), 膝=-1.8rad(~-103°), 轮接地") + else: + print(" 内收外展 ±0.4rad, 髋俯仰=1.65rad, 膝=-2.55rad（深蹲下趴）") + print(" 轮子可以保持任意角度，offset 强制为 0") + print(" 按回车开始采样...") + try: + input() + except EOFError: + pass + + print(f"\n[Calib] 开始采样 {args.samples} 帧并平均...") + pos_acc = np.zeros(16, dtype=np.float64) + valid = 0 + for i in range(args.samples): + drv1.process_messages() + drv2.process_messages() + real_pos = {} + for drv_idx, drv in enumerate((drv1, drv2)): + bus = drv_idx + 1 + for name, motor in drv.motors.items(): + parts = name.split("_", 1) + if len(parts) != 2: + continue + key = (parts[0], parts[1]) + if key not in mapper.CAN_ID_MAP: + continue + _, mid = mapper.CAN_ID_MAP[key] + real_pos[(bus, mid)] = motor.state.position + if len(real_pos) == 16: + ordered = np.array([real_pos[mapper.CAN_ID_MAP[jk]] + for jk in mapper.SIM_JOINT_ORDER], dtype=np.float64) + pos_acc += ordered + valid += 1 + time.sleep(0.02) + + if valid < args.samples * 0.5: + print(f"[Calib] 警告: 只收到 {valid}/{args.samples} 帧反馈，标定可能不可靠") + real_avg = pos_acc / max(valid, 1) + + # 反算 offset：offset = real - sign * sim + sign = mapper._sign + offsets = real_avg - sign * sim_pose + + # 轮子 offset 强制 0 + for i, jk in enumerate(mapper.SIM_JOINT_ORDER): + if jk[1] == "wheel": + offsets[i] = 0.0 + + # 打印结果（按 motor_mapping.py 的字典格式） + print("\n" + "=" * 64) + print(f"[Calib] 标定完成（{valid} 帧平均）") + print("=" * 64) + print("把以下字典覆盖 sim2real/interface/motor_mapping.py 中的 ZERO_OFFSET_MAP:") + print() + print(" ZERO_OFFSET_MAP = {") + for i, jk in enumerate(mapper.SIM_JOINT_ORDER): + leg, joint = jk + cur = offsets[i] + old = mapper.ZERO_OFFSET_MAP[jk] + delta = cur - old + marker = " *" if abs(delta) > 0.01 else "" + print(f' ("{leg}", "{joint:13s}"): {cur:>+8.4f}, ' + f'# old={old:+.4f} delta={delta:+.4f}{marker}') + print(" }") + print("\n标记 * 的项与现表偏离 > 0.01 rad，请重点核对该关节的 sign 是否正确。\n") + + # Disable + if not args.no_enable: + for drv in (drv1, drv2): + for name in drv.motors: + drv.disable(name) + drv1.disconnect() + drv2.disconnect() + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real_v2/tools/export_onnx.py b/05_software/real/sim2real_v2/tools/export_onnx.py new file mode 100644 index 0000000..fbd2825 --- /dev/null +++ b/05_software/real/sim2real_v2/tools/export_onnx.py @@ -0,0 +1,80 @@ +"""Export the current PyTorch actor checkpoint to ONNX and verify parity.""" + +from __future__ import annotations + +import argparse +from pathlib import Path +import sys + +import numpy as np +import torch + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +from policy.policy_runner import load_policy # noqa: E402 + + +def export_onnx(pt_path: Path, onnx_path: Path, opset: int = 14) -> None: + device = torch.device("cpu") + model = load_policy(pt_path, device) + if getattr(model, "backend", "torch") != "torch": + raise ValueError(f"export source must be a .pt policy, got {pt_path}") + + obs_dim = int(model.expected_obs_dim) + dummy = torch.randn(1, obs_dim, dtype=torch.float32, device=device) + onnx_path.parent.mkdir(parents=True, exist_ok=True) + + torch.onnx.export( + model, + dummy, + str(onnx_path), + export_params=True, + opset_version=opset, + do_constant_folding=True, + input_names=["obs"], + output_names=["action"], + dynamic_axes={"obs": {0: "batch_size"}, "action": {0: "batch_size"}}, + ) + + try: + import onnxruntime as ort + except ImportError: + print("[Export] onnxruntime not installed; export done but parity check skipped.") + return + + opts = ort.SessionOptions() + opts.intra_op_num_threads = 1 + opts.inter_op_num_threads = 1 + opts.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL + session = ort.InferenceSession(str(onnx_path), sess_options=opts, providers=["CPUExecutionProvider"]) + with torch.no_grad(): + torch_out = model(dummy).detach().cpu().numpy() + ort_out = session.run([session.get_outputs()[0].name], {session.get_inputs()[0].name: dummy.cpu().numpy()})[0] + max_diff = float(np.max(np.abs(torch_out - ort_out))) + mean_diff = float(np.mean(np.abs(torch_out - ort_out))) + print(f"[Export] ONNX parity max_diff={max_diff:.8f}, mean_diff={mean_diff:.8f}") + if max_diff > 1e-4: + raise RuntimeError(f"ONNX parity check failed: max_diff={max_diff:.8f}") + + +def main() -> int: + root = Path(__file__).resolve().parents[1] + parser = argparse.ArgumentParser() + parser.add_argument("--pt", default=str(root / "policies" / "model_rough.pt"), help="Source .pt checkpoint") + parser.add_argument("--onnx", default=None, help="Destination .onnx path; default replaces .pt suffix") + parser.add_argument("--opset", type=int, default=14) + args = parser.parse_args() + + pt_path = Path(args.pt) + onnx_path = Path(args.onnx) if args.onnx else pt_path.with_suffix(".onnx") + if not pt_path.exists(): + print(f"[Export] missing source policy: {pt_path}") + return 1 + + export_onnx(pt_path, onnx_path, args.opset) + print(f"[Export] wrote {onnx_path}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/05_software/real/sim2real_v2/tools/logger.py b/05_software/real/sim2real_v2/tools/logger.py new file mode 100644 index 0000000..91cbf69 --- /dev/null +++ b/05_software/real/sim2real_v2/tools/logger.py @@ -0,0 +1,296 @@ +"""Logging helpers for sim2real runs. + +Each session writes: +- `state.csv`: high-rate state stream +- `events.jsonl`: event / milestone stream +""" + +import json +import queue +import threading +import time +from datetime import datetime +from pathlib import Path +from typing import Any, Dict, Optional + +import numpy as np + + +class LogBundle: + """One session directory containing state CSV and event JSONL.""" + + JOINT_LABELS = ( + "fl_hip_abd", "fl_hip_pitch", "fl_knee", + "fr_hip_abd", "fr_hip_pitch", "fr_knee", + "rl_hip_abd", "rl_hip_pitch", "rl_knee", + "rr_hip_abd", "rr_hip_pitch", "rr_knee", + "fl_wheel", "fr_wheel", "rl_wheel", "rr_wheel", + ) + + def __init__(self, log_root: str = "logs"): + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + self.dir = Path(log_root) / timestamp + self.dir.mkdir(parents=True, exist_ok=True) + + self.state_path = self.dir / "state.csv" + self.events_path = self.dir / "events.jsonl" + + self._state_fp = open(self.state_path, "w", encoding="utf-8") + self._events_fp = open(self.events_path, "w", encoding="utf-8") + self._t0 = time.time() + self._closed = False + self._queue: "queue.Queue[tuple]" = queue.Queue(maxsize=20000) + self._dropped_state_rows = 0 + self._writer_thread = threading.Thread(target=self._writer_loop, name="sim2real-log-writer", daemon=True) + + self._write_state_header() + self._writer_thread.start() + self.event("LOG_START", session_dir=str(self.dir)) + print(f"[Log] {self.dir}") + + def _write_state_header(self): + cols = ["t", "t_rel", "phase"] + cols += [f"{joint}_pos" for joint in self.JOINT_LABELS] + cols += [f"{joint}_vel" for joint in self.JOINT_LABELS] + cols += [f"{joint}_tau" for joint in self.JOINT_LABELS] + cols += [f"{joint}_tgt" for joint in self.JOINT_LABELS] + cols += [f"{joint}_raw" for joint in self.JOINT_LABELS] + cols += ["gyro_x", "gyro_y", "gyro_z"] + cols += ["accel_x", "accel_y", "accel_z"] + cols += ["quat_w", "quat_x", "quat_y", "quat_z"] + cols += ["pgrav_x", "pgrav_y", "pgrav_z"] + cols += ["cmd_vx", "cmd_vy", "cmd_yaw"] + cols += ["imu_age_ms", "loop_dt_ms"] + cols += ["safety_level", "guard_level"] + cols += ["holdover", "stale_max", "fresh_count"] + cols += ["kp_scale", "nan_flag"] + cols += ["kp_leg_cmd", "kd_leg_cmd", "kd_wheel_cmd"] + cols += ["runtime_release_alpha", "runtime_release_hold_s", "runtime_blend_ratio"] + cols += ["hold_target_max_err", "policy_target_max_err", "hold_policy_max_gap"] + cols += [ + "stand_roll_deg", + "stand_pitch_deg", + "stand_roll_corr", + "stand_pitch_corr", + "stand_pitch_comp_enabled", + ] + cols += ["target_source_code"] + cols += [ + "clip_primary_joint_index", + "clip_primary_joint", + "clip_primary_target", + "clip_primary_measured", + "clip_primary_default", + "clip_primary_pos_err", + "clip_primary_raw", + "clip_primary_scaled", + ] + cols += ["safety_reason", "guard_reason"] + self._state_fp.write(",".join(cols) + "\n") + self._state_fp.flush() + + def state( + self, + phase: str, + joint_pos: np.ndarray, + joint_vel: np.ndarray, + joint_torque: np.ndarray, + target_pose: np.ndarray, + raw_action: Optional[np.ndarray], + gyro: np.ndarray, + accel: np.ndarray, + quat: np.ndarray, + proj_gravity: np.ndarray, + command: np.ndarray, + imu_age_ms: float, + loop_dt_ms: float, + safety_level: int = 0, + guard_level: int = 0, + holdover: int = 0, + stale_max: int = 0, + fresh_count: int = 16, + kp_scale: float = 1.0, + nan_flag: int = 0, + kp_leg_cmd: float = 0.0, + kd_leg_cmd: float = 0.0, + kd_wheel_cmd: float = 0.0, + runtime_release_alpha: float = 0.0, + runtime_release_hold_s: float = 0.0, + runtime_blend_ratio: float = 0.0, + hold_target_max_err: float = 0.0, + policy_target_max_err: float = 0.0, + hold_policy_max_gap: float = 0.0, + stand_roll_deg: float = 0.0, + stand_pitch_deg: float = 0.0, + stand_roll_corr: float = 0.0, + stand_pitch_corr: float = 0.0, + stand_pitch_comp_enabled: bool = False, + target_source: str = "", + clip_primary_joint: str = "", + clip_primary_target: float = 0.0, + clip_primary_measured: float = 0.0, + clip_primary_default: float = 0.0, + clip_primary_pos_err: float = 0.0, + clip_primary_raw: float = 0.0, + clip_primary_scaled: float = 0.0, + safety_reason: str = "", + guard_reason: str = "", + ): + if self._closed: + return + if target_pose is None: + target_pose = np.zeros(16, dtype=np.float32) + if raw_action is None: + raw_action = np.zeros(16, dtype=np.float32) + + now = time.time() + numeric_values = [] + numeric_values += joint_pos.tolist() + numeric_values += joint_vel.tolist() + numeric_values += joint_torque.tolist() + numeric_values += target_pose.tolist() + numeric_values += raw_action.tolist() + numeric_values += gyro.tolist() + numeric_values += accel.tolist() + numeric_values += quat.tolist() + numeric_values += proj_gravity.tolist() + numeric_values += command.tolist() + numeric_values += [imu_age_ms, loop_dt_ms] + numeric_values += [safety_level, guard_level, holdover, stale_max, fresh_count, kp_scale, nan_flag] + numeric_values += [kp_leg_cmd, kd_leg_cmd, kd_wheel_cmd] + numeric_values += [runtime_release_alpha, runtime_release_hold_s, runtime_blend_ratio] + numeric_values += [hold_target_max_err, policy_target_max_err, hold_policy_max_gap] + numeric_values += [ + stand_roll_deg, + stand_pitch_deg, + stand_roll_corr, + stand_pitch_corr, + 1.0 if stand_pitch_comp_enabled else 0.0, + ] + numeric_values += [_target_source_code(target_source)] + numeric_values += [_csv_numeric_joint_index(clip_primary_joint)] + numeric_values += [ + clip_primary_target, + clip_primary_measured, + clip_primary_default, + clip_primary_pos_err, + clip_primary_raw, + clip_primary_scaled, + ] + + parts = [f"{now:.6f}", f"{now - self._t0:.6f}", phase] + parts += [f"{value:.6f}" for value in numeric_values] + parts += [_csv_escape(clip_primary_joint), _csv_escape(safety_reason), _csv_escape(guard_reason)] + self._enqueue(("state", ",".join(parts) + "\n"), drop_if_full=True) + + def event(self, kind: str, **fields: Any): + if self._closed: + return + record = {"t": time.time(), "t_rel": time.time() - self._t0, "kind": kind} + for key, value in fields.items(): + if isinstance(value, np.ndarray): + record[key] = value.tolist() + elif isinstance(value, (np.integer, np.floating)): + record[key] = value.item() + else: + record[key] = value + self._enqueue(("event", json.dumps(record, ensure_ascii=False) + "\n", kind != "STATE_TICK"), drop_if_full=False) + if kind != "STATE_TICK": + print(f"[Event {record['t_rel']:7.2f}s] {kind} {_short_fields(fields)}") + + def flush(self): + if not self._closed: + self._queue.join() + self._state_fp.flush() + self._events_fp.flush() + + def close(self): + if self._closed: + return + self.event("LOG_END") + self._queue.join() + self._closed = True + self._enqueue(("close",), drop_if_full=False, allow_after_closed=True) + self._writer_thread.join(timeout=2.0) + self._state_fp.flush() + self._state_fp.close() + self._events_fp.flush() + self._events_fp.close() + print(f"[Log] saved -> {self.dir}") + + def _enqueue(self, item: tuple, drop_if_full: bool, allow_after_closed: bool = False): + if self._closed and not allow_after_closed: + return + try: + if drop_if_full: + self._queue.put_nowait(item) + else: + self._queue.put(item, timeout=0.2) + except queue.Full: + if item and item[0] == "state": + self._dropped_state_rows += 1 + + def _writer_loop(self): + while True: + item = self._queue.get() + try: + kind = item[0] + if kind == "close": + return + if kind == "state": + self._state_fp.write(item[1]) + elif kind == "event": + self._events_fp.write(item[1]) + if item[2]: + self._events_fp.flush() + finally: + self._queue.task_done() + + +def _csv_escape(text: str) -> str: + if not text: + return "" + return text.replace(",", ";").replace("\n", " ").replace("\r", " ") + + +def _csv_numeric_joint_index(joint_name: str) -> float: + if not joint_name: + return -1.0 + try: + return float(LogBundle.JOINT_LABELS.index(joint_name)) + except ValueError: + return -1.0 + + +def _target_source_code(target_source: str) -> float: + mapping = { + "": -1.0, + "startup_hold": 0.0, + "stand_balance": 1.0, + "stand_hold": 1.5, + "runtime_hold": 2.0, + "runtime_blend": 3.0, + "runtime_policy": 4.0, + } + return mapping.get(target_source, 99.0) + + +def _short_fields(fields: Dict[str, Any]) -> str: + parts = [] + for key, value in fields.items(): + if isinstance(value, (list, tuple, np.ndarray)): + arr = np.asarray(value).ravel() + if arr.size > 4: + continue + try: + parts.append(f"{key}=[{','.join(f'{float(x):.2f}' for x in arr)}]") + except (TypeError, ValueError): + parts.append(f"{key}={list(arr)[:4]}") + elif isinstance(value, float): + parts.append(f"{key}={value:.3f}") + else: + parts.append(f"{key}={value}") + return " ".join(parts) + + +SimpleLogger = LogBundle diff --git a/05_software/real/sim2real_v2/tools/math_utils.py b/05_software/real/sim2real_v2/tools/math_utils.py new file mode 100644 index 0000000..da7b2c3 --- /dev/null +++ b/05_software/real/sim2real_v2/tools/math_utils.py @@ -0,0 +1,111 @@ +"""数学工具 — 与 rc_mjlab/sim2sim/tools/math_utils.py 数值完全一致。""" +from typing import Optional +import numpy as np + + +def get_gravity_orientation(quat_wxyz: np.ndarray) -> np.ndarray: + qw, qx, qy, qz = quat_wxyz + gx = 2.0 * (-qz * qx + qw * qy) + gy = -2.0 * (qz * qy + qw * qx) + gz = 1.0 - 2.0 * (qw * qw + qz * qz) + return np.array([gx, gy, gz], dtype=np.float32) + + +def quat_rotate_inverse(quat_wxyz: np.ndarray, v: np.ndarray) -> np.ndarray: + q_w = quat_wxyz[0] + q_vec = quat_wxyz[1:] + a = v * (2.0 * q_w * q_w - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * np.dot(q_vec, v) * 2.0 + return a - b + c + + +def quat_from_accel(accel: np.ndarray) -> np.ndarray: + """用静止重力方向初始化机身姿态四元数。 + + 思想：仿真启动时 quat = [1,0,0,0] 隐含"机身完全水平"，但真机摆在地面上 + pitch/roll 通常各自有几度偏差，会让 projected_gravity 一开始就错。 + 用加速度计读数与 [0,0,-1] 的最短旋转作为初值，可以把首步重力误差 + 降到 IMU 噪声级。 + """ + g_meas = accel / (np.linalg.norm(accel) + 1e-9) + g_ref = np.array([0.0, 0.0, 1.0], dtype=np.float32) + cross = np.cross(g_ref, g_meas) + dot = float(np.dot(g_ref, g_meas)) + if dot < -0.999999: + return np.array([0.0, 1.0, 0.0, 0.0], dtype=np.float32) + s = float(np.sqrt((1.0 + dot) * 2.0)) + q = np.array([s * 0.5, cross[0] / s, cross[1] / s, cross[2] / s], dtype=np.float32) + return q / (np.linalg.norm(q) + 1e-9) + + +class LowPassFilter: + """一阶 IIR 低通，alpha 公式与训练侧 rc_mjlab/src/robot/mdp/lowpass_actions.py + `_lowpass_weights` 完全一致： + + alpha = 1 - exp(-2π · cutoff_freq / control_freq) + = 1 - exp(-2π · cutoff_freq · dt) + + 注意：这与 rc_mjlab/sim2sim/interface/mujoco_io.py 用的近似公式 + (dt / (dt + 1/(2π·fc))) 数值上不同，在 15Hz 截止时差约 30%。 + 我们以训练侧为准，因为策略是在那个滤波下学的。 + """ + + def __init__(self, cutoff_freq: float, dt: float, dim: int): + self.alpha = float(1.0 - np.exp(-2.0 * np.pi * cutoff_freq * dt)) + self.y_prev = None + + def filter(self, x: np.ndarray) -> np.ndarray: + if self.y_prev is None: + self.y_prev = x.copy() + y = self.alpha * x + (1.0 - self.alpha) * self.y_prev + self.y_prev = y.copy() + return y + + def reset(self): + self.y_prev = None + + +class MahonyFilter: + """互补滤波器：高频用陀螺仪积分，低频用加速度计修正。""" + + def __init__(self, kp: float = 2.0, ki: float = 0.0, dt: float = 0.02): + self.kp = kp + self.ki = ki + self.dt = dt + self.q = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32) + self.e_int = np.zeros(3, dtype=np.float32) + + def reset_with_accel(self, accel: np.ndarray): + self.q = quat_from_accel(accel) + self.e_int.fill(0.0) + + def update(self, accel: np.ndarray, gyro: np.ndarray, dt: Optional[float] = None) -> np.ndarray: + if dt is None: + dt = self.dt + norm_a = float(np.linalg.norm(accel)) + if norm_a > 1e-6: + a = accel / norm_a + q = self.q + v = np.array([ + 2.0 * (q[1] * q[3] - q[0] * q[2]), + 2.0 * (q[0] * q[1] + q[2] * q[3]), + q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3], + ], dtype=np.float32) + e = np.cross(a, v) + if self.ki > 0.0: + self.e_int += e * dt + else: + self.e_int.fill(0.0) + gyro = gyro + self.kp * e + self.ki * self.e_int + + q = self.q + q_dot = 0.5 * np.array([ + -q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2], + q[0] * gyro[0] + q[2] * gyro[2] - q[3] * gyro[1], + q[0] * gyro[1] - q[1] * gyro[2] + q[3] * gyro[0], + q[0] * gyro[2] + q[1] * gyro[1] - q[2] * gyro[0], + ], dtype=np.float32) + self.q += q_dot * dt + self.q /= (np.linalg.norm(self.q) + 1e-9) + return self.q diff --git a/05_software/real/sim2real_v2/tools/remote_live_test.py b/05_software/real/sim2real_v2/tools/remote_live_test.py new file mode 100644 index 0000000..b1306fd --- /dev/null +++ b/05_software/real/sim2real_v2/tools/remote_live_test.py @@ -0,0 +1,61 @@ +from __future__ import annotations + +import argparse +import sys +import time +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +from input_dev.remote_uart import RemoteCommandSource # noqa: E402 + + +def main() -> None: + parser = argparse.ArgumentParser() + parser.add_argument("--port", required=True) + parser.add_argument("--max-vx", type=float, default=0.8) + parser.add_argument("--max-vy", type=float, default=0.3) + parser.add_argument("--max-yaw", type=float, default=0.5) + parser.add_argument("--hz", type=float, default=20.0) + parser.add_argument("--baudrate", type=int, default=100000) + parser.add_argument("--timeout", type=float, default=0.02) + parser.add_argument("--deadzone", type=int, default=50) + args = parser.parse_args() + + remote = RemoteCommandSource( + port=args.port, + baudrate=args.baudrate, + timeout=args.timeout, + axis_deadzone=args.deadzone, + active_threshold=args.deadzone, + max_vx=args.max_vx, + max_vy=args.max_vy, + max_yaw=args.max_yaw, + ) + remote.open() + print(f"[remote-test] listening on {args.port}") + try: + period = 1.0 / max(args.hz, 1.0) + while True: + remote.poll() + status = remote.get_status() + print( + "cmd=({:+.3f}, {:+.3f}, {:+.3f}) active={} estop={} raw=({}, {}, {}, {})".format( + status["cmd"][0], + status["cmd"][1], + status["cmd"][2], + status["command_active"], + status["estop_requested"], + status["ch1"], + status["ch2"], + status["ch3"], + status["ch4"], + ) + ) + time.sleep(period) + finally: + remote.close() + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real_v2/tools/standalone_check.py b/05_software/real/sim2real_v2/tools/standalone_check.py new file mode 100644 index 0000000..ad752e8 --- /dev/null +++ b/05_software/real/sim2real_v2/tools/standalone_check.py @@ -0,0 +1,105 @@ +"""Check whether `sim2real/` is self-contained enough for direct deployment.""" + +from __future__ import annotations + +import importlib +from pathlib import Path +import sys + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + + +REQUIRED_FILES = [ + "config.yaml", + "deployment_manifest.yaml", + "main.py", + "policy/policy_runner.py", + "interface/real_io.py", + "interface/imu_client.py", + "interface/motor_driver.py", + "vendored/drivers/motor_driver.py", + "vendored/drivers/usb_can_adapter.py", + "vendored/odin1_imu/odin1_imu.py", + "vendored/odin1_imu/build/libodin1_imu_bridge.so", + "mjcf/wheelleg.xml", +] + +REQUIRED_IMPORTS = [ + "numpy", + "yaml", + "serial", +] + +OPTIONAL_IMPORTS = [ + ("onnxruntime", "required when deploying the default ONNX policy"), + ("torch", "required only for exporting/checking .pt policies"), + ("pynput", "only needed for CLI keyboard control"), +] + + +def check() -> int: + root = Path(__file__).resolve().parents[1] + issues: list[str] = [] + warnings: list[str] = [] + + print(f"[Check] sim2real root: {root}") + + for rel in REQUIRED_FILES: + path = root / rel + if path.exists(): + print(f"[Check] file: PASS {rel}") + else: + issues.append(f"missing required file: {rel}") + + if (root / "policies" / "model_rough.onnx").exists(): + print("[Check] file: PASS policies/model_rough.onnx") + elif (root / "policies" / "model_rough.pt").exists(): + warnings.append("policies/model_rough.onnx not found; runtime will fall back to .pt unless exported") + print("[Check] file: PASS policies/model_rough.pt fallback") + else: + issues.append("missing policy file: policies/model_rough.onnx or policies/model_rough.pt") + + for module_name in REQUIRED_IMPORTS: + try: + importlib.import_module(module_name) + print(f"[Check] import: PASS {module_name}") + except Exception as exc: + issues.append(f"missing python dependency `{module_name}`: {exc}") + + for module_name, note in OPTIONAL_IMPORTS: + try: + importlib.import_module(module_name) + print(f"[Check] optional import: PASS {module_name}") + except Exception: + warnings.append(f"optional dependency `{module_name}` not found ({note})") + + index_html = (root / "web" / "static" / "index.html").read_text(encoding="utf-8") + if "https://unpkg.com/three@" in index_html: + warnings.append( + "web 3D viewer depends on remote three.js CDN; CLI/web backend are standalone, " + "but full offline 3D viewer is not bundled yet" + ) + + if issues: + print("\n" + "=" * 60) + print("Standalone deployment check: FAIL") + for item in issues: + print(f"- {item}") + else: + print("\n" + "=" * 60) + print("Standalone deployment check: PASS") + + if warnings: + print("\nWarnings:") + for item in warnings: + print(f"- {item}") + + return 1 if issues else 0 + + +def main(): + raise SystemExit(check()) + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real_v2/tools/test_angle_wrap.py b/05_software/real/sim2real_v2/tools/test_angle_wrap.py new file mode 100644 index 0000000..e8e9e62 --- /dev/null +++ b/05_software/real/sim2real_v2/tools/test_angle_wrap.py @@ -0,0 +1,45 @@ +"""Offline checks for RS02 multi-turn angle wrapping in startup/control.""" + +from __future__ import annotations + +from pathlib import Path +import sys + +import numpy as np + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +from interface.motor_mapping import MotorMapping # noqa: E402 +from startup.pose_initializer import STAND_POSE, _periodic_leg_delta # noqa: E402 + + +def main() -> int: + mapper = MotorMapping() + + # Real log example: startup saw rl_knee sim angle 4.648rad while the stand + # target was -1.8rad. Those are close modulo 2*pi and must not plan a full turn. + raw_sim = STAND_POSE.copy() + raw_sim[8] = 4.648097991943359 + raw_real = mapper.sim_to_real(raw_sim) + canonical = mapper.real_to_sim({(2, 3): raw_real[(2, 3)]}) + delta = _periodic_leg_delta(canonical, STAND_POSE) + real_target = mapper.sim_to_real(STAND_POSE, current_real_pos={(2, 3): raw_real[(2, 3)]})[(2, 3)] + real_move = real_target - raw_real[(2, 3)] + + print(f"[WrapCheck] canonical_sim_idx8={canonical[8]:.6f}") + print(f"[WrapCheck] startup_delta_idx8={delta[8]:.6f}") + print(f"[WrapCheck] real_move_idx8={real_move:.6f}") + + if abs(float(delta[8])) > 0.25: + print("[WrapCheck] FAIL: periodic startup delta is too large") + return 1 + if abs(float(real_move)) > 0.25: + print("[WrapCheck] FAIL: real target would command a long-path move") + return 1 + + print("[WrapCheck] PASS") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/05_software/real/sim2real_v2/tools/test_remote_mapping.py b/05_software/real/sim2real_v2/tools/test_remote_mapping.py new file mode 100644 index 0000000..4af74bc --- /dev/null +++ b/05_software/real/sim2real_v2/tools/test_remote_mapping.py @@ -0,0 +1,54 @@ +from __future__ import annotations + +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +from input_dev.remote_uart import ( # noqa: E402 + RemoteCommandMapper, + RemoteControlState, + RemoteSwitchState, + SWITCH_HIGH, + SWITCH_MID, +) + + +def assert_close(actual: float, expected: float, tol: float = 1e-6) -> None: + if abs(actual - expected) > tol: + raise AssertionError(f"expected {expected}, got {actual}") + + +def test_deadzone() -> None: + mapper = RemoteCommandMapper(max_vx=0.8, max_vy=0.3, max_yaw=0.5, active_threshold=50) + state = RemoteControlState(ch1=40, ch2=-49, ch4=50, switches=RemoteSwitchState(ch7=SWITCH_MID), frame_ok=True) + cmd = mapper.map_command(state) + assert_close(float(cmd[0]), 0.0) + assert_close(float(cmd[1]), 0.0) + assert_close(float(cmd[2]), 0.0) + if mapper.is_command_active(state): + raise AssertionError("deadzone values should not be active") + + +def test_mapping() -> None: + mapper = RemoteCommandMapper(max_vx=0.8, max_vy=0.3, max_yaw=0.5, active_threshold=50) + state = RemoteControlState(ch1=330, ch2=-660, ch4=165, switches=RemoteSwitchState(ch7=SWITCH_MID), frame_ok=True) + cmd = mapper.map_command(state) + assert_close(float(cmd[0]), -0.8) + assert_close(float(cmd[1]), 0.075) + assert_close(float(cmd[2]), 0.25) + if not mapper.is_command_active(state): + raise AssertionError("mapped command should be active") + + +def test_soft_estop_flag() -> None: + state = RemoteControlState(ch1=0, ch2=0, ch4=0, switches=RemoteSwitchState(ch7=SWITCH_HIGH), frame_ok=True) + if not state.estop_requested: + raise AssertionError("switch high should request estop") + + +if __name__ == "__main__": + test_deadzone() + test_mapping() + test_soft_estop_flag() + print("remote command mapping tests passed") diff --git a/05_software/real/sim2real_v2/vendored/__init__.py b/05_software/real/sim2real_v2/vendored/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real_v2/vendored/drivers/__init__.py b/05_software/real/sim2real_v2/vendored/drivers/__init__.py new file mode 100644 index 0000000..0f1cd3d --- /dev/null +++ b/05_software/real/sim2real_v2/vendored/drivers/__init__.py @@ -0,0 +1,3 @@ +from drivers.motor_driver import RobStrideDriver, RobStrideMotor, MotorState +from drivers.motor_params import CommunicationType, ParamIndex, RunMode +from drivers.usb_can_adapter import DmUsbAdapter diff --git a/05_software/real/sim2real_v2/vendored/drivers/motor_driver.py b/05_software/real/sim2real_v2/vendored/drivers/motor_driver.py new file mode 100644 index 0000000..653b7c4 --- /dev/null +++ b/05_software/real/sim2real_v2/vendored/drivers/motor_driver.py @@ -0,0 +1,371 @@ +import struct +import time +import queue +import numpy as np +from typing import Dict, Optional, Any, List +from dataclasses import dataclass + +from drivers.usb_can_adapter import DmUsbAdapter +from drivers.motor_params import ( + CommunicationType, ParamIndex, ParamType, + MODEL_MIT_POSITION_TABLE, MODEL_MIT_VELOCITY_TABLE, + MODEL_MIT_TORQUE_TABLE, MODEL_MIT_KP_TABLE, MODEL_MIT_KD_TABLE, + get_pack_format, PARAM_TABLE +) + +@dataclass +class MotorState: + position: float = 0.0 + velocity: float = 0.0 + torque: float = 0.0 + temperature: float = 0.0 + current: float = 0.0 + update_count: int = 0 + +class RobStrideMotor: + def __init__(self, name: str, motor_id: int, model: str): + """ + 初始化电机对象。 + + :param name: 电机名称 (例如 "knee") + :param motor_id: 电机 ID + :param model: 电机型号 (例如 "rs-06") + """ + self.name = name + self.id = motor_id + self.model = model + self.state = MotorState() + + def update_state(self, pos: float, vel: float, torque: float, temp: float, current: float = 0.0): + """ + 更新电机状态。 + """ + self.state.position = pos + self.state.velocity = vel + self.state.torque = torque + self.state.temperature = temp + self.state.update_count += 1 + if current != 0.0: + self.state.current = current + +class RobStrideDriver: + def __init__(self, port: str, debug: bool = False): + """ + 初始化驱动器。 + + :param port: 串口名称 + :param debug: 是否开启调试模式 + """ + self.adapter = DmUsbAdapter(port, debug=debug) + self.motors: Dict[str, RobStrideMotor] = {} + self.motors_by_id: Dict[int, RobStrideMotor] = {} + self.host_id = 0xFD # 根据文档，主机 ID 默认为 0xFD + + self.parameter_values = {} # 读取参数缓存: (motor_id, param_index) -> value + + def connect(self): + """连接到底层适配器。""" + self.adapter.open() + print(f"已连接到 RobStride 驱动器，端口: {self.adapter.serial.port}") + # 设置 CAN 波特率为 1000kbps (Index 0) + self.adapter.set_can_baudrate(0) + + def disconnect(self): + """断开连接。""" + self.adapter.close() + print("已断开 RobStride 驱动器连接") + + def set_can_id(self, current_id: int, new_id: int): + """ + 设置电机 CAN ID。 + + :param current_id: 当前电机 ID + :param new_id: 新电机 ID + """ + # Type 7: Set CAN ID + # Bits 23-16: New ID (Preset ID) + # Bits 15-8: Master ID + # Bits 7-0: Target ID + extra_data = (new_id << 8) | self.host_id + self._send_command(CommunicationType.SET_CAN_ID, extra_data, current_id) + print(f"已发送 ID 修改指令: {current_id} -> {new_id} (Master: {self.host_id})") + + def scan_motors(self, timeout: float = 0.1) -> List[int]: + """ + 快速扫描总线上的电机 (ID 1-127)。 + + :param timeout: 等待响应的超时时间 + :return: 发现的电机 ID 列表 + """ + found_ids = [] + print("正在快速扫描所有电机 (ID 1-127)...") + + # 清空缓冲区 + while self.adapter.read_can_frame(): + pass + + # 快速发送查询指令 + for dev_id in range(1, 128): + # 发送获取设备 ID 命令 + self._send_command(CommunicationType.GET_DEVICE_ID, self.host_id, dev_id) + + # 等待响应 + start_time = time.time() + while time.time() - start_time < timeout: + frame = self.adapter.read_can_frame() + if frame: + can_id, data, cmd, ide, rtr = frame + if not ide: continue + + # 解析回复 + # 通信类型 0 (GET_DEVICE_ID/Status) + comm_type = (can_id >> 24) & 0x1F + + if comm_type == CommunicationType.GET_DEVICE_ID: # Type 0 + # Type 0 回复格式: + # Bits 23-8: Status info + # Bits 7-0: Motor ID + extra_data = (can_id >> 8) & 0xFFFF + motor_id = extra_data & 0xFF # Device ID + + if motor_id not in found_ids: + print(f"发现电机 ID: {motor_id}") + found_ids.append(motor_id) + + return sorted(found_ids) + + + def add_motor(self, name: str, motor_id: int, model: str): + """ + 添加电机到控制列表。 + + :param name: 电机名称 + :param motor_id: 电机 ID + :param model: 电机型号 + """ + motor = RobStrideMotor(name, motor_id, model) + self.motors[name] = motor + self.motors_by_id[motor_id] = motor + + def _send_command(self, comm_type: int, extra_data: int, device_id: int, data: bytes = b''): + # 构建 29 位扩展 CAN ID + # Bits 28-24: 通信类型 (Communication Type) + # Bits 23-8: 额外数据 (Extra Data) + # Bits 7-0: 设备 ID (Device ID) + can_id = (comm_type << 24) | (extra_data << 8) | device_id + + # 通过适配器发送 + # RobStride 使用扩展帧 + self.adapter.send_can_frame(can_id, data, extended=True) + + def enable(self, motor_name: str): + """使能电机。""" + motor = self.motors[motor_name] + self._send_command(CommunicationType.ENABLE, self.host_id, motor.id) + + def disable(self, motor_name: str): + """失能电机 (Type 4: Stop)。""" + motor = self.motors[motor_name] + # Data: 全 0 + data = bytes([0x00]*8) + self._send_command(CommunicationType.DISABLE, self.host_id, motor.id, data) + + def clear_warnings(self, motor_name: str): + """ + 清除警告/故障 (Type 4: Stop Motor with Byte0=1)。 + 根据文档 Type 4: Byte[0]=1 时清除故障。 + """ + motor = self.motors[motor_name] + data = bytes([0x01] + [0x00]*7) + self._send_command(CommunicationType.DISABLE, self.host_id, motor.id, data) + + def set_zero_position(self, motor_name: str): + """设置电机当前位置为零点。""" + motor = self.motors[motor_name] + # Type 6: Set Zero Position + # Data: Byte0=1 + data = bytes([0x01] + [0x00]*7) + self._send_command(CommunicationType.SET_ZERO_POSITION, self.host_id, motor.id, data) + + def control_mit(self, motor_name: str, + position: float, velocity: float, + kp: float, kd: float, torque: float): + """ + 发送 MIT 控制指令。 + + :param motor_name: 电机名称 + :param position: 期望位置 (rad) + :param velocity: 期望速度 (rad/s) + :param kp: 位置增益 + :param kd: 速度增益 + :param torque: 前馈力矩 (Nm) + """ + motor = self.motors[motor_name] + model = motor.model + + # 获取限制值 + p_limit = MODEL_MIT_POSITION_TABLE.get(model, 12.5) + v_limit = MODEL_MIT_VELOCITY_TABLE.get(model, 50.0) + t_limit = MODEL_MIT_TORQUE_TABLE.get(model, 60.0) + kp_limit = MODEL_MIT_KP_TABLE.get(model, 500.0) + kd_limit = MODEL_MIT_KD_TABLE.get(model, 5.0) + + # 限幅 + position = np.clip(position, -p_limit, p_limit) + velocity = np.clip(velocity, -v_limit, v_limit) + kp = np.clip(kp, 0, kp_limit) + kd = np.clip(kd, 0, kd_limit) + torque = np.clip(torque, -t_limit, t_limit) + + # 转换为 uint16 + # Position: [-L, L] -> [0, 65535] + p_u16 = int(((position / p_limit) + 1.0) * 32767.0) + p_u16 = np.clip(p_u16, 0, 65535) + + # Velocity: [-L, L] -> [0, 65535] + v_u16 = int(((velocity / v_limit) + 1.0) * 32767.0) + v_u16 = np.clip(v_u16, 0, 65535) + + # Kp: [0, L] -> [0, 65535] + kp_u16 = int((kp / kp_limit) * 65535.0) + kp_u16 = np.clip(kp_u16, 0, 65535) + + # Kd: [0, L] -> [0, 65535] + kd_u16 = int((kd / kd_limit) * 65535.0) + kd_u16 = np.clip(kd_u16, 0, 65535) + + # Torque: [-L, L] -> [0, 65535] (发送在 Extra Data 域) + t_u16 = int(((torque / t_limit) + 1.0) * 32767.0) + t_u16 = np.clip(t_u16, 0, 65535) + + # 打包数据 (大端序) + data = struct.pack('>HHHH', p_u16, v_u16, kp_u16, kd_u16) + + # 发送 + self._send_command(CommunicationType.OPERATION_CONTROL, t_u16, motor.id, data) + + def read_parameter(self, motor_id: int, param_index: int): + """ + 发送读取参数指令 (Type 17)。 + """ + # Type 17 + # Data: Index (2B) + 00 00 + 00 00 00 00 + data = struct.pack('> 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('> 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) diff --git a/05_software/real/sim2real_v2/vendored/drivers/motor_params.py b/05_software/real/sim2real_v2/vendored/drivers/motor_params.py new file mode 100644 index 0000000..0930c19 --- /dev/null +++ b/05_software/real/sim2real_v2/vendored/drivers/motor_params.py @@ -0,0 +1,422 @@ +import numpy as np +import struct + +class CommunicationType: + """ + 电机通信类型定义 (Bit28~24) + 参考说明书 4.1 章节 + + 通信 ID 结构 (29位扩展帧): + | Bit 28-24 | Bit 23-8 | Bit 7-0 | + | 通信类型 | 数据区2 | 目标地址 | + """ + GET_DEVICE_ID = 0 # 获取设备 ID 和 64 位 MCU 唯一标识符 (Type 0) + OPERATION_CONTROL = 1 # 运控模式电机控制指令 (MIT 模式) (Type 1) + OPERATION_STATUS = 2 # 电机反馈数据 (标准反馈帧) (Type 2) + ENABLE = 3 # 电机使能运行 (Type 3) + DISABLE = 4 # 电机停止运行 (可用于清除故障) (Type 4) + SET_ZERO_POSITION = 6 # 设置电机机械零位 (设置当前位置为零点) (Type 6) + SET_CAN_ID = 7 # 设置电机 CAN ID (立即生效，需保存) (Type 7) + READ_PARAMETER = 17 # 单个参数读取 (Type 17, 0x11) + WRITE_PARAMETER = 18 # 单个参数写入 (Type 18, 0x12, 掉电丢失) + FAULT_REPORT = 21 # 故障反馈帧 (Type 21, 0x15) + SAVE_PARAMETERS = 22 # 电机数据保存帧 (保存所有参数到 Flash) (Type 22) + SET_BAUDRATE = 23 # 电机波特率修改帧 (重新上电生效) (Type 23) + ACTIVE_REPORT = 24 # 电机主动上报设置帧 (开启/关闭主动上报) (Type 24) + PROTOCOL_SWITCH = 25 # 电机协议修改帧 (切换 Canopen/MIT/私有协议) (Type 25) + READ_VERSION = 26 # 版本号读取帧 (Type 26) + +class RunMode: + """ + 电机运行模式 (参数索引 0x7005) + 参考说明书 4.3 章节 + """ + MIT = 0 # 运控模式 (默认): 适用于高动态响应控制 + POS_PP = 1 # 位置模式 (PP): 梯形加减速位置控制 + SPEED = 2 # 速度模式: 闭环速度控制 + CURRENT = 3 # 电流模式: 闭环力矩(电流)控制 + POS_CSP = 5 # 位置模式 (CSP): 循环同步位置模式 (适用于周期性指令) + +class BaudRate: + """ + 电机波特率 (通信类型 23) + 参考说明书 4.1 通信类型 23 + 注意: 修改后需重新上电生效 + """ + BAUD_1M = 1 # 1 Mbps (默认) + BAUD_500K = 2 # 500 Kbps + BAUD_250K = 3 # 250 Kbps + BAUD_125K = 4 # 125 Kbps + +class ActiveReportStatus: + """ + 电机主动上报状态 (通信类型 24) + 参考说明书 4.1 通信类型 24 + """ + DISABLE = 0 # 关闭主动上报 (默认) + ENABLE = 1 # 开启主动上报 (默认间隔 10ms, 可通过 EP_SCAN_TIME 修改) + +class ProtocolType: + """ + 电机协议类型 (通信类型 25) + 参考说明书 4.2.4 章节 + 注意: 切换协议后需重新上电生效 + """ + PRIVATE = 0 # 私有协议 (默认): 使用 29 位扩展帧 + CANOPEN = 1 # CANopen 协议: 符合 CiA 402 标准 + MIT = 2 # MIT 协议 (标准帧): 使用 11 位标准帧 + +class ParamType: + """ + 参数数据类型定义 + + - 私有协议 (Type 17/18) 参数表主要使用 UINT8/UINT16/UINT32/FLOAT + - CANopen 对象字典会用到有符号类型 (INTEGER8/16/32) + """ + UINT8 = 0 # 无符号 8 位整数 + UINT16 = 1 # 无符号 16 位整数 + UINT32 = 2 # 无符号 32 位整数 + FLOAT = 3 # 32 位浮点数 (IEEE 754) + INT8 = 4 # 有符号 8 位整数 + INT16 = 5 # 有符号 16 位整数 + INT32 = 6 # 有符号 32 位整数 + +class ErrorCode: + """ + 异常状态 fault 值位定义 + + 说明书位置: + - 章节 6 (Mit) 的“异常状态应答帧”对 fault 值 bit 位做了明确描述 + - 私有协议 Type 21 故障反馈帧也会携带 fault/warning 值 + """ + OVER_TEMP = 1 << 0 # bit0: 电机过温故障 (默认 >145°C) + DRIVE_CHIP = 1 << 1 # bit1: 驱动芯片故障 (DRV8353 等报告错误) + UNDER_VOLTAGE = 1 << 2 # bit2: 欠压故障 (电压 < 12V) + OVER_VOLTAGE = 1 << 3 # bit3: 过压故障 (电压 > 60V) + CURRENT_B_OVER = 1 << 4 # bit4: B 相电流采样过流 + CURRENT_C_OVER = 1 << 5 # bit5: C 相电流采样过流 + ENCODER_NOT_CALIB = 1 << 7 # bit7: 编码器未标定 + HARDWARE_ERR = 1 << 8 # bit8: 硬件识别故障 + POS_INIT_ERR = 1 << 9 # bit9: 位置初始化故障 + LOAD_BLOCK = 1 << 14 # bit14: 堵转过载算法保护 + CURRENT_A_OVER = 1 << 16 # bit16: A 相电流采样过流 + +class WarningCode: + """ + 预警状态 warning 值位定义 (Type 21 Byte 4-7) + """ + OVER_TEMP_WARNING = 1 << 0 # bit0: 电机过温预警 (默认 >135°C) + +class DriveFault1: + """ + 驱动芯片故障码 1 (0x3024) - DRV8353 状态寄存器 1 + 参考说明书 3.3.7 章节 + """ + VDS_LC = 1 << 0 # VDS overcurrent on C low-side (C相下管VDS过流) + VDS_HC = 1 << 1 # VDS overcurrent on C high-side (C相上管VDS过流) + VDS_LB = 1 << 2 # VDS overcurrent on B low-side (B相下管VDS过流) + VDS_HB = 1 << 3 # VDS overcurrent on B high-side (B相上管VDS过流) + VDS_LA = 1 << 4 # VDS overcurrent on A low-side (A相下管VDS过流) + VDS_HA = 1 << 5 # VDS overcurrent on A high-side (A相上管VDS过流) + OTSD = 1 << 6 # Overtemperature shutdown (过温关断) + UVLO = 1 << 7 # Undervoltage lockout (欠压锁定) + GDF = 1 << 8 # Gate drive fault (栅极驱动故障) + VDS_OCP = 1 << 9 # VDS monitor overcurrent (VDS 监控过流) + FAULT = 1 << 10 # Logic OR of FAULT status (故障状态逻辑或) + +class DriveFault2: + """ + 驱动芯片故障码 2 (0x3025) - DRV8353 状态寄存器 2 + 参考说明书 3.3.7 章节 + """ + VGS_LC = 1 << 0 # Gate drive fault on C low-side (C相下管栅极故障) + VGS_HC = 1 << 1 # Gate drive fault on C high-side (C相上管栅极故障) + VGS_LB = 1 << 2 # Gate drive fault on B low-side (B相下管栅极故障) + VGS_HB = 1 << 3 # Gate drive fault on B high-side (B相上管栅极故障) + VGS_LA = 1 << 4 # Gate drive fault on A low-side (A相下管栅极故障) + VGS_HA = 1 << 5 # Gate drive fault on A high-side (A相上管栅极故障) + GDUV = 1 << 6 # VCP charge pump / VGLS undervoltage (电荷泵欠压) + OTW = 1 << 7 # Overtemperature warning (过温预警) + SC_OC = 1 << 8 # Overcurrent on phase C sense amplifier (C相采样过流) + SB_OC = 1 << 9 # Overcurrent on phase B sense amplifier (B相采样过流) + SA_OC = 1 << 10 # Overcurrent on phase A sense amplifier (A相采样过流) + +class MotorParams: + """ + 电机物理参数限制 (用于 MIT 模式数据压缩) + 参考说明书 4.1 通信类型 1 + + 注意: + - P_MIN/MAX: 位置范围 (RS03: -12.57 ~ 12.57 rad) + - V_MIN/MAX: 速度范围 (RS03: -20 ~ 20 rad/s) + - T_MIN/MAX: 力矩范围 (RS03: -60 ~ 60 Nm) + - KP/KD: 刚度和阻尼系数范围 + """ + def __init__(self, + p_min: float = -12.57, + p_max: float = 12.57, # RS03: -12.57 ~ 12.57 rad (约 -4pi ~ 4pi) + v_min: float = -20.0, + v_max: float = 20.0, # RS03: -20 ~ 20 rad/s + kp_min: float = 0.0, + kp_max: float = 5000.0, # RS03: 0 ~ 5000 + kd_min: float = 0.0, + kd_max: float = 100.0, # RS03: 0 ~ 100 + t_min: float = -60.0, + t_max: float = 60.0): # RS03: -60 ~ 60 Nm + self.P_MIN = p_min + self.P_MAX = p_max + self.V_MIN = v_min + self.V_MAX = v_max + self.KP_MIN = kp_min + self.KP_MAX = kp_max + self.KD_MIN = kd_min + self.KD_MAX = kd_max + self.T_MIN = t_min + self.T_MAX = t_max + +class ParamIndex: + """ + 电机参数索引表 (Index) + 参考说明书 4.1 可读写单个参数列表 + """ + RUN_MODE = 0x7005 # 运行模式: 0:运控, 1:PP, 2:速度, 3:电流, 5:CSP (W/R) + IQ_REF = 0x7006 # 电流模式 Iq 指令 (-43~43A) (W/R) + SPD_REF = 0x700A # 转速模式转速指令 (-20~20rad/s) (W/R) + LIMIT_TORQUE = 0x700B # 转矩限制 (0~60Nm) (W/R) + CUR_KP = 0x7010 # 电流 Kp (默认 0.17) (W/R) + CUR_KI = 0x7011 # 电流 Ki (默认 0.012) (W/R) + CUR_FILT_GAIN = 0x7014 # 电流滤波系数 (0~1.0, 默认 0.1) (W/R) + LOC_REF = 0x7016 # 位置模式角度指令 (rad) (W/R) + LIMIT_SPD = 0x7017 # 位置模式(CSP)速度限制 (0~20rad/s) (W/R) + LIMIT_CUR = 0x7018 # 速度/位置模式电流限制 (0~43A) (W/R) + MECH_POS = 0x7019 # 负载端计圈机械角度 (rad) (Read Only) + IQF = 0x701A # Iq 滤波值 (A) (Read Only) + MECH_VEL = 0x701B # 负载端转速 (rad/s) (Read Only) + VBUS = 0x701C # 母线电压 (V) (Read Only) + LOC_KP = 0x701E # 位置环 Kp (默认 60) (W/R) + SPD_KP = 0x701F # 速度环 Kp (默认 6) (W/R) + SPD_KI = 0x7020 # 速度环 Ki (默认 0.02) (W/R) + SPD_FILT_GAIN = 0x7021 # 速度滤波值 (默认 0.1) (W/R) + ACC_RAD = 0x7022 # 速度模式加速度 (默认 20rad/s^2) (W/R) + VEL_MAX = 0x7024 # 位置模式(PP)速度 (默认 10rad/s) (W/R) + ACC_SET = 0x7025 # 位置模式(PP)加速度 (默认 10rad/s^2) (W/R) + EP_SCAN_TIME = 0x7026 # 主动上报时间 (1=10ms, +1=+5ms) (W) + CAN_TIMEOUT = 0x7028 # CAN 超时阈值 (20000=1s, 0=禁用) (W) + ZERO_STA = 0x7029 # 零点标志位 (0: 0~2pi, 1: -pi~pi) (W) + DAMPER = 0x702A # 阻尼开关 (1: 取消关机反驱保护) (W/R) + ADD_OFFSET = 0x702B # 零位偏置 (rad) (W/R) + +class CanopenIndex: + """ + CANopen 对象字典常用索引 + 参考说明书第 5 章 (Canopen) + """ + ERROR_CODE = 0x603F # 错误码 + CONTROLWORD = 0x6040 # 控制字 + STATUSWORD = 0x6041 # 状态字 + MODES_OF_OPERATION = 0x6060 # 运行模式 + MODES_OF_OPERATION_DISPLAY = 0x6061 # 当前运行模式显示 + POSITION_DEMAND_VALUE = 0x6062 # 位置指令值 + POSITION_ACTUAL_VALUE = 0x6064 # 位置实际值 + POSITION_WINDOW = 0x6067 # 位置窗口 + POSITION_WINDOW_TIME = 0x6068 # 位置窗口时间 + VELOCITY_DEMAND_VALUE = 0x606B # 速度指令值 + VELOCITY_ACTUAL_VALUE = 0x606C # 速度实际值 + TARGET_TORQUE = 0x6071 # 目标力矩 (0.1% 额定力矩) + TORQUE_ACTUAL_VALUE = 0x6077 # 力矩实际值 + CURRENT_ACTUAL_VALUE = 0x6078 # 电流实际值 + DC_LINK_CIRCUIT_VOLTAGE = 0x6079 # 母线电压 + TARGET_POSITION = 0x607A # 目标位置 + PROFILE_VELOCITY = 0x6081 # 轮廓速度 + PROFILE_ACCELERATION = 0x6083 # 轮廓加速度 + TARGET_VELOCITY = 0x60FF # 目标速度 + +class CanopenModeOfOperation: + """CANopen 模式 (6060)""" + PP = 1 # Profile Position Mode + SPEED = 3 # Profile Velocity Mode + TORQUE = 4 # Profile Torque Mode + CSP = 5 # Cyclic Synchronous Position Mode + HOMING = 6 # Homing Mode + +class CanopenControlword: + """CANopen 控制字 (6040) 常用值""" + SHUTDOWN = 0x0006 # Shutdown + SWITCH_ON = 0x0007 # Switch On + ENABLE_OPERATION = 0x000F # Enable Operation + DISABLE_VOLTAGE = 0x0001 # Disable Voltage + QUICK_STOP = 0x000B # Quick Stop + +# CANopen 协议切换帧 (扩展帧) +# 说明书 5.10: 29 位 ID 为 0xFFF，数据区 Byte0~6 固定 01~06，Byte7=F_CMD(协议类型) +CANOPEN_PROTOCOL_SWITCH_EXT_ID = 0xFFF + +class MitStdCommandType: + """ + MIT 标准帧指令类型 (对应说明书第 6 章的指令 1~11) + + 标准帧 ID (11位) 结构: + | Bit 10-8 | Bit 7-0 | + | 模式/指令 | 电机 ID | + + 注意: + - 指令 1~9: CAN ID 的 Bit10~8 为 0，通过数据区 Payload 区分功能 + - 指令 10: CAN ID 的 Bit10~8 为 1 (位置模式) + - 指令 11: CAN ID 的 Bit10~8 为 2 (速度模式) + """ + ENABLE = 1 # 指令 1: 电机使能运行 + STOP = 2 # 指令 2: 电机停止运行 + DYNAMIC_PARAM = 3 # 指令 3: MIT 动态参数 + SET_ZERO = 4 # 指令 4: 设置零点 (非位置模式) + CLEAR_ERROR_OR_READ_STATUS = 5 # 指令 5: 清错 / 读取异常状态 + SET_RUN_MODE = 6 # 指令 6: 设置运行模式 + SET_MOTOR_CAN_ID = 7 # 指令 7: 修改电机 CANID + SET_PROTOCOL = 8 # 指令 8: 修改电机协议 (重新上电生效) + SET_MASTER_CAN_ID = 9 # 指令 9: 修改主机 CANID + POS_CONTROL = 10 # 指令 10: 位置模式控制指令 (ID Bit10-8=1) + SPEED_CONTROL = 11 # 指令 11: 速度模式控制指令 (ID Bit10-8=2) + +def get_mit_can_id_mode(cmd_type: int) -> int: + """ + 获取 MIT 标准帧 CAN ID 的 Bit10~8 值 + + :param cmd_type: MitStdCommandType 枚举值 + :return: 模式位 (0, 1, 或 2) + """ + if cmd_type in (MitStdCommandType.POS_CONTROL,): + return 1 + elif cmd_type in (MitStdCommandType.SPEED_CONTROL,): + return 2 + else: + # 指令 1~9 (以及其他潜在指令) 默认为 0 + return 0 + +def build_mit_std_id(cmd_type: int, motor_id: int) -> int: + """ + 构建 MIT 标准帧 11 位 CAN ID + + :param cmd_type: MitStdCommandType 枚举值 + :param motor_id: 电机 ID (0~127) + :return: 11 位 CAN ID + """ + mode = get_mit_can_id_mode(cmd_type) + return ((mode & 0x07) << 8) | (motor_id & 0xFF) + +class MitPayloads: + """ + MIT 协议特殊指令的固定 Payload 定义 (指令 1, 2, 4, 5, 6, 7, 8, 9) + 部分指令的 Payload 末尾字节需要根据参数动态修改 + """ + # 指令 1: FF FF FF FF FF FF FF FC + ENABLE = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFC' + + # 指令 2: FF FF FF FF FF FF FF FD + STOP = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFD' + + # 指令 3: 动态参数 (全 0 或根据参数设置) + DYNAMIC_PARAM_ZERO = b'\x00\x00\x00\x00\x00\x00\x00\x00' + + # 指令 4: FF FF FF FF FF FF FF FE + SET_ZERO = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE' + + # 指令 5: FF FF FF FF FF FF FF FB (清除错误) + # 若 F_CMD (Byte6) 为 0xFF 则清除错误，否则为读取异常状态 + CLEAR_ERROR = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFB' + + # 指令 6: FF FF FF FF FF FF [Mode] FC + # Template, last 2 bytes are [Mode, FC] + SET_RUN_MODE_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF' + + # 指令 7: FF FF FF FF FF FF [NewID] FA + SET_MOTOR_CAN_ID_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF' + + # 指令 8: FF FF FF FF FF FF [Protocol] FD + SET_PROTOCOL_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF' + + # 指令 9: FF FF FF FF FF FF [MasterID] 01 + SET_MASTER_CAN_ID_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF' + + +# 参数表配置: (参数名, 数据类型, 字节数) +PARAM_TABLE = { + ParamIndex.RUN_MODE: ("run_mode", ParamType.UINT8, 1), + ParamIndex.IQ_REF: ("iq_ref", ParamType.FLOAT, 4), + ParamIndex.SPD_REF: ("spd_ref", ParamType.FLOAT, 4), + ParamIndex.LIMIT_TORQUE: ("limit_torque", ParamType.FLOAT, 4), + ParamIndex.CUR_KP: ("cur_kp", ParamType.FLOAT, 4), + ParamIndex.CUR_KI: ("cur_ki", ParamType.FLOAT, 4), + ParamIndex.CUR_FILT_GAIN: ("cur_filt_gain", ParamType.FLOAT, 4), + ParamIndex.LOC_REF: ("loc_ref", ParamType.FLOAT, 4), + ParamIndex.LIMIT_SPD: ("limit_spd", ParamType.FLOAT, 4), + ParamIndex.LIMIT_CUR: ("limit_cur", ParamType.FLOAT, 4), + ParamIndex.MECH_POS: ("mechPos", ParamType.FLOAT, 4), + ParamIndex.IQF: ("iqf", ParamType.FLOAT, 4), + ParamIndex.MECH_VEL: ("mechVel", ParamType.FLOAT, 4), + ParamIndex.VBUS: ("VBUS", ParamType.FLOAT, 4), + ParamIndex.LOC_KP: ("loc_kp", ParamType.FLOAT, 4), + ParamIndex.SPD_KP: ("spd_kp", ParamType.FLOAT, 4), + ParamIndex.SPD_KI: ("spd_ki", ParamType.FLOAT, 4), + ParamIndex.SPD_FILT_GAIN: ("spd_filt_gain", ParamType.FLOAT, 4), + ParamIndex.ACC_RAD: ("acc_rad", ParamType.FLOAT, 4), + ParamIndex.VEL_MAX: ("vel_max", ParamType.FLOAT, 4), + ParamIndex.ACC_SET: ("acc_set", ParamType.FLOAT, 4), + ParamIndex.EP_SCAN_TIME: ("EPScan_time", ParamType.UINT16, 2), + ParamIndex.CAN_TIMEOUT: ("cantimeout", ParamType.UINT32, 4), + ParamIndex.ZERO_STA: ("zero_sta", ParamType.UINT8, 1), + ParamIndex.DAMPER: ("damper", ParamType.UINT8, 1), + ParamIndex.ADD_OFFSET: ("add_offset", ParamType.FLOAT, 4), +} + +MODEL_MIT_POSITION_TABLE = { + "rs-00": 4 * np.pi, "rs-01": 4 * np.pi, "rs-02": 4 * np.pi, + "rs-03": 4 * np.pi, "rs-04": 4 * np.pi, "rs-05": 4 * np.pi, "rs-06": 4 * np.pi, + "el-05": 4 * np.pi, +} + +MODEL_MIT_VELOCITY_TABLE = { + "rs-00": 50, "rs-01": 44, "rs-02": 44, + "rs-03": 50, "rs-04": 15, "rs-05": 33, "rs-06": 20, + "el-05": 50, +} + +MODEL_MIT_TORQUE_TABLE = { + "rs-00": 17, "rs-01": 17, "rs-02": 17, + "rs-03": 60, "rs-04": 120, "rs-05": 17, "rs-06": 60, + "el-05": 6, +} + +MODEL_MIT_KP_TABLE = { + "rs-00": 500.0, "rs-01": 500.0, "rs-02": 500.0, + "rs-03": 5000.0, "rs-04": 5000.0, "rs-05": 500.0, "rs-06": 5000.0, + "el-05": 500.0, +} + +MODEL_MIT_KD_TABLE = { + "rs-00": 5.0, "rs-01": 5.0, "rs-02": 5.0, + "rs-03": 100.0, "rs-04": 100.0, "rs-05": 5.0, "rs-06": 100.0, + "el-05": 5.0, +} + +def get_pack_format(param_type): + """ + 获取 struct.pack 的格式字符串和字节大小 + + 说明: + - Type 17/18 参数读写使用小端序 + - CANopen SDO 数据同样通常按小端序解释 (取决于实现) + """ + if param_type == ParamType.UINT8: + return ' 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(' 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(' 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 \ No newline at end of file diff --git a/05_software/real/sim2real_v2/vendored/odin1_imu/include/lidar_api_type.h b/05_software/real/sim2real_v2/vendored/odin1_imu/include/lidar_api_type.h new file mode 100644 index 0000000..4d127ae --- /dev/null +++ b/05_software/real/sim2real_v2/vendored/odin1_imu/include/lidar_api_type.h @@ -0,0 +1,242 @@ +/* +Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co) +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + http://www.apache.org/licenses/LICENSE-2.0 +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ +#ifndef LIDAR_TYPES_H +#define LIDAR_TYPES_H + +#include +#include +#include + +#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 diff --git a/05_software/real/sim2real_v2/vendored/odin1_imu/include/odin1_imu_bridge.h b/05_software/real/sim2real_v2/vendored/odin1_imu/include/odin1_imu_bridge.h new file mode 100644 index 0000000..145928c --- /dev/null +++ b/05_software/real/sim2real_v2/vendored/odin1_imu/include/odin1_imu_bridge.h @@ -0,0 +1,159 @@ +#ifndef ODIN1_IMU_BRIDGE_H +#define ODIN1_IMU_BRIDGE_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * 输入: 无 + * 输出: odin1_imu_sample_t + * 作用: 描述一帧 IMU 数据, 供 C/C++/Python 共享使用 + * 注意: 坐标系已从 SDK 重映射为 ROS 标准 (right-handed: x前 y左 z上) + * 重映射规则: accel_x←SDK_accel_y, accel_y←-SDK_accel_x, accel_z←SDK_accel_z + * gyro_x ←SDK_gyro_y, gyro_y ←-SDK_gyro_x, gyro_z ←SDK_gyro_z + */ +typedef struct odin1_imu_sample_t { + float accel_x; /* m/s², ROS 标准坐标系 */ + float accel_y; + float accel_z; + float gyro_x; /* rad/s, ROS 标准坐标系 */ + float gyro_y; + float gyro_z; + uint64_t stamp_ns; + uint64_t sequence; +} odin1_imu_sample_t; + +/** + * 输入: 无 + * 输出: odin1_odom_type_e + * 作用: 标识里程计数据类型 + */ +typedef enum { + ODIN1_ODOM_STANDARD = 0, /* 标准里程计, 含位置/姿态/速度/协方差 */ + ODIN1_ODOM_HIGHFREQ, /* 高频里程计, 含位置/姿态 */ + ODIN1_ODOM_TF, /* TF 变换(重定位后), 全局坐标系下位姿 */ +} odin1_odom_type_e; + +/** + * 输入: 无 + * 输出: odin1_odom_sample_t + * 作用: 描述一帧里程计数据, 统一容纳三种类型, 供 C/C++/Python 共享使用 + * 缩放: SDK 原始 int64 值已按 ÷1e6 转换为 double (位置:米, 姿态:四元数, 速度:m/s, rad/s) + * 注意: 速度与协方差仅 ODIN1_ODOM_STANDARD 有效, 其他类型为零 + */ +typedef struct odin1_odom_sample_t { + odin1_odom_type_e type; + uint64_t stamp_ns; + double pos_x; /* 米, odom 坐标系 */ + double pos_y; + double pos_z; + double orient_w; /* 单位四元数 */ + double orient_x; + double orient_y; + double orient_z; + double linear_vel_x; /* m/s (仅 STANDARD 有效) */ + double linear_vel_y; + double linear_vel_z; + double angular_vel_x; /* rad/s (仅 STANDARD 有效) */ + double angular_vel_y; + double angular_vel_z; + double pose_cov[36]; /* 位姿协方差 (仅 STANDARD 有效) */ + double twist_cov[36]; /* 速度协方差 (仅 STANDARD 有效) */ +} odin1_odom_sample_t; + +/** + * 输入: 无 + * 输出: const char* + * 作用: 返回当前 bridge 的版本字符串 + */ +const char* odin1_imu_version(void); + +/** + * 输入: timeout_ms[int] + * 输出: int, 0 表示成功, 非 0 表示失败 + * 作用: 初始化 SDK, 等待设备连接并开始 IMU 数据流 + */ +int odin1_imu_start(int timeout_ms); + +/** + * 输入: 无 + * 输出: 无 + * 作用: 停止数据流并释放 SDK 资源 + */ +void odin1_imu_stop(void); + +/** + * 输入: 无 + * 输出: int, 1 表示运行中, 0 表示未运行 + * 作用: 返回当前 bridge 是否处于运行状态 + */ +int odin1_imu_is_running(void); + +/** + * 输入: timeout_ms[int] + * 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常 + * 作用: 阻塞等待 IMU 数据到达 + */ +int odin1_imu_wait_for_data(int timeout_ms); + +/** + * 输入: out_sample[odin1_imu_sample_t*] + * 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常 + * 作用: 从内部队列中弹出一帧 IMU 数据 + */ +int odin1_imu_pop_sample(odin1_imu_sample_t* out_sample); + +/** + * 输入: out_sample[odin1_imu_sample_t*] + * 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常 + * 作用: 获取最近一帧 IMU 数据, 不会从队列中删除 + */ +int odin1_imu_get_latest(odin1_imu_sample_t* out_sample); + +/** + * 输入: 无 + * 输出: const char* + * 作用: 返回最近一次错误信息 + */ +const char* odin1_imu_last_error(void); + +// --------------------------------------------------------------------------- +// Odom (里程计) 接口 +// --------------------------------------------------------------------------- + +/** + * 输入: timeout_ms[int] + * 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常 + * 作用: 阻塞等待里程计数据到达（任意类型） + */ +int odin1_odom_wait_for_data(int timeout_ms); + +/** + * 输入: out_sample[odin1_odom_sample_t*] + * 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常 + * 作用: 从内部队列中弹出一帧里程计数据 + */ +int odin1_odom_pop_sample(odin1_odom_sample_t* out_sample); + +/** + * 输入: out_sample[odin1_odom_sample_t*] + * 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常 + * 作用: 获取最近一帧里程计数据, 不会从队列中删除 + */ +int odin1_odom_get_latest(odin1_odom_sample_t* out_sample); + +/** + * 输入: 无 + * 输出: const char* + * 作用: 返回最近一次里程计错误信息 + */ +const char* odin1_odom_last_error(void); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/05_software/real/sim2real_v2/vendored/odin1_imu/lib/liblydHostApi_arm.a b/05_software/real/sim2real_v2/vendored/odin1_imu/lib/liblydHostApi_arm.a new file mode 100644 index 0000000..67f027c Binary files /dev/null and b/05_software/real/sim2real_v2/vendored/odin1_imu/lib/liblydHostApi_arm.a differ diff --git a/05_software/real/sim2real_v2/vendored/odin1_imu/odin1_imu.py b/05_software/real/sim2real_v2/vendored/odin1_imu/odin1_imu.py new file mode 100644 index 0000000..4c007a1 --- /dev/null +++ b/05_software/real/sim2real_v2/vendored/odin1_imu/odin1_imu.py @@ -0,0 +1,230 @@ +#!/usr/bin/python3 +"""ODIN1 IMU / Odom ctypes 封装.""" + +from __future__ import annotations + +import ctypes +from enum import IntEnum +from pathlib import Path +from typing import Iterator, Optional + + +class Odin1ImuSample(ctypes.Structure): + """输入: 无; 输出: Odin1ImuSample; 作用: 映射 C++ bridge 的 IMU 结构体.""" + + _fields_ = [ + ("accel_x", ctypes.c_float), + ("accel_y", ctypes.c_float), + ("accel_z", ctypes.c_float), + ("gyro_x", ctypes.c_float), + ("gyro_y", ctypes.c_float), + ("gyro_z", ctypes.c_float), + ("stamp_ns", ctypes.c_uint64), + ("sequence", ctypes.c_uint64), + ] + + +class Odin1OdomType(IntEnum): + """输入: 无; 输出: Odin1OdomType; 作用: 匹配 C 端 odin1_odom_type_e.""" + + STANDARD = 0 + HIGHFREQ = 1 + TF = 2 + + +class Odin1OdomSample(ctypes.Structure): + """输入: 无; 输出: Odin1OdomSample; 作用: 映射 C++ bridge 的里程计结构体. + 位置: 米, 姿态: 单位四元数, 速度: m/s 和 rad/s, 协方差: 原始 double.""" + + _fields_ = [ + ("type", ctypes.c_int), + ("stamp_ns", ctypes.c_uint64), + ("pos_x", ctypes.c_double), + ("pos_y", ctypes.c_double), + ("pos_z", ctypes.c_double), + ("orient_w", ctypes.c_double), + ("orient_x", ctypes.c_double), + ("orient_y", ctypes.c_double), + ("orient_z", ctypes.c_double), + ("linear_vel_x", ctypes.c_double), + ("linear_vel_y", ctypes.c_double), + ("linear_vel_z", ctypes.c_double), + ("angular_vel_x", ctypes.c_double), + ("angular_vel_y", ctypes.c_double), + ("angular_vel_z", ctypes.c_double), + ("pose_cov", ctypes.c_double * 36), + ("twist_cov", ctypes.c_double * 36), + ] + + +class Odin1ImuClient: + """输入: lib_path[Optional[str|Path]]; 输出: Odin1ImuClient; 作用: 提供 Python 对 ODIN1 IMU bridge 的访问接口.""" + + def __init__(self, lib_path: Optional[str | Path] = None) -> None: + self._project_root = Path(__file__).resolve().parents[1] + resolved_path = Path(lib_path) if lib_path else self._project_root / "build" / "libodin1_imu_bridge.so" + self._lib = ctypes.CDLL(str(resolved_path)) + self._configure_signatures() + + def _configure_signatures(self) -> None: + """输入: 无; 输出: 无; 作用: 配置 ctypes 函数签名.""" + + self._lib.odin1_imu_version.restype = ctypes.c_char_p + + self._lib.odin1_imu_start.argtypes = [ctypes.c_int] + self._lib.odin1_imu_start.restype = ctypes.c_int + + self._lib.odin1_imu_stop.argtypes = [] + self._lib.odin1_imu_stop.restype = None + + self._lib.odin1_imu_is_running.argtypes = [] + self._lib.odin1_imu_is_running.restype = ctypes.c_int + + self._lib.odin1_imu_wait_for_data.argtypes = [ctypes.c_int] + self._lib.odin1_imu_wait_for_data.restype = ctypes.c_int + + self._lib.odin1_imu_pop_sample.argtypes = [ctypes.POINTER(Odin1ImuSample)] + self._lib.odin1_imu_pop_sample.restype = ctypes.c_int + + self._lib.odin1_imu_get_latest.argtypes = [ctypes.POINTER(Odin1ImuSample)] + self._lib.odin1_imu_get_latest.restype = ctypes.c_int + + self._lib.odin1_imu_last_error.argtypes = [] + self._lib.odin1_imu_last_error.restype = ctypes.c_char_p + + # ---- Odom ---- + self._lib.odin1_odom_wait_for_data.argtypes = [ctypes.c_int] + self._lib.odin1_odom_wait_for_data.restype = ctypes.c_int + + self._lib.odin1_odom_pop_sample.argtypes = [ctypes.POINTER(Odin1OdomSample)] + self._lib.odin1_odom_pop_sample.restype = ctypes.c_int + + self._lib.odin1_odom_get_latest.argtypes = [ctypes.POINTER(Odin1OdomSample)] + self._lib.odin1_odom_get_latest.restype = ctypes.c_int + + self._lib.odin1_odom_last_error.argtypes = [] + self._lib.odin1_odom_last_error.restype = ctypes.c_char_p + + def version(self) -> str: + """输入: 无; 输出: str; 作用: 获取 C++ bridge 版本号.""" + + return self._lib.odin1_imu_version().decode("utf-8") + + def last_error(self) -> str: + """输入: 无; 输出: str; 作用: 获取最近一次 bridge 错误信息.""" + + return self._lib.odin1_imu_last_error().decode("utf-8") + + def start(self, timeout_ms: int = 5000) -> None: + """输入: timeout_ms[int]; 输出: 无; 作用: 启动 IMU 数据接收.""" + + result = self._lib.odin1_imu_start(timeout_ms) + if result != 0: + raise RuntimeError(f"启动 ODIN1 IMU 失败: {self.last_error()} (code={result})") + + def stop(self) -> None: + """输入: 无; 输出: 无; 作用: 停止 IMU 数据接收.""" + + self._lib.odin1_imu_stop() + + def is_running(self) -> bool: + """输入: 无; 输出: bool; 作用: 返回 bridge 是否仍在运行.""" + + return bool(self._lib.odin1_imu_is_running()) + + def wait_for_data(self, timeout_ms: int = 1000) -> bool: + """输入: timeout_ms[int]; 输出: bool; 作用: 等待 IMU 数据到达（bridge 停止时返回 False 而非抛异常）.""" + + result = self._lib.odin1_imu_wait_for_data(timeout_ms) + if result < 0: + if not self.is_running(): + return False + raise RuntimeError(f"等待 IMU 数据失败: {self.last_error()} (code={result})") + return bool(result) + + def pop_sample(self) -> Optional[Odin1ImuSample]: + """输入: 无; 输出: Optional[Odin1ImuSample]; 作用: 从队列中取出一帧 IMU 数据.""" + + sample = Odin1ImuSample() + result = self._lib.odin1_imu_pop_sample(ctypes.byref(sample)) + if result < 0: + raise RuntimeError(f"读取 IMU 队列失败: {self.last_error()} (code={result})") + return sample if result == 1 else None + + def get_latest(self) -> Optional[Odin1ImuSample]: + """输入: 无; 输出: Optional[Odin1ImuSample]; 作用: 获取最近一帧 IMU 数据（无缝锁，永不等锁）.""" + + sample = Odin1ImuSample() + result = self._lib.odin1_imu_get_latest(ctypes.byref(sample)) + if result < 0: + raise RuntimeError(f"读取最新 IMU 数据失败: {self.last_error()} (code={result})") + return sample if result == 1 else None + + @property + def latest(self) -> Optional[Odin1ImuSample]: + """输入: 无; 输出: Optional[Odin1ImuSample]; 作用: get_latest 的属性形式，控制循环中推荐使用.""" + return self.get_latest() + + def iter_samples(self, timeout_ms: int = 1000) -> Iterator[Odin1ImuSample]: + """输入: timeout_ms[int]; 输出: Iterator[Odin1ImuSample]; 作用: 连续迭代输出 IMU 数据.""" + + while self.is_running(): + if not self.wait_for_data(timeout_ms): + continue + while True: + sample = self.pop_sample() + if sample is None: + break + yield sample + + # ---- Odom (里程计) 方法 ---- + + def odom_last_error(self) -> str: + """输入: 无; 输出: str; 作用: 获取最近一次里程计错误信息.""" + + return self._lib.odin1_odom_last_error().decode("utf-8") + + def odom_wait_for_data(self, timeout_ms: int = 1000) -> bool: + """输入: timeout_ms[int]; 输出: bool; 作用: 等待里程计数据到达（bridge 停止时返回 False 而非抛异常）.""" + + result = self._lib.odin1_odom_wait_for_data(timeout_ms) + if result < 0: + if not self.is_running(): + return False + raise RuntimeError(f"等待里程计数据失败: {self.odom_last_error()} (code={result})") + return bool(result) + + def odom_pop_sample(self) -> Optional[Odin1OdomSample]: + """输入: 无; 输出: Optional[Odin1OdomSample]; 作用: 从队列中取出一帧里程计数据.""" + + sample = Odin1OdomSample() + result = self._lib.odin1_odom_pop_sample(ctypes.byref(sample)) + if result < 0: + raise RuntimeError(f"读取里程计队列失败: {self.odom_last_error()} (code={result})") + return sample if result == 1 else None + + def odom_get_latest(self) -> Optional[Odin1OdomSample]: + """输入: 无; 输出: Optional[Odin1OdomSample]; 作用: 获取最近一帧里程计数据（无缝锁）.""" + + sample = Odin1OdomSample() + result = self._lib.odin1_odom_get_latest(ctypes.byref(sample)) + if result < 0: + raise RuntimeError(f"读取最新里程计数据失败: {self.odom_last_error()} (code={result})") + return sample if result == 1 else None + + @property + def odom_latest(self) -> Optional[Odin1OdomSample]: + """输入: 无; 输出: Optional[Odin1OdomSample]; 作用: odom_get_latest 的属性形式.""" + return self.odom_get_latest() + + def iter_odom(self, timeout_ms: int = 1000) -> Iterator[Odin1OdomSample]: + """输入: timeout_ms[int]; 输出: Iterator[Odin1OdomSample]; 作用: 连续迭代输出里程计数据.""" + + while self.is_running(): + if not self.odom_wait_for_data(timeout_ms): + continue + while True: + sample = self.odom_pop_sample() + if sample is None: + break + yield sample diff --git a/05_software/real/sim2real_v2/vendored/odin1_imu/src/odin1_imu_bridge.cpp b/05_software/real/sim2real_v2/vendored/odin1_imu/src/odin1_imu_bridge.cpp new file mode 100644 index 0000000..78458c1 --- /dev/null +++ b/05_software/real/sim2real_v2/vendored/odin1_imu/src/odin1_imu_bridge.cpp @@ -0,0 +1,655 @@ +#include "odin1_imu_bridge.h" + +#include "lidar_api.h" +#include "lidar_api_type.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +constexpr const char* kBridgeVersion = "0.2.0"; +constexpr std::size_t kMaxQueueSize = 1024; +constexpr std::size_t kMaxOdomQueueSize = 2048; +constexpr int kDefaultMode = LIDAR_MODE_SLAM; + +std::atomic g_running{false}; +std::atomic g_sdk_initialized{false}; +std::atomic g_device_connected{false}; +std::atomic 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 g_queue; +odin1_imu_sample_t g_latest_sample{}; +bool g_has_latest_sample = false; + +// 无缝锁 (seqlock) 专用：让 get_latest 与 SDK 回调完全无争抢 +std::atomic g_latest_seq{0}; +odin1_imu_sample_t g_latest_lockfree{}; + +std::mutex g_error_mutex; +std::string g_last_error = "bridge not started"; + +// ---- Odom (里程计) 专用状态 ---- +std::mutex g_odom_queue_mutex; +std::condition_variable g_odom_queue_cv; +std::deque g_odom_queue; +bool g_odom_enabled = false; + +// 无缝锁 (seqlock) 专用：让 get_latest 与 SDK 回调完全无争抢 +std::atomic g_odom_latest_seq{0}; +odin1_odom_sample_t g_odom_latest_lockfree{}; + +std::mutex g_odom_error_mutex; +std::string g_odom_last_error = "odom bridge not started"; + +/** + * 输入: message[const std::string&] + * 输出: 无 + * 作用: 线程安全地记录最近一次错误信息 + */ +void set_last_error(const std::string& message) { + std::lock_guard lock(g_error_mutex); + g_last_error = message; +} + +/** + * 输入: message[const std::string&] + * 输出: 无 + * 作用: 线程安全地记录最近一次 odom 错误信息 + */ +void set_odom_last_error(const std::string& message) { + std::lock_guard lock(g_odom_error_mutex); + g_odom_last_error = message; +} + +/** + * 输入: 无 + * 输出: 无 + * 作用: 清空内部 IMU 队列和最近一帧缓存 + */ +void clear_queue_locked_state() { + std::lock_guard lock(g_queue_mutex); + g_queue.clear(); + g_latest_sample = {}; + g_has_latest_sample = false; +} + +/** + * 输入: 无 + * 输出: 无 + * 作用: 清空内部 Odom 队列 + */ +void clear_odom_queue_locked_state() { + std::lock_guard lock(g_odom_queue_mutex); + g_odom_queue.clear(); +} + +/** + * 输入: raw_sample[const imu_convert_data_t*] + * 输出: odin1_imu_sample_t + * 作用: 将 SDK IMU 结构转换为 bridge 对外结构, 并重映射为 ROS 标准坐标系 + * 坐标系: x前 y左 z上 (right-handed) + * 重映射: accel_x←SDK_accel_y, accel_y←-SDK_accel_x (来源: host_sdk_sample.h publishImu) + */ +odin1_imu_sample_t convert_sample(const imu_convert_data_t* raw_sample) { + odin1_imu_sample_t converted{}; + if (raw_sample == nullptr) { + return converted; + } + + // 加速度轴重映射 (SDK → ROS 标准) + converted.accel_x = raw_sample->accel_y; + converted.accel_y = -raw_sample->accel_x; + converted.accel_z = raw_sample->accel_z; + // 陀螺仪轴重映射 + converted.gyro_x = raw_sample->gyro_y; + converted.gyro_y = -raw_sample->gyro_x; + converted.gyro_z = raw_sample->gyro_z; + converted.stamp_ns = raw_sample->stamp; + converted.sequence = raw_sample->sequence; + return converted; +} + +/** + * 输入: raw[const ros_odom_convert_complete_t*] + * 输出: odin1_odom_sample_t + * 作用: 将 SDK 标准里程计结构转换为 bridge 对外结构 + * 缩放因子: ÷1e6 (来源: host_sdk_sample.h publishOdometry) + */ +odin1_odom_sample_t convert_odom_standard(const ros_odom_convert_complete_t* raw) { + odin1_odom_sample_t out{}; + if (raw == nullptr) return out; + out.type = ODIN1_ODOM_STANDARD; + out.stamp_ns = raw->timestamp_ns; + out.pos_x = static_cast(raw->pos[0]) / 1e6; + out.pos_y = static_cast(raw->pos[1]) / 1e6; + out.pos_z = static_cast(raw->pos[2]) / 1e6; + out.orient_w = static_cast(raw->orient[0]) / 1e6; + out.orient_x = static_cast(raw->orient[1]) / 1e6; + out.orient_y = static_cast(raw->orient[2]) / 1e6; + out.orient_z = static_cast(raw->orient[3]) / 1e6; + out.linear_vel_x = static_cast(raw->linear_velocity[0]) / 1e6; + out.linear_vel_y = static_cast(raw->linear_velocity[1]) / 1e6; + out.linear_vel_z = static_cast(raw->linear_velocity[2]) / 1e6; + out.angular_vel_x = static_cast(raw->angular_velocity[0]) / 1e6; + out.angular_vel_y = static_cast(raw->angular_velocity[1]) / 1e6; + out.angular_vel_z = static_cast(raw->angular_velocity[2]) / 1e6; + for (int i = 0; i < 36; ++i) { + out.pose_cov[i] = raw->pose_cov[i]; + out.twist_cov[i] = raw->twist_cov[i]; + } + return out; +} + +/** + * 输入: raw[const ros2_odom_convert_t*], type[odin1_odom_type_e] + * 输出: odin1_odom_sample_t + * 作用: 将 SDK 高频/TF 里程计结构转换为 bridge 对外结构 + * 缩放因子: ÷1e6 (来源: host_sdk_sample.h publishOdometry) + */ +odin1_odom_sample_t convert_odom_compact(const ros2_odom_convert_t* raw, odin1_odom_type_e type) { + odin1_odom_sample_t out{}; + if (raw == nullptr) return out; + out.type = type; + out.stamp_ns = raw->timestamp_ns; + out.pos_x = static_cast(raw->pos[0]) / 1e6; + out.pos_y = static_cast(raw->pos[1]) / 1e6; + out.pos_z = static_cast(raw->pos[2]) / 1e6; + out.orient_w = static_cast(raw->orient[0]) / 1e6; + out.orient_x = static_cast(raw->orient[1]) / 1e6; + out.orient_y = static_cast(raw->orient[2]) / 1e6; + out.orient_z = static_cast(raw->orient[3]) / 1e6; + return out; +} + +/** + * 输入: 无 + * 输出: 无 + * 作用: 安全关闭当前设备与 SDK 资源 + */ +void cleanup_device_and_sdk() { + std::lock_guard lock(g_state_mutex); + + if (g_device != nullptr) { + if (g_stream_started.load()) { + lidar_deactivate_stream_type(g_device, LIDAR_DT_RAW_IMU); // SDK接口，来源: include/lidar_api.h + if (g_odom_enabled) { + lidar_deactivate_stream_type(g_device, LIDAR_DT_SLAM_ODOMETRY); // SDK接口 + lidar_deactivate_stream_type(g_device, LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ); // SDK接口 + lidar_deactivate_stream_type(g_device, LIDAR_DT_SLAM_ODOMETRY_TF); // SDK接口 + } + lidar_stop_stream(g_device, kDefaultMode); // SDK接口，来源: include/lidar_api.h + g_stream_started = false; + } + + lidar_unregister_stream_callback(g_device); // SDK接口，来源: include/lidar_api.h + lidar_close_device(g_device); // SDK接口，来源: include/lidar_api.h + lidar_destory_device(g_device); // SDK接口，来源: include/lidar_api.h + g_device = nullptr; + } + + if (g_sdk_initialized.load()) { + lidar_system_deinit(); // SDK接口，来源: include/lidar_api.h + g_sdk_initialized = false; + } + + g_device_connected = false; +} + +/** + * 输入: data[const lidar_data_t*], user_data[void*] + * 输出: 无 + * 作用: 接收 SDK 回调中的 IMU 数据并写入内部缓存队列 + */ +void lidar_data_callback(const lidar_data_t* data, void* user_data) { + (void)user_data; + + if (!g_running.load() || data == nullptr) { + return; + } + + // ---- IMU 处理 ---- + if (data->type == LIDAR_DT_RAW_IMU) { + if (data->stream.imageList[0].pAddr == nullptr) { + set_last_error("sdk imu callback returned null payload"); + return; + } + + const auto* raw_sample = + static_cast(data->stream.imageList[0].pAddr); + odin1_imu_sample_t sample = convert_sample(raw_sample); + + { + std::lock_guard lock(g_queue_mutex); + if (g_queue.size() >= kMaxQueueSize) { + g_queue.pop_front(); + } + g_queue.push_back(sample); + g_latest_sample = sample; + g_has_latest_sample = true; + } + + // 无缝锁写入：Python get_latest 可同时读取，永不等锁 + { + std::uint64_t seq = g_latest_seq.fetch_add(1, std::memory_order_acquire) + 1; + g_latest_lockfree = sample; + g_latest_seq.store(seq + 1, std::memory_order_release); + } + + g_queue_cv.notify_all(); + return; + } + + // ---- Odom 处理 ---- + if (g_odom_enabled) { + odin1_odom_sample_t odom_sample{}; + bool valid = false; + + switch (data->type) { + case LIDAR_DT_SLAM_ODOMETRY: { + if (data->stream.imageList[0].pAddr == nullptr) { + set_odom_last_error("sdk standard odom callback returned null payload"); + return; + } + const auto* raw = + static_cast(data->stream.imageList[0].pAddr); + odom_sample = convert_odom_standard(raw); + valid = true; + break; + } + case LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ: { + if (data->stream.imageList[0].pAddr == nullptr) { + set_odom_last_error("sdk high-freq odom callback returned null payload"); + return; + } + const auto* raw = + static_cast(data->stream.imageList[0].pAddr); + odom_sample = convert_odom_compact(raw, ODIN1_ODOM_HIGHFREQ); + valid = true; + break; + } + case LIDAR_DT_SLAM_ODOMETRY_TF: { + if (data->stream.imageList[0].pAddr == nullptr) { + set_odom_last_error("sdk tf odom callback returned null payload"); + return; + } + const auto* raw = + static_cast(data->stream.imageList[0].pAddr); + odom_sample = convert_odom_compact(raw, ODIN1_ODOM_TF); + valid = true; + break; + } + default: + return; + } + + if (!valid) return; + + { + std::lock_guard lock(g_odom_queue_mutex); + if (g_odom_queue.size() >= kMaxOdomQueueSize) { + g_odom_queue.pop_front(); + } + g_odom_queue.push_back(odom_sample); + } + + // 无缝锁写入 + { + std::uint64_t seq = g_odom_latest_seq.fetch_add(1, std::memory_order_acquire) + 1; + g_odom_latest_lockfree = odom_sample; + g_odom_latest_seq.store(seq + 1, std::memory_order_release); + } + + g_odom_queue_cv.notify_all(); + return; + } + + return; +} + +/** + * 输入: device_info[const lidar_device_info_t*], attach[bool] + * 输出: 无 + * 作用: 响应 SDK 设备插拔事件并启动 IMU 数据流 + */ +void lidar_device_callback(const lidar_device_info_t* device_info, bool attach) { + if (!g_running.load()) { + return; + } + + if (!attach) { + g_device_connected = false; + g_stream_started = false; + return; + } + + if (device_info == nullptr) { + set_last_error("sdk device callback returned null device info"); + return; + } + + std::lock_guard lock(g_state_mutex); + + if (g_device != nullptr) { + return; + } + + device_handle device_handle_local = nullptr; + if (lidar_create_device(const_cast(device_info), &device_handle_local) != 0) { // SDK接口，来源: include/lidar_api.h + set_last_error("lidar_create_device failed"); + return; + } + + if (lidar_open_device(device_handle_local) != 0) { // SDK接口，来源: include/lidar_api.h + set_last_error("lidar_open_device failed"); + lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + return; + } + + lidar_data_callback_info_t callback_info{}; + callback_info.data_callback = lidar_data_callback; + callback_info.user_data = nullptr; + if (lidar_register_stream_callback(device_handle_local, callback_info) != 0) { // SDK接口，来源: include/lidar_api.h + set_last_error("lidar_register_stream_callback failed"); + lidar_close_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + return; + } + + uint32_t dtof_subframe_odr = 0; + if (lidar_start_stream(device_handle_local, kDefaultMode, dtof_subframe_odr) != 0) { // SDK接口，来源: include/lidar_api.h + (void)dtof_subframe_odr; + set_last_error("lidar_start_stream failed"); + lidar_unregister_stream_callback(device_handle_local); // SDK接口，来源: include/lidar_api.h + lidar_close_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + return; + } + + if (lidar_activate_stream_type(device_handle_local, LIDAR_DT_RAW_IMU) != 0) { // SDK接口，来源: include/lidar_api.h + set_last_error("lidar_activate_stream_type(raw_imu) failed"); + lidar_stop_stream(device_handle_local, kDefaultMode); // SDK接口，来源: include/lidar_api.h + lidar_unregister_stream_callback(device_handle_local); // SDK接口，来源: include/lidar_api.h + lidar_close_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + return; + } + + // 激活里程计数据流（STANDARD 类型，SDK 会自动启用 HIGHFREQ 和 TF） + // 参考: official ROS driver 只激活 LIDAR_DT_SLAM_ODOMETRY + if (lidar_activate_stream_type(device_handle_local, LIDAR_DT_SLAM_ODOMETRY) != 0) { // SDK接口 + set_odom_last_error("lidar_activate_stream_type(slam_odometry) failed, odom disabled"); + g_odom_enabled = false; + fprintf(stderr, "[bridge] WARNING: odom stream activation failed, odom disabled\n"); + } else { + g_odom_enabled = true; + set_odom_last_error(""); + clear_odom_queue_locked_state(); + fprintf(stdout, "[bridge] odom stream activated (STANDARD + HIGHFREQ + TF)\n"); + } + + g_device = device_handle_local; + g_stream_started = true; + g_device_connected = true; + set_last_error(""); + g_queue_cv.notify_all(); +} + +} // namespace + +extern "C" { + +/** + * 输入: 无 + * 输出: const char* + * 作用: 返回当前 bridge 的版本字符串 + */ +const char* odin1_imu_version(void) { + return kBridgeVersion; +} + +/** + * 输入: timeout_ms[int] + * 输出: int, 0 表示成功, 非 0 表示失败 + * 作用: 初始化 SDK, 等待设备连接并开始 IMU 数据流 + */ +int odin1_imu_start(int timeout_ms) { + if (timeout_ms <= 0) { + timeout_ms = 5000; + } + + if (g_running.load()) { + return 0; + } + + clear_queue_locked_state(); + set_last_error("waiting for odin1 device"); + + if (lidar_system_init(lidar_device_callback) != 0) { // SDK接口，来源: include/lidar_api.h + set_last_error("lidar_system_init failed"); + return -1; + } + + g_sdk_initialized = true; + g_running = true; + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); + while (std::chrono::steady_clock::now() < deadline) { + if (g_device_connected.load()) { + return 0; + } + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + set_last_error("timeout waiting for odin1 imu stream"); + odin1_imu_stop(); + return -2; +} + +/** + * 输入: 无 + * 输出: 无 + * 作用: 停止数据流并释放 SDK 资源 + */ +void odin1_imu_stop(void) { + g_running = false; + g_odom_enabled = false; + cleanup_device_and_sdk(); + clear_queue_locked_state(); + clear_odom_queue_locked_state(); + g_queue_cv.notify_all(); + g_odom_queue_cv.notify_all(); +} + +/** + * 输入: 无 + * 输出: int, 1 表示运行中, 0 表示未运行 + * 作用: 返回当前 bridge 是否处于运行状态 + */ +int odin1_imu_is_running(void) { + return g_running.load() ? 1 : 0; +} + +/** + * 输入: timeout_ms[int] + * 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常 + * 作用: 阻塞等待 IMU 数据到达 + */ +int odin1_imu_wait_for_data(int timeout_ms) { + if (!g_running.load()) { + return -1; + } + + std::unique_lock 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 lock(g_queue_mutex); + if (g_queue.empty()) { + return 0; + } + + *out_sample = g_queue.front(); + g_queue.pop_front(); + return 1; +} + +/** + * 输入: out_sample[odin1_imu_sample_t*] + * 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常 + * 作用: 获取最近一帧 IMU 数据, 不会从队列中删除 + */ +int odin1_imu_get_latest(odin1_imu_sample_t* out_sample) { + if (out_sample == nullptr) { + set_last_error("odin1_imu_get_latest received null output pointer"); + return -1; + } + + // 无缝锁读取：与 SDK 回调无锁争抢，延迟最低 + std::uint64_t before, after; + do { + before = g_latest_seq.load(std::memory_order_acquire); + if (before & 1) { + // 写者正在写，自旋等待（纳秒级，200Hz 写入下概率极低） + continue; + } + if (before == 0) { + // 还没有任何数据写入 + return 0; + } + *out_sample = g_latest_lockfree; + after = g_latest_seq.load(std::memory_order_acquire); + } while (before != after); + + return 1; +} + +/** + * 输入: 无 + * 输出: const char* + * 作用: 返回最近一次错误信息 + */ +const char* odin1_imu_last_error(void) { + std::lock_guard lock(g_error_mutex); + return g_last_error.c_str(); +} + +// --------------------------------------------------------------------------- +// Odom (里程计) 接口实现 +// --------------------------------------------------------------------------- + +/** + * 输入: timeout_ms[int] + * 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常 + * 作用: 阻塞等待里程计数据到达（任意类型） + */ +int odin1_odom_wait_for_data(int timeout_ms) { + if (!g_running.load()) { + return -1; + } + + std::unique_lock lock(g_odom_queue_mutex); + const bool ready = g_odom_queue_cv.wait_for( + lock, + std::chrono::milliseconds(timeout_ms > 0 ? timeout_ms : 1000), + [] { return !g_odom_queue.empty() || !g_running.load(); }); + + if (!g_running.load()) { + return -1; + } + + return ready && !g_odom_queue.empty() ? 1 : 0; +} + +/** + * 输入: out_sample[odin1_odom_sample_t*] + * 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常 + * 作用: 从内部队列中弹出一帧里程计数据 + */ +int odin1_odom_pop_sample(odin1_odom_sample_t* out_sample) { + if (out_sample == nullptr) { + set_odom_last_error("odin1_odom_pop_sample received null output pointer"); + return -1; + } + + std::lock_guard lock(g_odom_queue_mutex); + if (g_odom_queue.empty()) { + return 0; + } + + *out_sample = g_odom_queue.front(); + g_odom_queue.pop_front(); + return 1; +} + +/** + * 输入: out_sample[odin1_odom_sample_t*] + * 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常 + * 作用: 获取最近一帧里程计数据, 不会从队列中删除 + */ +int odin1_odom_get_latest(odin1_odom_sample_t* out_sample) { + if (out_sample == nullptr) { + set_odom_last_error("odin1_odom_get_latest received null output pointer"); + return -1; + } + + // 无缝锁读取：与 SDK 回调无锁争抢，延迟最低 + std::uint64_t before, after; + do { + before = g_odom_latest_seq.load(std::memory_order_acquire); + if (before & 1) { + continue; + } + if (before == 0) { + return 0; + } + *out_sample = g_odom_latest_lockfree; + after = g_odom_latest_seq.load(std::memory_order_acquire); + } while (before != after); + + return 1; +} + +/** + * 输入: 无 + * 输出: const char* + * 作用: 返回最近一次里程计错误信息 + */ +const char* odin1_odom_last_error(void) { + std::lock_guard lock(g_odom_error_mutex); + return g_odom_last_error.c_str(); +} + +} // extern "C" diff --git a/05_software/real/sim2real_v2/web/__init__.py b/05_software/real/sim2real_v2/web/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real_v2/web/server.py b/05_software/real/sim2real_v2/web/server.py new file mode 100644 index 0000000..6856e75 --- /dev/null +++ b/05_software/real/sim2real_v2/web/server.py @@ -0,0 +1,361 @@ +"""Minimal HTTP + SSE server for the sim2real web console.""" + +from __future__ import annotations + +import argparse +import json +import os +import queue +import sys +import threading +import time +import traceback +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from pathlib import Path +from urllib.parse import urlparse + +import yaml + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +from web.session import RobotSession # noqa: E402 + + +SESSION: "RobotSession" = None # type: ignore + + +def make_real_factory(): + def outer(): + sim2real_root = Path(__file__).resolve().parents[1] + workspace_root = sim2real_root.parent.parent + for path in (workspace_root,): + path_str = str(path) + if path_str not in sys.path and path.exists(): + sys.path.append(path_str) + + from dm_socket.sim2real_factory import driver_factory + + return driver_factory + + return outer + + +def make_dry_factory(): + def outer(): + class MockMotor: + def __init__(self): + class State: + position = 0.0 + velocity = 0.0 + torque = 0.0 + update_count = 0 + + self.state = State() + + class MockDriver: + def __init__(self, port, debug): + self.port = port + self.motors = {} + + def connect(self): + pass + + def disconnect(self): + pass + + def add_motor(self, name, motor_id, model): + self.motors[name] = MockMotor() + + def enable(self, name): + pass + + def disable(self, name): + pass + + def clear_warnings(self, name): + pass + + def process_messages(self): + for motor in self.motors.values(): + motor.state.update_count += 1 + + def control_mit(self, name, q, dq, kp, kd, tau): + if name in self.motors: + self.motors[name].state.position = q + self.motors[name].state.velocity = dq + self.motors[name].state.torque = tau + + def factory(can1_port, can2_port, debug): + return MockDriver(can1_port, debug), MockDriver(can2_port, debug) + + return factory + + return outer + + +def _send_json(handler: BaseHTTPRequestHandler, code: int, obj): + body = json.dumps(obj, ensure_ascii=False).encode("utf-8") + handler.send_response(code) + handler.send_header("Content-Type", "application/json; charset=utf-8") + handler.send_header("Content-Length", str(len(body))) + handler.send_header("Cache-Control", "no-store") + handler.end_headers() + handler.wfile.write(body) + + +def _send_static(handler: BaseHTTPRequestHandler, path: Path, content_type: str): + if not path.exists(): + handler.send_error(404, str(path)) + return + body = path.read_bytes() + handler.send_response(200) + handler.send_header("Content-Type", content_type) + handler.send_header("Content-Length", str(len(body))) + handler.send_header("Cache-Control", "no-store, no-cache, must-revalidate, max-age=0") + handler.send_header("Pragma", "no-cache") + handler.send_header("Expires", "0") + handler.end_headers() + handler.wfile.write(body) + + +class Handler(BaseHTTPRequestHandler): + server_version = "Sim2RealConsole/1.1" + + def log_message(self, fmt, *args): + if "GET /events" in (fmt % args): + return + super().log_message(fmt, *args) + + def do_GET(self): + url = urlparse(self.path) + if url.path in ("/", "/index.html"): + return _send_static(self, Path(__file__).parent / "static" / "index.html", "text/html; charset=utf-8") + if url.path == "/static/app.js": + return _send_static(self, Path(__file__).parent / "static" / "app.js", "application/javascript; charset=utf-8") + if url.path == "/static/style.css": + return _send_static(self, Path(__file__).parent / "static" / "style.css", "text/css; charset=utf-8") + if url.path.startswith("/static/viewer/"): + viewer_file = url.path.split("/static/viewer/", 1)[1] + viewer_path = Path(__file__).parent / "static" / "viewer" / viewer_file + content_type = "text/javascript" if not viewer_file.endswith(".css") else "text/css" + return _send_static(self, viewer_path, content_type) + if url.path.startswith("/meshes/"): + mesh_name = url.path.split("/meshes/", 1)[1] + mesh_path = Path(__file__).resolve().parents[1] / "mjcf" / "meshes" / mesh_name + if not mesh_path.exists(): + return self.send_error(404, f"mesh not found: {mesh_name}") + self.send_response(200) + self.send_header("Content-Type", "application/octet-stream") + self.send_header("Content-Length", str(mesh_path.stat().st_size)) + self.send_header("Cache-Control", "max-age=3600") + self.end_headers() + with open(mesh_path, "rb") as file_obj: + while True: + chunk = file_obj.read(64 * 1024) + if not chunk: + break + self.wfile.write(chunk) + return + if url.path.startswith("/mjcf/"): + mjcf_name = url.path.split("/mjcf/", 1)[1] + mjcf_path = Path(__file__).resolve().parents[1] / "mjcf" / mjcf_name + if not mjcf_path.exists(): + return self.send_error(404, f"mjcf not found: {mjcf_name}") + self.send_response(200) + self.send_header("Content-Type", "application/xml; charset=utf-8") + self.send_header("Content-Length", str(mjcf_path.stat().st_size)) + self.end_headers() + self.wfile.write(mjcf_path.read_bytes()) + return + if url.path == "/api/status": + return _send_json(self, 200, SESSION.get_status()) + if url.path == "/api/debug": + return _send_json(self, 200, SESSION.get_debug_snapshot()) + if url.path == "/api/logs": + return _send_json(self, 200, {"sessions": SESSION.list_logs()}) + if url.path.startswith("/api/logs/"): + parts = url.path.split("/") + if len(parts) >= 5: + session_id = parts[3] + filename = parts[4] + file_path = Path(SESSION.cfg.get("log_dir", "logs")) / session_id / filename + if file_path.exists() and filename in ("state.csv", "events.jsonl"): + self.send_response(200) + self.send_header( + "Content-Type", + "text/csv" if filename.endswith("csv") else "application/json", + ) + self.send_header("Content-Disposition", f'attachment; filename="{session_id}_{filename}"') + self.send_header("Content-Length", str(file_path.stat().st_size)) + self.end_headers() + with open(file_path, "rb") as file_obj: + while True: + chunk = file_obj.read(64 * 1024) + if not chunk: + break + self.wfile.write(chunk) + return + return self.send_error(404) + if url.path == "/events": + return self._handle_sse() + return self.send_error(404, self.path) + + def do_POST(self): + url = urlparse(self.path) + try: + length = int(self.headers.get("Content-Length", "0")) + body = self.rfile.read(length) if length else b"" + data = json.loads(body) if body else {} + except Exception as exc: + SESSION.note_api_error() + return _send_json(self, 400, {"error": f"bad body: {exc}"}) + + try: + result = self._handle_post(url.path, data) + except Exception as exc: + SESSION.note_api_error() + return _send_json( + self, + 500, + { + "error": f"{type(exc).__name__}: {exc}", + "traceback": traceback.format_exc(), + }, + ) + if result is None: + return self.send_error(404) + return _send_json(self, 200, {"ok": True, **(result if isinstance(result, dict) else {})}) + + def _handle_post(self, path: str, data: dict): + if path == "/api/connect": + return {"queued": SESSION.connect(dry_run=bool(data.get("dry_run", False)))} + if path == "/api/disconnect": + return {"queued": SESSION.disconnect()} + if path == "/api/enable": + return {"queued": SESSION.enable_motors()} + if path == "/api/disable": + return {"queued": SESSION.disable_motors()} + if path == "/api/test_motor": + return { + "queued": SESSION.test_motor( + leg=data["leg"], + joint=data["joint"], + delta_rad=float(data.get("delta_rad", 0.1)), + kp=float(data.get("kp", 5.0)), + kd=float(data.get("kd", 1.0)), + duration_s=float(data.get("duration_s", 1.0)), + ) + } + if path == "/api/calibrate_offsets": + return { + "queued": SESSION.calibrate_offsets( + target_pose_name=data.get("target_pose", "stand"), + samples=int(data.get("samples", 100)), + ) + } + if path == "/api/startup": + return {"queued": SESSION.startup()} + if path == "/api/runtime/start": + return {"queued": SESSION.runtime_start(policy_path=data.get("policy_path"))} + if path == "/api/runtime/stop": + return {"queued": SESSION.runtime_stop()} + if path == "/api/cmd": + SESSION.set_command( + vx=float(data.get("vx", 0.0)), + vy=float(data.get("vy", 0.0)), + yaw=float(data.get("yaw", 0.0)), + ) + return {} + if path == "/api/remote_takeover": + return {"ok": SESSION.set_remote_takeover(enabled=bool(data.get("enabled", False)))} + if path == "/api/estop": + SESSION.estop() + return {} + if path == "/api/reset_estop": + SESSION.reset_estop() + return {} + return None + + def _handle_sse(self): + self.send_response(200) + self.send_header("Content-Type", "text/event-stream") + self.send_header("Cache-Control", "no-cache") + self.send_header("Connection", "keep-alive") + self.send_header("Access-Control-Allow-Origin", "*") + self.end_headers() + + event_queue: "queue.Queue" = queue.Queue(maxsize=1024) + SESSION.add_listener(event_queue) + try: + initial = {"kind": "STATUS_FULL", **SESSION.get_status()} + self.wfile.write(f"data: {json.dumps(initial, ensure_ascii=False)}\n\n".encode()) + self.wfile.flush() + last_keepalive = time.time() + while True: + try: + event = event_queue.get(timeout=1.0) + self.wfile.write(f"data: {json.dumps(event, ensure_ascii=False)}\n\n".encode()) + self.wfile.flush() + except queue.Empty: + if time.time() - last_keepalive > 15: + self.wfile.write(b": keepalive\n\n") + self.wfile.flush() + last_keepalive = time.time() + except (BrokenPipeError, ConnectionResetError): + pass + finally: + SESSION.remove_listener(event_queue) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--port", type=int, default=8080) + parser.add_argument("--host", default="0.0.0.0") + parser.add_argument("--config", default=str(Path(__file__).resolve().parents[1] / "config.yaml")) + parser.add_argument("--dry-run", action="store_true") + args = parser.parse_args() + + cfg_path = Path(args.config) + with open(cfg_path, "r", encoding="utf-8") as file_obj: + cfg = yaml.safe_load(file_obj) + + global SESSION + SESSION = RobotSession( + cfg=cfg, + cfg_path=cfg_path, + driver_factory_real=make_real_factory(), + driver_factory_dry=make_dry_factory(), + ) + + def _pulse(): + while True: + try: + SESSION._broadcast({"kind": "PULSE", **SESSION.get_status()}) + except Exception: + pass + time.sleep(1.0) + + threading.Thread(target=_pulse, daemon=True).start() + + httpd = ThreadingHTTPServer((args.host, args.port), Handler) + print(f"\n[Web] sim2real console -> http://{args.host}:{args.port}\n") + try: + httpd.serve_forever() + except KeyboardInterrupt: + print("\n[Web] Ctrl+C received, shutting down...") + finally: + if SESSION is not None and SESSION.status.stage == "RUNTIME": + try: + SESSION.runtime_stop() + except Exception: + pass + try: + SESSION._do_disconnect() + except Exception: + pass + httpd.server_close() + os._exit(0) + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real_v2/web/session.py b/05_software/real/sim2real_v2/web/session.py new file mode 100644 index 0000000..c314b45 --- /dev/null +++ b/05_software/real/sim2real_v2/web/session.py @@ -0,0 +1,1630 @@ +"""Web-facing session state machine for current sim2real deployment.""" + +from __future__ import annotations + +import threading +import time +import traceback +import queue +from collections import deque +from dataclasses import asdict, dataclass, field +from enum import Enum +from pathlib import Path +from typing import Any, Callable, Dict, Optional + +import numpy as np + +from tools.logger import LogBundle +from input_dev.remote_uart import RemoteCommandSource + + +class Stage(str, Enum): + DISCONNECTED = "DISCONNECTED" + CONNECTING = "CONNECTING" + CONNECTED = "CONNECTED" + ENABLING = "ENABLING" + ENABLED = "ENABLED" + JOINT_TEST = "JOINT_TEST" + CALIBRATING = "CALIBRATING" + STARTING_UP = "STARTING_UP" + STAND_HOLD = "STAND_HOLD" + RUNTIME = "RUNTIME" + FAULTED = "FAULTED" + ESTOPPED = "ESTOPPED" + + +@dataclass +class SessionStatus: + stage: str = Stage.DISCONNECTED.value + detail: str = "" + last_event: str = "" + busy: bool = False + cmd: list = field(default_factory=lambda: [0.0, 0.0, 0.0]) + input_mode: str = "WEB" + remote_takeover_active: bool = False + remote_takeover_allowed: bool = False + remote_soft_estop: bool = False + remote_status: Dict[str, Any] = field(default_factory=dict) + last_state: Optional[Dict[str, Any]] = None + log_dir: Optional[str] = None + fault_reason: Optional[str] = None + last_error: Optional[str] = None + last_traceback: Optional[str] = None + diagnostics: Dict[str, Any] = field(default_factory=dict) + + +class RobotSession: + JOINT_LABELS = LogBundle.JOINT_LABELS + DEFAULT_ACTION_SCALE = np.array( + [ + 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, + ) + + def __init__( + self, + cfg: Dict[str, Any], + cfg_path: Path, + driver_factory_real: Callable, + driver_factory_dry: Callable, + ): + self.cfg = cfg + self.cfg_path = cfg_path + self.driver_factory_real = driver_factory_real + self.driver_factory_dry = driver_factory_dry + + self.lock = threading.RLock() + self.status = SessionStatus() + + self.io = None + self.runner = None + self.guard = None + self.safety = None + self.initializer = None + self.stand_balance = None + self.logger = None + + self._stop_runtime = threading.Event() + self._cmd_lock = threading.Lock() + self._cmd = np.zeros(3, dtype=np.float32) + self._filtered_cmd = np.zeros(3, dtype=np.float32) + self._last_raw_cmd = np.zeros(3, dtype=np.float32) + self._estop = False + self._busy_thread: Optional[threading.Thread] = None + self._stand_target = None + + self._remote_source: Optional[RemoteCommandSource] = None + self._remote_takeover_active: bool = False + self._remote_soft_estop: bool = False + self._remote_poll_error_count: int = 0 + self._remote_last_error: Optional[str] = None + + self._stop_poll = threading.Event() + self._poll_thread: Optional[threading.Thread] = None + + self._event_listeners: list = [] + self._recent_events = deque(maxlen=300) + self._listener_lock = threading.Lock() + + self._last_runtime_ts: float = 0.0 + self._last_poll_ts: float = 0.0 + self._last_command_ts: float = 0.0 + self._runtime_loop_count: int = 0 + self._runtime_overrun_count: int = 0 + self._runtime_overrun_max_ms: float = 0.0 + self._runtime_policy_stale_count: int = 0 + self._runtime_policy_stale_max_ms: float = 0.0 + self._last_loop_profile: Dict[str, float] = {} + self._latest_target_info: Dict[str, Any] = {} + self._latest_target: Optional[np.ndarray] = None + self._poll_error_count: int = 0 + self._api_error_count: int = 0 + self._disconnecting: bool = False + + self._target_queue = queue.Queue(maxsize=1) + self._state_lock = threading.Lock() + self._latest_hardware_state = None + self._policy_exception = None + self._motor_exception = None + self._status_exception = None + self._runtime_error_event = threading.Event() + + def _policy_action_scale(self) -> np.ndarray: + values = self.cfg.get("policy", {}).get("action_scale", self.DEFAULT_ACTION_SCALE.tolist()) + action_scale = np.asarray(values, dtype=np.float32) + if action_scale.shape != (16,): + raise ValueError(f"policy.action_scale must be 16 values, got shape {action_scale.shape}") + return action_scale + + def _diag_snapshot(self) -> Dict[str, Any]: + return { + "runtime_active": bool(self.status.stage == Stage.RUNTIME.value and not self._stop_runtime.is_set()), + "runtime_loop_count": int(self._runtime_loop_count), + "runtime_overrun_count": int(self._runtime_overrun_count), + "runtime_overrun_max_ms": round(float(self._runtime_overrun_max_ms), 3), + "runtime_policy_stale_count": int(self._runtime_policy_stale_count), + "runtime_policy_stale_max_ms": round(float(self._runtime_policy_stale_max_ms), 3), + "last_loop_profile": dict(self._last_loop_profile), + "latest_target": self._latest_target_snapshot(), + "filtered_cmd": self._filtered_cmd.tolist(), + "raw_cmd": self._last_raw_cmd.tolist(), + "last_runtime_age_s": round(time.time() - self._last_runtime_ts, 3) if self._last_runtime_ts else None, + "last_poll_age_s": round(time.time() - self._last_poll_ts, 3) if self._last_poll_ts else None, + "last_command_age_s": round(time.time() - self._last_command_ts, 3) if self._last_command_ts else None, + "poll_thread_alive": bool(self._poll_thread and self._poll_thread.is_alive()), + "busy_thread_alive": bool(self._busy_thread and self._busy_thread.is_alive()), + "estop": bool(self._estop), + "poll_error_count": int(self._poll_error_count), + "api_error_count": int(self._api_error_count), + "remote_poll_error_count": int(self._remote_poll_error_count), + "remote_last_error": self._remote_last_error, + "remote_takeover_active": bool(self._remote_takeover_active), + "remote_soft_estop": bool(self._remote_soft_estop), + "zero_cmd_suppression": ( + bool(getattr(self.runner, "enable_zero_cmd_suppression", False)) + if self.runner is not None + else None + ), + "policy_path": str(getattr(self.runner, "policy_path", "")) if self.runner is not None else None, + "stand_balance": self._stand_balance_snapshot(), + } + + def _stand_balance_snapshot(self) -> Dict[str, Any]: + if self.stand_balance is None: + return {"enabled": False} + debug = self.stand_balance.last_debug + return { + "enabled": bool(self.stand_balance.enabled), + "pitch_compensation_enabled": bool(getattr(debug, "pitch_compensation_enabled", False)), + "roll_deg": float(np.degrees(debug.roll)), + "pitch_deg": float(np.degrees(debug.pitch)), + "roll_rate_deg_s": float(np.degrees(debug.roll_rate)), + "pitch_rate_deg_s": float(np.degrees(debug.pitch_rate)), + "hip_base": float(debug.hip_base), + "knee_base": float(debug.knee_base), + "roll_corr": float(debug.roll_corr), + "pitch_corr": float(debug.pitch_corr), + "stable": bool(debug.stable), + } + + def _policy_release_cfg(self) -> Dict[str, float]: + policy_cfg = self.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(self, state: Dict[str, Any], hold_target: np.ndarray, cmd: np.ndarray) -> Dict[str, float]: + joint_pos = np.asarray(state["joint_pos"], dtype=np.float32) + default_pose = np.asarray(self.runner.default_dof_pos, dtype=np.float32) + hold_target = np.asarray(hold_target, dtype=np.float32) + planar_cmd, yaw_cmd = self.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( + self, + 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 * (self.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( + self, + state: Dict[str, Any], + 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 _record_loop_profile(self, profile: Dict[str, float]) -> Dict[str, float]: + compact = {k: round(float(v), 3) for k, v in profile.items()} + self._last_loop_profile = compact + return compact + + def _note_overrun(self, over_ms: float) -> None: + self._runtime_overrun_count += 1 + self._runtime_overrun_max_ms = max(self._runtime_overrun_max_ms, float(over_ms)) + + def _note_policy_stale(self, age_ms: float) -> None: + self._runtime_policy_stale_count += 1 + self._runtime_policy_stale_max_ms = max(self._runtime_policy_stale_max_ms, float(age_ms)) + + def _build_last_state( + self, + *, + state: Dict[str, Any], + target: np.ndarray, + raw: np.ndarray, + projected_gravity: np.ndarray, + cmd: np.ndarray, + loop_dt_ms: float, + phase: str, + safety_level: int = 0, + guard_level: int = 0, + safety_reason: str = "", + guard_reason: str = "", + zero_command: bool = True, + runtime_released: bool = False, + release_alpha: float = 0.0, + release_active_hold_s: float = 0.0, + release_max_hold_err: float = 0.0, + loop_profile: Optional[Dict[str, float]] = None, + extra: Optional[Dict[str, Any]] = None, + ) -> Dict[str, Any]: + motor_diag = state.get("motor_stale", {}) or {} + extra_dict = extra or {} + raw_cmd_list = extra_dict.get("raw_cmd", [0.0, 0.0, 0.0]) + filtered_cmd_list = cmd.tolist() + payload: Dict[str, Any] = { + "joint_pos": state["joint_pos"].tolist(), + "joint_vel": state["joint_vel"].tolist(), + "joint_torque": state["joint_torque"].tolist(), + "target": target.tolist(), + "raw": raw.tolist(), + "gyro": state["imu_gyro"].tolist(), + "proj_gravity": projected_gravity.tolist(), + "imu_age_ms": float(state["imu_age_ms"]), + "imu_fresh": bool(state.get("imu_fresh", False)), + "odom": state.get("odom"), + "loop_dt_ms": float(loop_dt_ms), + "loop_profile": loop_profile or {}, + "holdover_total": int(self.io.hw.holdover_total), + "safety_level": int(safety_level), + "guard_level": int(guard_level), + "phase": phase, + "cmd": cmd.tolist(), + "raw_cmd": raw_cmd_list, + "filtered_cmd": filtered_cmd_list, + "safety_reason": safety_reason, + "guard_reason": guard_reason, + "zero_command": bool(zero_command), + "runtime_released": bool(runtime_released), + "release_alpha": float(release_alpha), + "release_active_hold_s": float(release_active_hold_s), + "release_max_hold_err": float(release_max_hold_err), + "per_motor_stale": motor_diag.get("per_motor_stale", [0] * 16), + "motor_fresh_count": int(motor_diag.get("fresh_count", 16)), + "motor_fresh_by_update_count": int(motor_diag.get("fresh_by_update_count", 0)), + "motor_fresh_by_value_change": int(motor_diag.get("fresh_by_value_change", 0)), + "motor_update_counts": motor_diag.get("update_counts", [0] * 16), + "motor_temperatures": motor_diag.get("temperatures", [0.0] * 16), + "motor_fault_codes": motor_diag.get("fault_codes", [0] * 16), + "motor_mode_states": motor_diag.get("mode_states", [0] * 16), + "latest_target": self._latest_target_snapshot(), + } + if extra: + payload.update(extra) + return payload + + def _signal_stats(self, obs: Optional[np.ndarray], raw: Optional[np.ndarray], scaled: Optional[np.ndarray]) -> Dict[str, float]: + stats: Dict[str, float] = {} + if obs is not None and np.asarray(obs).size: + obs_arr = np.asarray(obs, dtype=np.float32) + stats.update( + { + "obs_min": float(np.min(obs_arr)), + "obs_max": float(np.max(obs_arr)), + "obs_abs_max": float(np.max(np.abs(obs_arr))), + } + ) + if raw is not None and np.asarray(raw).size: + raw_arr = np.asarray(raw, dtype=np.float32) + stats.update( + { + "raw_min": float(np.min(raw_arr)), + "raw_max": float(np.max(raw_arr)), + "raw_abs_max": float(np.max(np.abs(raw_arr))), + } + ) + if scaled is not None and np.asarray(scaled).size: + scaled_arr = np.asarray(scaled, dtype=np.float32) + stats.update( + { + "scaled_min": float(np.min(scaled_arr)), + "scaled_max": float(np.max(scaled_arr)), + "scaled_abs_max": float(np.max(np.abs(scaled_arr))), + } + ) + return stats + + def _update_latest_target(self, target: np.ndarray, source: str) -> None: + target = np.asarray(target, dtype=np.float32) + now = time.time() + delta_max = 0.0 + if self._latest_target is not None and self._latest_target.shape == target.shape: + delta_max = float(np.max(np.abs(target - self._latest_target))) + self._latest_target = target.copy() + self._latest_target_info = { + "source": source, + "t": now, + "delta_max": delta_max, + "target_abs_max": float(np.max(np.abs(target))) if target.size else 0.0, + } + + def _latest_target_snapshot(self) -> Dict[str, Any]: + if not self._latest_target_info: + return {} + snap = dict(self._latest_target_info) + snap["age_ms"] = (time.time() - float(snap.get("t", time.time()))) * 1000.0 + return snap + + def _filter_command(self, raw_cmd: np.ndarray, dt: float) -> np.ndarray: + raw_cmd = np.asarray(raw_cmd, dtype=np.float32) + self._last_raw_cmd = raw_cmd.copy() + cfg = self.cfg.get("command_filter", {}) or {} + if not bool(cfg.get("enabled", False)): + self._filtered_cmd = raw_cmd.copy() + return raw_cmd + + limits = np.array( + [ + float(cfg.get("max_vx_acc", 1.0)), + float(cfg.get("max_vy_acc", 1.0)), + float(cfg.get("max_yaw_acc", 1.5)), + ], + dtype=np.float32, + ) + max_delta = np.maximum(limits * max(float(dt), 1e-3), 0.0) + delta = np.clip(raw_cmd - self._filtered_cmd, -max_delta, max_delta) + self._filtered_cmd = (self._filtered_cmd + delta).astype(np.float32) + return self._filtered_cmd.copy() + + def _remote_cfg(self) -> Dict[str, Any]: + return dict(self.cfg.get("remote", {}) or {}) + + def _build_remote_source(self) -> Optional[RemoteCommandSource]: + remote_cfg = self._remote_cfg() + port = str(remote_cfg.get("port") or "").strip() + if not remote_cfg.get("enabled", False) or not port: + return None + return RemoteCommandSource( + port=port, + baudrate=int(remote_cfg.get("baudrate", 100000)), + timeout=float(remote_cfg.get("timeout", 0.02)), + axis_deadzone=int(remote_cfg.get("axis_deadzone", 50)), + active_threshold=int(remote_cfg.get("active_threshold", 50)), + axis_full_scale=float(remote_cfg.get("axis_full_scale", 660.0)), + max_vx=float(remote_cfg.get("max_vx", self.cfg["controller"]["max_vx"])), + max_vy=float(remote_cfg.get("max_vy", self.cfg["controller"]["max_vy"])), + max_yaw=float(remote_cfg.get("max_yaw_rate", self.cfg["controller"]["max_yaw_rate"])), + invert_vx=bool(remote_cfg.get("invert_vx", False)), + invert_vy=bool(remote_cfg.get("invert_vy", False)), + invert_yaw=bool(remote_cfg.get("invert_yaw", False)), + ) + + def _remote_takeover_allowed(self) -> bool: + return bool( + self.status.stage == Stage.RUNTIME.value + and self._remote_source is not None + ) + + def _refresh_input_status(self) -> None: + remote_status = {} + if self._remote_source is not None: + remote_status = self._remote_source.get_status() + remote_status["available"] = True + remote_status["last_error"] = self._remote_last_error + else: + remote_status = {"available": False, "last_error": self._remote_last_error} + self.status.input_mode = "REMOTE" if self._remote_takeover_active else "WEB" + self.status.remote_takeover_active = bool(self._remote_takeover_active) + self.status.remote_takeover_allowed = bool(self._remote_takeover_allowed()) + self.status.remote_soft_estop = bool(self._remote_soft_estop) + self.status.remote_status = remote_status + + def _poll_remote(self) -> None: + if self._remote_source is None: + self._remote_soft_estop = False + self._refresh_input_status() + return + try: + remote_state = self._remote_source.poll() + self._remote_last_error = None + self._remote_soft_estop = bool(remote_state.estop_requested) + except Exception as exc: + self._remote_poll_error_count += 1 + self._remote_last_error = f"{type(exc).__name__}: {exc}" + self._remote_soft_estop = False + if self._remote_poll_error_count <= 3 or self._remote_poll_error_count % 20 == 0: + self._broadcast({"kind": "REMOTE_POLL_ERROR", "error": self._remote_last_error}) + self._refresh_input_status() + + def _update_diag_locked(self) -> None: + self._refresh_input_status() + self.status.diagnostics = self._diag_snapshot() + + def note_api_error(self) -> None: + with self.lock: + self._api_error_count += 1 + self._update_diag_locked() + + def _set_fault(self, exc: Exception, tb: str) -> None: + error_text = f"{type(exc).__name__}: {exc}" + with self.lock: + self.status.fault_reason = error_text + self.status.last_error = error_text + self.status.last_traceback = tb + self._update_diag_locked() + + def get_status(self) -> Dict[str, Any]: + with self.lock: + self._update_diag_locked() + return asdict(self.status) + + def get_debug_snapshot(self) -> Dict[str, Any]: + return {"status": self.get_status(), "recent_events": list(self._recent_events)} + + def _set(self, **kwargs): + with self.lock: + for key, value in kwargs.items(): + setattr(self.status, key, value) + self._update_diag_locked() + snapshot = asdict(self.status) + self._broadcast({"kind": "STATUS", **snapshot}) + + def _set_stage(self, stage: Stage, detail: str = ""): + self._set(stage=stage.value, detail=detail) + + def _build_action_diag( + self, + *, + state: Dict[str, Any], + raw: np.ndarray, + scaled: np.ndarray, + tentative: np.ndarray, + cmd: np.ndarray, + zero_command: bool, + runtime_released: bool, + release_alpha: float, + safety_details: Optional[Dict[str, Any]] = None, + ) -> Dict[str, Any]: + details = dict(safety_details or {}) + joint_indices = list(details.get("joint_indices", [])) + joint_pos = np.asarray(state["joint_pos"], dtype=np.float32) + default_pose = np.asarray(self.runner.default_dof_pos, dtype=np.float32) + pos_err = tentative - joint_pos + leg_offset = tentative[:12] - default_pose[:12] + + diag: Dict[str, Any] = { + "joint_indices": joint_indices, + "joint_names": [self.JOINT_LABELS[i] for i in joint_indices if 0 <= i < len(self.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": self.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 add_listener(self, q: "queue.Queue"): + with self._listener_lock: + self._event_listeners.append(q) + for event in list(self._recent_events): + try: + q.put_nowait(event) + except Exception: + pass + + def remove_listener(self, q: "queue.Queue"): + with self._listener_lock: + if q in self._event_listeners: + self._event_listeners.remove(q) + + def _broadcast(self, event: Dict[str, Any]): + payload = dict(event) + payload["t"] = time.time() + self._recent_events.append(payload) + with self._listener_lock: + for listener in list(self._event_listeners): + try: + listener.put_nowait(payload) + except Exception: + pass + + def _run_async(self, fn, *args, **kwargs) -> bool: + with self.lock: + if self.status.busy: + return False + self.status.busy = True + self.status.fault_reason = None + self.status.last_error = None + self.status.last_traceback = None + self._update_diag_locked() + + def _wrap(): + try: + fn(*args, **kwargs) + except Exception as exc: + tb = traceback.format_exc() + print(f"\n[Background Task Error] {fn.__name__}") + print(tb) + self._set_fault(exc, tb) + self._broadcast({"kind": "BG_TASK_ERROR", "fn": fn.__name__, "error": str(exc), "traceback": tb}) + self._set_stage(Stage.FAULTED, detail=str(exc)) + try: + if self.io: + self.io.damping_brake() + except Exception: + pass + finally: + with self.lock: + self.status.busy = False + self._update_diag_locked() + self._broadcast({"kind": "BG_TASK_DONE", "fn": fn.__name__}) + + self._busy_thread = threading.Thread(target=_wrap, daemon=True) + self._busy_thread.start() + return True + + def connect(self, dry_run: bool = False): + return self._run_async(self._do_connect, dry_run) + + def _do_connect(self, dry_run: bool): + if self.status.stage != Stage.DISCONNECTED.value: + self._broadcast({"kind": "WARN", "msg": "already connected"}) + return + self._set_stage(Stage.CONNECTING, "connecting hardware") + + from interface.real_io import RealIO + from safety.runtime_guard import RuntimeGuard + from safety.safety_monitor import SafetyMonitor + from startup.pose_initializer import PoseInitializer + from startup.stand_balance import StandBalanceController + from tools.logger import LogBundle + + cfg = self.cfg + control_dt = 1.0 / float(cfg["control_freq"]) + motor_dt = 1.0 / float(cfg.get("motor_freq", 200)) + driver_factory = self.driver_factory_dry() if dry_run else self.driver_factory_real() + + self.logger = LogBundle(cfg["log_dir"]) + self._set(log_dir=str(self.logger.dir)) + self.logger.event( + "CONFIG_LOADED", + config_path=str(self.cfg_path), + dry_run=dry_run, + control_freq=cfg["control_freq"], + motor_model=cfg["motor_model"], + ) + + self.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, + motor_dt=motor_dt, + kp_leg=cfg["controller"]["kp_leg"], + kd_leg=cfg["controller"]["kd_leg"], + hold_kp_leg=cfg["controller"].get("hold_kp_leg", cfg["controller"]["kp_leg"]), + hold_kd_leg=cfg["controller"].get("hold_kd_leg", cfg["controller"]["kd_leg"]), + kd_wheel=cfg["controller"]["kd_wheel"], + debug=cfg.get("debug", False), + dry_run=dry_run, + ) + self.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), + ) + self.safety = SafetyMonitor( + max_target_offset=cfg["safety"]["max_target_offset"], + max_ang_vel=cfg["safety"]["max_ang_vel"], + max_tilt_z=cfg["safety"]["max_tilt_z"], + clip_to_brake=cfg["safety"].get("clip_to_brake", 0), + hard_target_offset=cfg["safety"].get("hard_target_offset", 1.2), + ) + self.initializer = PoseInitializer( + self.io, + control_dt=control_dt, + transition_time_min=cfg["startup"].get("transition_time_min", 2.0), + transition_time_max=cfg["startup"].get("transition_time_max", 6.0), + transition_seconds_per_rad=cfg["startup"].get("transition_seconds_per_rad", 1.5), + hold_time=cfg["startup"]["hold_time"], + settle_pos_threshold=cfg["startup"]["settle_pos_threshold"], + settle_vel_threshold=cfg["startup"]["settle_vel_threshold"], + timeout_extra=cfg["startup"].get("timeout_extra", 3.0), + imu_fresh_wait_s=cfg["startup"].get("imu_fresh_wait_s", 1.0), + progress_log_interval=cfg["startup"]["progress_log_interval"], + ramp_kp_time=cfg["startup"].get("ramp_kp_time", 1.0), + soft_hold_duration=cfg["startup"].get("soft_hold_duration", 1.0), + max_dev_warn=cfg["startup"].get("max_dev_warn", 1.5), + max_dev_abort=cfg["startup"].get("max_dev_abort", 3.0), + ) + self.stand_balance = StandBalanceController(cfg.get("stand_balance", {}), control_dt=control_dt) + self._remote_source = self._build_remote_source() + self._remote_takeover_active = False + self._remote_soft_estop = False + self._remote_last_error = None + + class _WebEstop: + def __init__(self, owner): + self.owner = owner + + def is_estop_triggered(self): + return self.owner._estop + + self.initializer.attach(self.logger, self.guard, _WebEstop(self)) + self.io.connect(imu_timeout_ms=cfg.get("imu_start_timeout_ms", 8000)) + if self._remote_source is not None: + try: + self._remote_source.open() + self.logger.event("REMOTE_CONNECTED", port=self._remote_source.port) + self._poll_remote() + except Exception as exc: + print(f"[RobotSession] 警告: 无法打开遥控器串口 {self._remote_source.port} ({exc})。已自动禁用遥控。") + self._remote_source = None + self.logger.event("CAN_IMU_CONNECTED", initial_gravity=self.io.imu.initial_gravity) + self._set_stage(Stage.CONNECTED, "hardware connected") + + def disconnect(self): + return self._run_async(self._do_disconnect) + + def _do_disconnect(self): + if self._disconnecting: + return + self._disconnecting = True + self._stop_runtime.set() + self._stop_state_poll() + time.sleep(0.05) + try: + if self.io: + self.io.damping_brake() + except Exception: + pass + try: + if self._remote_source: + self._remote_source.close() + except Exception: + pass + try: + if self._remote_source: + self._remote_source.close() + except Exception: + pass + try: + if self.io: + self.io.disconnect() + except Exception: + pass + if self.logger: + self.logger.event("HARDWARE_DISCONNECTED") + self.logger.close() + self.io = None + self.runner = None + self.logger = None + self._stand_target = None + self._remote_source = None + self._remote_takeover_active = False + self._remote_soft_estop = False + self._remote_last_error = None + self._set_stage(Stage.DISCONNECTED, "hardware disconnected") + self._disconnecting = False + + def enable_motors(self): + return self._run_async(self._do_enable) + + def _do_enable(self): + if self.status.stage not in (Stage.CONNECTED.value, Stage.STAND_HOLD.value, Stage.FAULTED.value): + return + self._set_stage(Stage.ENABLING, "enabling motors") + self.io.enable_motors() + self.logger.event("MOTORS_ENABLED") + time.sleep(0.5) + self._set_stage(Stage.ENABLED, "motors enabled") + self._start_state_poll() + + def _start_state_poll(self): + self._stop_poll.clear() + if self._poll_thread and self._poll_thread.is_alive(): + return + + def _poll_loop(): + while not self._stop_poll.is_set(): + if self.status.stage == Stage.RUNTIME.value: + time.sleep(0.2) + continue + try: + stage = self.status.stage + self._poll_remote() + if stage == Stage.ENABLED.value: + self.io.hw.passive_poll() + + state = self.io.read_state() + if stage == Stage.STAND_HOLD.value and self.stand_balance is not None and self.stand_balance.enabled: + self._stand_target = self.stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) + self.io.hold_pose(self._stand_target, kp_scale=1.0) + self._update_latest_target(self._stand_target, "stand_balance") + elif stage == Stage.STAND_HOLD.value and self._stand_target is not None: + self.io.hold_pose(self._stand_target, kp_scale=1.0) + self._update_latest_target(self._stand_target, "stand_hold") + + motor_diag = state.get("motor_stale", {}) + self._last_poll_ts = time.time() + self._set( + last_state={ + "joint_pos": state["joint_pos"].tolist(), + "joint_vel": state["joint_vel"].tolist(), + "joint_torque": state["joint_torque"].tolist(), + "target": self._stand_target.tolist() if self._stand_target is not None else [0.0] * 16, + "raw": [0.0] * 16, + "gyro": state["imu_gyro"].tolist(), + "proj_gravity": state["projected_gravity"].tolist(), + "imu_age_ms": float(state["imu_age_ms"]), + "loop_dt_ms": 0.0, + "holdover_total": int(getattr(self.io.hw, "holdover_total", 0)), + "safety_level": 0, + "guard_level": 0, + "phase": "POLL", + "stand_balance": self._stand_balance_snapshot(), + "latest_target": self._latest_target_snapshot(), + "per_motor_stale": motor_diag.get("per_motor_stale", [0] * 16), + } + ) + except Exception as exc: + self._poll_error_count += 1 + self._broadcast({"kind": "POLL_ERROR", "error": str(exc), "traceback": traceback.format_exc()}) + time.sleep(0.2) + + self._poll_thread = threading.Thread(target=_poll_loop, daemon=True) + self._poll_thread.start() + + def _stop_state_poll(self): + self._stop_poll.set() + thread = self._poll_thread + if thread and thread.is_alive() and thread is not threading.current_thread(): + thread.join(timeout=0.5) + self._poll_thread = None + + def disable_motors(self): + return self._run_async(self._do_disable) + + def _do_disable(self): + self._stop_state_poll() + try: + self.io.damping_brake() + except Exception: + pass + time.sleep(0.05) + self.io.disable_motors() + self.logger.event("MOTORS_DISABLED") + self._set_stage(Stage.CONNECTED, "motors disabled") + + def startup(self): + return self._run_async(self._do_startup) + + def _do_startup(self): + from startup.pose_initializer import PoseInitFailed, STAND_POSE + + if self.status.stage != Stage.ENABLED.value: + raise RuntimeError("startup requires ENABLED") + + self._set_stage(Stage.STARTING_UP, detail="transition to stand pose") + try: + target = self.initializer.transition_to_stand_from_current(target_pose=STAND_POSE) + self._stand_target = target + if self.stand_balance is not None and self.stand_balance.enabled: + self.logger.event("STAND_BALANCE_BEGIN") + self.stand_balance.reset() + stable_deadline = time.perf_counter() + 6.0 + while time.perf_counter() < stable_deadline: + state = self.io.read_state() + self._stand_target = self.stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) + self.io.hold_pose(self._stand_target, kp_scale=1.0) + if self.stand_balance.is_stable(): + debug = self.stand_balance.last_debug + self.logger.event( + "STAND_BALANCE_STABLE", + roll_deg=float(np.degrees(debug.roll)), + pitch_deg=float(np.degrees(debug.pitch)), + pitch_corr=float(debug.pitch_corr), + pitch_compensation_enabled=bool(debug.pitch_compensation_enabled), + ) + break + time.sleep(1.0 / float(self.cfg["control_freq"])) + self.logger.event("STAND_BALANCE_END") + self._set_stage(Stage.STAND_HOLD, detail="stand-balance hold active") + else: + self._set_stage(Stage.STAND_HOLD, detail="holding stand pose with PD") + except PoseInitFailed as exc: + self.logger.event("POSE_INIT_FAILED", error=str(exc)) + try: + self.io.damping_brake() + except Exception: + pass + self._set_stage(Stage.FAULTED, detail=str(exc)) + raise + + def _run_policy_loop_wrap(self, control_dt: float, require_active_command: bool): + from safety.runtime_guard import GuardLevel + from safety.safety_monitor import SafetyLevel + try: + next_exec = time.perf_counter() + runtime_released = not require_active_command + release_cfg = self._policy_release_cfg() + release_active_time = 0.0 + release_block_reason = "active command required" if require_active_command else "" + + while not self._stop_runtime.is_set() and not self._runtime_error_event.is_set(): + if getattr(self, "_debug_hang_policy", False): + time.sleep(0.1) + continue + with self._cmd_lock: + web_cmd = self._cmd.copy() + self._poll_remote() + raw_cmd = self._remote_source.get_command() if self._remote_takeover_active and self._remote_source is not None else web_cmd + cmd = self._filter_command(raw_cmd, control_dt) + + with self._state_lock: + state = self._latest_hardware_state + + if state is None: + next_exec += control_dt + slack = next_exec - time.perf_counter() + if slack > 0: + time.sleep(slack) + continue + + obs = self.io.get_obs_policy(state, cmd, self.runner.default_dof_pos, self.runner.last_actions) + zero_command = self.runner._is_zero_command(cmd, state["imu_gyro"]) + + if np.any(np.isnan(obs)) or np.any(np.isinf(obs)): + raise ValueError("Observation vector contains NaN or Inf values") + + if self._remote_takeover_active and self._remote_soft_estop: + raise RuntimeError("Remote soft estop triggered") + + if not runtime_released and zero_command: + raw = np.zeros(16, dtype=np.float32) + scaled = np.zeros(16, dtype=np.float32) + target_hold = self.stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if self.stand_balance is not None and self.stand_balance.enabled else self.runner.default_dof_pos.copy() + tentative = target_hold.copy().astype(np.float32) + policy_target = self.runner.default_dof_pos.copy() + release_active_time = 0.0 + release_block_reason = "zero command" + runtime_blend_ratio = 0.0 + else: + target_hold = self.stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if self.stand_balance is not None and self.stand_balance.enabled else self.runner.default_dof_pos.copy() + release_metrics = self._compute_release_metrics(state, target_hold, cmd) + if not runtime_released: + release_active_time += control_dt if self.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 + self.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}") + release_block_reason = ",".join(reasons) + + if not runtime_released: + raw = np.zeros(16, dtype=np.float32) + scaled = np.zeros(16, dtype=np.float32) + tentative = target_hold.copy().astype(np.float32) + policy_target = self.runner.default_dof_pos.copy() + runtime_blend_ratio = 0.0 + else: + scaled, raw = self.runner.step(obs, control_dt) + if np.any(np.isnan(raw)) or np.any(np.isinf(raw)): + raise ValueError("Policy action contains NaN or Inf values") + policy_target = (scaled + self.runner.default_dof_pos).astype(np.float32) + tentative = self._blend_runtime_target( + target_hold, + policy_target, + float(getattr(self.runner, "_command_release_alpha", 0.0)), + release_cfg["target_blend_s"], + control_dt, + ) + scaled = tentative - self.runner.default_dof_pos + runtime_blend_ratio = min( + 1.0, + float(getattr(self.runner, "_command_release_alpha", 0.0)) + * (self.runner.command_release_s / max(release_cfg["target_blend_s"], control_dt)), + ) + + joint_cmd = { + "time": time.perf_counter(), + "target": tentative, + "raw": raw, + "scaled": scaled, + "policy_target": policy_target, + "zero_command": zero_command, + "runtime_released": runtime_released, + "release_alpha": float(getattr(self.runner, "_command_release_alpha", 0.0)), + "release_active_time": release_active_time, + "release_block_reason": release_block_reason, + "runtime_blend_ratio": runtime_blend_ratio, + "obs": obs, + "cmd": cmd.copy(), + "raw_cmd": raw_cmd.copy(), + } + + try: + self._target_queue.put_nowait(joint_cmd) + except queue.Full: + try: + self._target_queue.get_nowait() + self._target_queue.put_nowait(joint_cmd) + except Exception: + pass + + next_exec += control_dt + slack = next_exec - time.perf_counter() + if slack > 0: + time.sleep(slack) + except Exception as exc: + self._policy_exception = exc + self._policy_traceback = traceback.format_exc() + self._runtime_error_event.set() + + def _run_motor_loop_wrap(self, motor_dt: float, policy_timeout_s: float, policy_stale_warn_s: float, log_every: int): + from safety.runtime_guard import GuardLevel + from safety.safety_monitor import SafetyLevel + try: + next_exec = time.perf_counter() + loop_count = 0 + last_target = None + last_target_time = 0.0 + last_stale_log_time = 0.0 + last_overrun_log_time = 0.0 + + self._last_runtime_ts = time.time() + self._runtime_loop_count = 0 + self._last_loop_dt_ms = 0.0 + self._last_loop_profile_snapshot = {} + + log_decimation = max(1, int(round(0.02 / motor_dt))) if motor_dt < 0.02 else 1 + + while not self._stop_runtime.is_set() and not self._runtime_error_event.is_set(): + loop_t0 = time.perf_counter() + loop_profile = {} + + def mark_profile(name: str) -> None: + nonlocal profile_last + now_profile = time.perf_counter() + loop_profile[name] = (now_profile - profile_last) * 1000.0 + profile_last = now_profile + + profile_last = loop_t0 + + state = self.io.read_state() + mark_profile("read_state_ms") + + with self._state_lock: + self._latest_hardware_state = state + + try: + joint_cmd = self._target_queue.get_nowait() + last_target = joint_cmd + last_target_time = joint_cmd["time"] + except queue.Empty: + pass + + current_time = time.perf_counter() + if last_target is None: + actual_target = self.io.hold_pose(self._stand_target, kp_scale=1.0) + self._update_latest_target(actual_target, "runtime_zero_hold") + mark_profile("hold_pose_ms") + + if loop_count * motor_dt > 2.0: + raise TimeoutError("Initial policy target wait timeout") + + raw = np.zeros(16, dtype=np.float32) + scaled = np.zeros(16, dtype=np.float32) + projected_gravity = state["projected_gravity"] + safety_level = 0 + guard_level = 0 + safety_reason = "" + guard_reason = "" + zero_command = True + runtime_released = False + release_alpha = 0.0 + release_active_time = 0.0 + release_max_hold_err = 0.0 + cmd = np.zeros(3, dtype=np.float32) + raw_cmd = np.zeros(3, dtype=np.float32) + extra = { + "raw_cmd": raw_cmd.tolist(), + } + else: + age_s = current_time - last_target_time + if age_s > policy_stale_warn_s: + age_ms = age_s * 1000.0 + self._note_policy_stale(age_ms) + if current_time - last_stale_log_time > 1.0: + self.logger.event("POLICY_TARGET_STALE", age_ms=age_ms, timeout_ms=policy_timeout_s * 1000.0) + last_stale_log_time = current_time + if age_s > policy_timeout_s: + self.logger.event("POLICY_TIMEOUT", age_ms=age_s * 1000.0) + raise TimeoutError(f"Policy target age {age_s*1000.0:.1f}ms exceeds safety limit {policy_timeout_s*1000.0:.1f}ms") + + tentative = last_target["target"] + raw = last_target["raw"] + scaled = last_target["scaled"] + policy_target = last_target["policy_target"] + zero_command = last_target["zero_command"] + runtime_released = last_target["runtime_released"] + release_alpha = last_target["release_alpha"] + release_active_time = last_target["release_active_time"] + obs = last_target["obs"] + cmd = last_target["cmd"] + raw_cmd = last_target["raw_cmd"] + runtime_blend_ratio = last_target["runtime_blend_ratio"] + + projected_gravity = state["projected_gravity"] + guard_decision = self.guard.check( + imu_gyro=state["imu_gyro"], + projected_gravity=projected_gravity, + imu_age_ms=float(state["imu_age_ms"]), + estop_triggered=self._estop, + extra_nan_arrays=(raw, tentative), + ) + mark_profile("safety_ms") + + if guard_decision.level == GuardLevel.STOP: + self.logger.event("GUARD_STOP", phase="RUNTIME", reason=guard_decision.reason) + raise RuntimeError(f"RuntimeGuard STOP: {guard_decision.reason}") + + safety_decision = self.safety.check( + target_pose=tentative, + default_pose=self.runner.default_dof_pos, + imu_gyro=state["imu_gyro"], + projected_gravity=projected_gravity, + estop_triggered=self._estop, + ) + + if safety_decision.level == SafetyLevel.ESTOP: + self.logger.event("SAFETY_ESTOP", reason=safety_decision.message) + raise RuntimeError(f"SafetyMonitor ESTOP: {safety_decision.message}") + if safety_decision.level == SafetyLevel.BRAKE: + safety_diag = self._build_action_diag( + state=state, + raw=raw, + scaled=scaled, + tentative=tentative, + cmd=cmd, + zero_command=zero_command, + runtime_released=runtime_released, + release_alpha=release_alpha, + safety_details=safety_decision.details, + ) + self.logger.event("SAFETY_BRAKE", reason=safety_decision.message, details=safety_diag) + raise RuntimeError(f"SafetyMonitor BRAKE: {safety_decision.message}") + + if safety_decision.level == SafetyLevel.CLIP and safety_decision.clipped_target is not None: + scaled = safety_decision.clipped_target - self.runner.default_dof_pos + tentative = safety_decision.clipped_target + + if runtime_released or not zero_command: + actual_target = self.io.send_actions(scaled, self.runner.default_dof_pos) + self._update_latest_target(actual_target, "runtime_policy") + mark_profile("send_actions_ms") + else: + actual_target = self.io.hold_pose(tentative, kp_scale=1.0) + self._update_latest_target(actual_target, "runtime_zero_hold") + mark_profile("hold_pose_ms") + + loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0 + self._last_runtime_ts = time.time() + self._runtime_loop_count += 1 + self._last_loop_dt_ms = loop_dt_ms + self._last_loop_profile_snapshot = loop_profile.copy() + + release_max_hold_err = float(np.max(np.abs(state["joint_pos"][:12] - tentative[:12]))) + target_metrics = self._compute_target_error_metrics(state, tentative, policy_target) + + extra = self._signal_stats(obs, raw, scaled) + extra.update({ + "raw_cmd": raw_cmd.tolist(), + "max_raw": float(np.max(np.abs(raw))), + "clip_joint_indices": (safety_decision.details or {}).get("joint_indices", []), + "clip_joint_names": (safety_decision.details or {}).get("joint_names", []), + "clip_primary_joint": (safety_decision.details or {}).get("primary_joint_name"), + "clip_primary_target": (safety_decision.details or {}).get("primary_target"), + "clip_primary_measured": (safety_decision.details or {}).get("primary_measured"), + "clip_primary_default": (safety_decision.details or {}).get("primary_default"), + "clip_primary_pos_err": (safety_decision.details or {}).get("primary_pos_err"), + "clip_primary_raw": (safety_decision.details or {}).get("primary_raw"), + "clip_primary_scaled": (safety_decision.details or {}).get("primary_scaled"), + }) + + safety_level = int(safety_decision.level) + guard_level = int(guard_decision.level) + safety_reason = safety_decision.message + guard_reason = guard_decision.reason + + if log_every and (loop_count % (log_every * log_decimation) == 0): + motor_diag = state.get("motor_stale", {}) or {} + stand_diag = self._stand_balance_snapshot() + self.logger.state( + phase="RUNTIME", + joint_pos=state["joint_pos"], + joint_vel=state["joint_vel"], + joint_torque=state["joint_torque"], + 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=safety_level, + guard_level=guard_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_leg_cmd=float(self.io.kp_leg), + kd_leg_cmd=float(self.io.kd_leg), + kd_wheel_cmd=float(self.io.kd_wheel), + runtime_release_alpha=release_alpha, + 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"], + stand_roll_deg=float(stand_diag.get("roll_deg", 0.0)), + stand_pitch_deg=float(stand_diag.get("pitch_deg", 0.0)), + stand_roll_corr=float(stand_diag.get("roll_corr", 0.0)), + stand_pitch_corr=float(stand_diag.get("pitch_corr", 0.0)), + stand_pitch_comp_enabled=bool(stand_diag.get("pitch_compensation_enabled", False)), + 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=safety_reason, + guard_reason=guard_reason, + ) + mark_profile("log_ms") + + with self._state_lock: + self._latest_motor_diagnostics = { + "actual_target": actual_target, + "raw": raw, + "scaled": scaled, + "projected_gravity": projected_gravity, + "safety_level": safety_level, + "guard_level": guard_level, + "safety_reason": safety_reason, + "guard_reason": guard_reason, + "zero_command": zero_command, + "runtime_released": runtime_released, + "release_alpha": release_alpha, + "release_active_time": release_active_time, + "release_max_hold_err": release_max_hold_err, + "extra": extra, + "cmd": cmd, + } + + loop_profile["total_ms"] = (time.perf_counter() - loop_t0) * 1000.0 + self._record_loop_profile(loop_profile) + + next_exec += motor_dt + slack = next_exec - time.perf_counter() + if slack > 0: + coarse = slack - 0.002 + if coarse > 0: + time.sleep(coarse) + micro_slack = next_exec - time.perf_counter() + if micro_slack > 0: + time.sleep(min(micro_slack, 0.001)) + elif slack < -motor_dt: + over_ms = -slack * 1000.0 + self._note_overrun(over_ms) + if time.perf_counter() - last_overrun_log_time > 0.5: + self.logger.event("LOOP_OVERRUN", over_ms=over_ms) + last_overrun_log_time = time.perf_counter() + next_exec = time.perf_counter() + + loop_count += 1 + except Exception as exc: + self._motor_exception = exc + self._motor_traceback = traceback.format_exc() + self._runtime_error_event.set() + + def _run_status_loop_wrap(self, status_dt: float): + try: + while not self._stop_runtime.is_set() and not self._runtime_error_event.is_set(): + start_time = time.perf_counter() + + with self._state_lock: + state = self._latest_hardware_state + diag = getattr(self, "_latest_motor_diagnostics", None) + + if state is not None and diag is not None: + loop_profile = getattr(self, "_last_loop_profile_snapshot", {}) + loop_dt_ms = getattr(self, "_last_loop_dt_ms", 0.0) + + self._set( + cmd=diag["cmd"].tolist(), + last_state=self._build_last_state( + state=state, + target=diag["actual_target"], + raw=diag["raw"], + projected_gravity=diag["projected_gravity"], + cmd=diag["cmd"], + loop_dt_ms=loop_dt_ms, + phase="RUNTIME", + safety_level=diag["safety_level"], + guard_level=diag["guard_level"], + safety_reason=diag["safety_reason"], + guard_reason=diag["guard_reason"], + zero_command=diag["zero_command"], + runtime_released=diag["runtime_released"], + release_alpha=diag["release_alpha"], + release_active_hold_s=diag["release_active_time"], + release_max_hold_err=diag["release_max_hold_err"], + loop_profile=loop_profile, + extra=diag["extra"], + ) + ) + + elapsed = time.perf_counter() - start_time + sleep_time = max(0.0, status_dt - elapsed) + time.sleep(sleep_time) + except Exception as exc: + self._status_exception = exc + self._status_traceback = traceback.format_exc() + self._runtime_error_event.set() + + def runtime_start(self, policy_path: Optional[str] = None): + return self._run_async(self._do_runtime_start, policy_path) + + def _do_runtime_start(self, policy_path: Optional[str]): + from policy.policy_runner import PolicyRunner, resolve_policy_path + from safety.runtime_guard import GuardLevel + from safety.safety_monitor import SafetyLevel + + if self.status.stage != Stage.STAND_HOLD.value: + raise RuntimeError("runtime start requires STAND_HOLD") + + sim2real_root = Path(__file__).resolve().parents[1] + resolved_policy = resolve_policy_path(policy_path, sim2real_root) + if not resolved_policy.exists(): + raise RuntimeError(f"policy not found: {resolved_policy}") + + self.runner = PolicyRunner( + resolved_policy, + enable_zero_cmd_suppression=self.cfg.get("policy", {}).get("enable_zero_cmd_suppression", True), + hold_zero_command_pose=self.cfg.get("policy", {}).get("hold_zero_command_pose", True), + command_release_s=self.cfg.get("policy", {}).get("command_release_s", 0.35), + action_scale=self._policy_action_scale(), + zero_cmd_use_yaw_rate=self.cfg.get("policy", {}).get("zero_cmd_use_yaw_rate", False), + clip_obs=self.cfg.get("policy", {}).get("clip_obs", 100.0), + ) + self.safety.reset() + require_active_command = self.cfg.get("policy", {}).get("require_active_command_to_release", True) + self.logger.event("POLICY_LOADED", path=str(resolved_policy)) + self._stop_state_poll() + + control_dt = 1.0 / float(self.cfg.get("policy_freq", 50)) + target = self._stand_target + + self.logger.event("PRIME_BEGIN") + zero_cmd = np.zeros(3, dtype=np.float32) + for index in range(1): + if self.stand_balance is not None and self.stand_balance.enabled: + state = self.io.read_state() + target = self.stand_balance.compute_target(state, zero_cmd) + self.io.hold_pose(target, kp_scale=1.0) + else: + self.io.hold_pose(target, kp_scale=1.0) + state = self.io.read_state() + obs = self.io.get_obs_policy(state, zero_cmd, self.runner.default_dof_pos, self.runner.last_actions) + if index == 0: + self.runner.reset(prime_obs=obs) + time.sleep(control_dt) + self.logger.event("PRIME_END") + + state = self.io.read_state() + projected_gravity = state["projected_gravity"] + if abs(projected_gravity[0]) > 0.5 or abs(projected_gravity[1]) > 0.5: + error_message = ( + f"IMU frame mismatch or body tilt too large: " + f"gravity projection X={projected_gravity[0]:.2f}, Y={projected_gravity[1]:.2f}" + ) + self.logger.event("GUARD_STOP", phase="STARTUP", reason=error_message) + self._set_stage(Stage.FAULTED, error_message) + return + + self.logger.event("HISTORY_PRIMED", initial_obs=obs) + self._stop_runtime.clear() + self._runtime_loop_count = 0 + self._runtime_overrun_count = 0 + self._runtime_overrun_max_ms = 0.0 + self._runtime_policy_stale_count = 0 + self._runtime_policy_stale_max_ms = 0.0 + self._filtered_cmd[:] = 0.0 + self._last_raw_cmd[:] = 0.0 + self._latest_target = None + self._latest_target_info = {} + self._remote_takeover_active = False + self._remote_soft_estop = False + self._set_stage(Stage.RUNTIME, detail="Decoupled policy & motor loop active") + self.logger.event("RUNTIME_BEGIN") + + # 启动解耦的多 Loop + # 1. 清空解耦队列与状态缓存 + while not self._target_queue.empty(): + try: + self._target_queue.get_nowait() + except Exception: + break + + self._latest_hardware_state = state + self._policy_exception = None + self._motor_exception = None + self._status_exception = None + self._runtime_error_event.clear() + + # 2. 读取配置频率 + motor_dt = 1.0 / float(self.cfg.get("motor_freq", 200)) + status_dt = 1.0 / float(self.cfg.get("status_freq", 10)) + policy_timeout_s = float(self.cfg.get("policy_timeout_ms", 150.0)) / 1000.0 + policy_stale_warn_s = float(self.cfg.get("policy_stale_warn_ms", 60.0)) / 1000.0 + log_every = int(self.cfg.get("log_every", 1)) + + # 3. 创建并启动线程 + policy_thread = threading.Thread( + target=self._run_policy_loop_wrap, + args=(control_dt, require_active_command), + name="PolicyLoop", + daemon=True + ) + motor_thread = threading.Thread( + target=self._run_motor_loop_wrap, + args=(motor_dt, policy_timeout_s, policy_stale_warn_s, log_every), + name="MotorLoop", + daemon=True + ) + status_thread = threading.Thread( + target=self._run_status_loop_wrap, + args=(status_dt,), + name="StatusLoop", + daemon=True + ) + + policy_thread.start() + motor_thread.start() + status_thread.start() + + # 4. 主工作监控忙等，直到用户触发 stop 或是线程抛出异常 + try: + while not self._stop_runtime.is_set() and not self._runtime_error_event.is_set(): + time.sleep(0.05) + + if self._runtime_error_event.is_set(): + for exc, tb, name in [ + (self._motor_exception, getattr(self, "_motor_traceback", ""), "MotorLoop"), + (self._policy_exception, getattr(self, "_policy_traceback", ""), "PolicyLoop"), + (self._status_exception, getattr(self, "_status_traceback", ""), "StatusLoop"), + ]: + if exc is not None: + raise exc + finally: + # 5. 确保通知所有工作线程退出并等待它们 + self._stop_runtime.set() + policy_thread.join(timeout=1.0) + motor_thread.join(timeout=1.0) + status_thread.join(timeout=1.0) + + if self.status.stage == Stage.RUNTIME.value: + self.logger.event("RUNTIME_STOP") + self._remote_takeover_active = False + self._remote_soft_estop = False + self._filtered_cmd[:] = 0.0 + self._last_raw_cmd[:] = 0.0 + self._latest_target = None + self._latest_target_info = {} + self._set_stage(Stage.STAND_HOLD, detail="runtime stopped, back to stand-balance hold") + self._start_state_poll() + + def runtime_stop(self): + self._stop_runtime.set() + return True + + def estop(self): + self._estop = True + try: + if self.io: + self.io.damping_brake() + except Exception: + pass + self._stop_runtime.set() + if self.logger: + self.logger.event("USER_ESTOP_WEB") + self._set_stage(Stage.ESTOPPED, detail="web emergency stop") + return True + + def reset_estop(self): + self._estop = False + self._set(detail="estop cleared") + return True + + def set_remote_takeover(self, enabled: bool): + enabled = bool(enabled) + if enabled: + if self.status.stage != Stage.RUNTIME.value: + raise RuntimeError("remote takeover requires RUNTIME") + if self._remote_source is None: + raise RuntimeError("remote controller not configured") + self._poll_remote() + self._remote_takeover_active = True + self._filtered_cmd[:] = 0.0 + self._last_raw_cmd[:] = 0.0 + if self.logger: + self.logger.event("REMOTE_TAKEOVER_ENABLED", port=self._remote_source.port) + self._set(detail="remote takeover enabled") + return True + + self._remote_takeover_active = False + self._remote_soft_estop = False + self._filtered_cmd[:] = 0.0 + self._last_raw_cmd[:] = 0.0 + if self.logger: + self.logger.event("REMOTE_TAKEOVER_DISABLED") + self._set(detail="remote takeover disabled") + return True + + def set_command(self, vx: float, vy: float, yaw: float): + with self._cmd_lock: + self._cmd[0] = float(vx) + self._cmd[1] = float(vy) + self._cmd[2] = float(yaw) + self._last_command_ts = time.time() + self._set(cmd=self._cmd.tolist()) + return True + + def test_motor(self, leg: str, joint: str, delta_rad: float, kp: float, kd: float, duration_s: float): + return self._run_async(self._do_test_motor, leg, joint, delta_rad, kp, kd, duration_s) + + def _do_test_motor(self, leg: str, joint: str, delta_rad: float, kp: float, kd: float, duration_s: float): + if self.io is None: + raise RuntimeError("hardware not connected") + if self.status.stage not in (Stage.ENABLED.value, Stage.STAND_HOLD.value, Stage.FAULTED.value): + raise RuntimeError("test motor requires ENABLED/STAND_HOLD/FAULTED") + + joint_key = (str(leg), str(joint)) + if joint_key not in self.io.hw.mapper.SIM_INDEX_MAP: + raise RuntimeError(f"unknown joint: {leg}_{joint}") + + idx = self.io.hw.mapper.SIM_INDEX_MAP[joint_key] + base_pose = self.io.read_measured_pose().astype(np.float32) + target_pose = base_pose.copy() + target_pose[idx] += float(delta_rad) + + prev_stage = self.status.stage + self._set_stage(Stage.JOINT_TEST, detail=f"testing {leg}_{joint}") + if self.logger: + self.logger.event( + "JOINT_TEST_BEGIN", + joint=f"{leg}_{joint}", + joint_index=idx, + delta_rad=float(delta_rad), + kp=float(kp), + kd=float(kd), + duration_s=float(duration_s), + start_pos=float(base_pose[idx]), + target_pos=float(target_pose[idx]), + ) + + self._stop_state_poll() + next_exec = time.perf_counter() + deadline = next_exec + max(float(duration_s), 0.1) + samples = [] + while time.perf_counter() < deadline: + state = self.io.read_state() + measured = float(state["joint_pos"][idx]) + error = float(target_pose[idx] - measured) + torque = float(state["joint_torque"][idx]) + vel = float(state["joint_vel"][idx]) + samples.append((measured, error, vel, torque)) + self.io.hw.send_control(target_pose, float(kp), float(kd), self.io.kd_wheel) + next_exec += 1.0 / float(self.cfg["control_freq"]) + slack = next_exec - time.perf_counter() + if slack > 0: + time.sleep(slack) + + self.io.hold_pose(base_pose, kp_scale=1.0) + final_state = self.io.read_state() + final_measured = float(final_state["joint_pos"][idx]) + if self.logger: + self.logger.event( + "JOINT_TEST_END", + joint=f"{leg}_{joint}", + joint_index=idx, + final_pos=final_measured, + final_err=float(target_pose[idx] - final_measured), + max_abs_err=float(max(abs(s[1]) for s in samples) if samples else 0.0), + max_abs_vel=float(max(abs(s[2]) for s in samples) if samples else 0.0), + max_abs_tau=float(max(abs(s[3]) for s in samples) if samples else 0.0), + ) + self._set(stage=prev_stage, detail=f"joint test {leg}_{joint} done") + if prev_stage in (Stage.ENABLED.value, Stage.STAND_HOLD.value): + self._start_state_poll() + + def list_logs(self): + log_root = Path(self.cfg.get("log_dir", "logs")) + if not log_root.exists(): + return [] + out = [] + for directory in sorted(log_root.iterdir(), reverse=True): + if not directory.is_dir(): + continue + state_path = directory / "state.csv" + events_path = directory / "events.jsonl" + out.append( + { + "id": directory.name, + "state_csv": state_path.exists(), + "events_jsonl": events_path.exists(), + "size_kb": ( + (state_path.stat().st_size + events_path.stat().st_size) // 1024 + if state_path.exists() and events_path.exists() + else 0 + ), + } + ) + return out diff --git a/05_software/real/sim2real_v2/web/static/app.js b/05_software/real/sim2real_v2/web/static/app.js new file mode 100644 index 0000000..b82e9ba --- /dev/null +++ b/05_software/real/sim2real_v2/web/static/app.js @@ -0,0 +1,570 @@ +const SIM_JOINT_ORDER = [ + ["fl", "hip_abduction"], ["fl", "hip_pitch"], ["fl", "knee"], + ["fr", "hip_abduction"], ["fr", "hip_pitch"], ["fr", "knee"], + ["rl", "hip_abduction"], ["rl", "hip_pitch"], ["rl", "knee"], + ["rr", "hip_abduction"], ["rr", "hip_pitch"], ["rr", "knee"], + ["fl", "wheel"], ["fr", "wheel"], ["rl", "wheel"], ["rr", "wheel"], +]; + +const $ = (id) => document.getElementById(id); +const PLOTS = {}; +let CURRENT_STATUS = null; +let SSE_CONN = null; +let SSE_RECONNECT_TIMER = null; +let LAST_RENDER_TS = 0; + +async function api(path, body = null) { + const options = { method: body ? "POST" : "GET" }; + if (body) { + options.headers = { "Content-Type": "application/json" }; + options.body = JSON.stringify(body); + } + const response = await fetch(path, options); + const payload = await response.json().catch(() => ({})); + if (!response.ok) { + throw new Error(payload.error || `HTTP ${response.status}`); + } + return payload; +} + +function safeText(value, fallback = "--") { + return value === undefined || value === null || Number.isNaN(value) ? fallback : value; +} + +function appendEvent(ev) { + const el = $("events-log"); + if (!el) return; + const item = document.createElement("div"); + let cls = "ev-name"; + if (/ERROR|STOP|NAN/.test(ev.kind || "")) cls = "ev-stop"; + else if (/FAULT|BRAKE/.test(ev.kind || "")) cls = "ev-fault"; + else if (/DONE|CONNECTED|ENABLED|PRIMED/.test(ev.kind || "")) cls = "ev-ok"; + const t = ev.t ? new Date(ev.t * 1000).toLocaleTimeString() : new Date().toLocaleTimeString(); + const detail = Object.entries(ev) + .filter(([k]) => !["t", "kind"].includes(k)) + .slice(0, 6) + .map(([k, v]) => `${k}=${typeof v === "number" ? v.toFixed(3) : JSON.stringify(v).slice(0, 80)}`) + .join(" "); + item.innerHTML = `${t} ${ev.kind} ${detail}`; + el.appendChild(item); + while (el.children.length > 300) el.removeChild(el.firstChild); + el.scrollTop = el.scrollHeight; +} + +function setStage(stage, detail) { + const el = $("stage"); + if (!el) return; + el.textContent = stage + (detail ? ` · ${detail}` : ""); + el.className = "stage " + stage; +} + +function setButtonEnabled(id, enabled) { + const el = $(id); + if (!el) return; + el.disabled = !enabled; +} + +function updateButtons(status) { + if (!status) return; + const stage = status.stage || "DISCONNECTED"; + const busy = !!status.busy; + const runtime = stage === "RUNTIME"; + const connected = stage !== "DISCONNECTED" && stage !== "CONNECTING"; + const enabled = ["ENABLED", "STARTING_UP", "STAND_HOLD", "RUNTIME"].includes(stage); + const canStartup = stage === "ENABLED"; + const canRuntimeStart = stage === "STAND_HOLD"; + const canRuntimeStop = runtime; + const remoteAllowed = !!status.remote_takeover_allowed; + const remoteActive = !!status.remote_takeover_active; + const runtimeUiBusy = busy && !runtime; + + setButtonEnabled("btn-connect", !runtimeUiBusy && stage === "DISCONNECTED"); + setButtonEnabled("btn-disconnect", !runtimeUiBusy && connected); + setButtonEnabled("btn-enable", !runtimeUiBusy && ["CONNECTED", "FAULTED"].includes(stage)); + setButtonEnabled("btn-disable", !runtimeUiBusy && enabled); + setButtonEnabled("btn-startup", !runtimeUiBusy && canStartup); + setButtonEnabled("btn-runtime-start", !runtimeUiBusy && canRuntimeStart); + setButtonEnabled("btn-runtime-stop", canRuntimeStop); + setButtonEnabled("btn-reset-estop", !runtimeUiBusy && stage === "ESTOPPED"); + setButtonEnabled("btn-estop", connected); + setButtonEnabled("btn-remote-release", runtime && remoteAllowed && !remoteActive); + setButtonEnabled("btn-remote-reclaim", runtime && remoteActive); +} + +function renderState(state) { + const el = $("state-summary"); + if (!el) return; + if (!state) { + el.innerHTML = '
STATUSNO DATA
'; + return; + } + const metric = (k, v, cls = "") => + `
${k}${v}
`; + const safetyText = ["NORMAL", "CLIP", "BRAKE", "ESTOP"][state.safety_level || 0]; + const guardText = ["NORMAL", "WARN", "STOP"][state.guard_level || 0] || "NORMAL"; + const imuCls = (state.imu_age_ms || 0) > 60 ? "bad" : (state.imu_age_ms || 0) > 30 ? "warn" : ""; + const dtCls = (state.loop_dt_ms || 0) > 25 ? "bad" : (state.loop_dt_ms || 0) > 22 ? "warn" : ""; + const gravityZ = state.proj_gravity?.[2] ?? -1; + const gravityCls = gravityZ > -0.5 ? "warn" : ""; + const rawMax = Math.max(...(state.raw || [0]).map((x) => Math.abs(x || 0))); + const odom = state.odom || null; + const odomLocal = odom?.local_pos || odom?.pos || [0, 0, 0]; + const odomText = odom ? `${odom.type || "ODOM"} ${(odom.age_ms || 0).toFixed(0)}ms ${odomLocal[0].toFixed(2)},${odomLocal[1].toFixed(2)}` : "no odom"; + const cmd = state.cmd || [0, 0, 0]; + const rawCmd = state.raw_cmd || cmd; + const stand = state.stand_balance || CURRENT_STATUS?.diagnostics?.stand_balance || {}; + const targetInfo = state.latest_target || CURRENT_STATUS?.diagnostics?.latest_target || {}; + const trackingErr = Math.max( + ...(state.joint_pos || []).slice(0, 12).map((pos, i) => Math.abs(pos - ((state.target || [])[i] || 0))), + 0, + ); + const inputModeEl = $("input-mode"); + if (inputModeEl) inputModeEl.textContent = CURRENT_STATUS?.input_mode || "WEB"; + const remoteAllowedEl = $("remote-allowed"); + if (remoteAllowedEl) remoteAllowedEl.textContent = CURRENT_STATUS?.remote_takeover_allowed ? "YES" : "NO"; + const remoteEstopEl = $("remote-estop"); + if (remoteEstopEl) remoteEstopEl.textContent = CURRENT_STATUS?.remote_soft_estop ? "ON" : "OFF"; + const remotePortEl = $("remote-port"); + if (remotePortEl) remotePortEl.textContent = CURRENT_STATUS?.remote_status?.port || "--"; + el.innerHTML = [ + metric("phase", safeText(state.phase, "?")), + metric("cmd", `${cmd.map((x) => (x || 0).toFixed(2)).join(",")}`), + metric("raw cmd", `${rawCmd.map((x) => (x || 0).toFixed(2)).join(",")}`), + metric("imu_age", `${(state.imu_age_ms || 0).toFixed(1)} ms`, imuCls), + metric("loop_dt", `${(state.loop_dt_ms || 0).toFixed(1)} ms`, dtCls), + metric("safety", safetyText, state.safety_level >= 2 ? "bad" : state.safety_level === 1 ? "warn" : ""), + metric("guard", guardText, state.guard_level >= 2 ? "bad" : state.guard_level === 1 ? "warn" : ""), + metric("holdover", String(state.holdover_total || 0)), + metric("raw max", rawMax.toFixed(2)), + metric("grav_z", gravityZ.toFixed(3), gravityCls), + metric("odom", odomText, odom && (odom.age_ms || 0) < 200 && !odom.jump_detected ? "" : "warn"), + metric("track_err", trackingErr.toFixed(3), trackingErr > 0.5 ? "bad" : trackingErr > 0.2 ? "warn" : ""), + metric("stand", stand.enabled ? `p ${((stand.pitch_deg) || 0).toFixed(1)} r ${((stand.roll_deg) || 0).toFixed(1)}` : "off", stand.enabled && !stand.pitch_compensation_enabled ? "warn" : ""), + metric("pitch_corr", `${((stand.pitch_corr) || 0).toFixed(3)} ${stand.pitch_compensation_enabled ? "on" : "off"}`), + metric("target_src", targetInfo.source ? `${targetInfo.source} ${(targetInfo.age_ms || 0).toFixed(0)}ms` : "--", targetInfo.age_ms > 100 ? "warn" : ""), + ].join(""); +} + +function renderDiagnostics(diag, state) { + if (!diag) return; + const setValue = (id, text, cls = "") => { + const el = $(id); + if (!el) return; + el.textContent = text; + el.className = "diag-value " + cls; + }; + setValue("diag-norm", "Aligned", "success"); + setValue("diag-latency", `${(state?.loop_dt_ms || 0).toFixed(1)} ms`, (state?.loop_dt_ms || 0) > 25 ? "danger" : (state?.loop_dt_ms || 0) > 22 ? "warning" : "success"); + const trackErr = Math.max( + ...(state?.joint_pos || []).slice(0, 12).map((pos, i) => Math.abs(pos - ((state?.target || [])[i] || 0))), + 0, + ); + setValue("diag-track-err", `${trackErr.toFixed(3)} rad`, trackErr > 0.5 ? "danger" : trackErr > 0.2 ? "warning" : "success"); + setValue("diag-runtime", diag.runtime_active ? "ACTIVE" : "IDLE", diag.runtime_active ? "success" : "warning"); + setValue("diag-runtime-age", diag.last_runtime_age_s == null ? "--" : `${diag.last_runtime_age_s.toFixed(2)} s`, diag.last_runtime_age_s != null && diag.last_runtime_age_s > 1.0 ? "danger" : "success"); + setValue("diag-poll-age", diag.last_poll_age_s == null ? "--" : `${diag.last_poll_age_s.toFixed(2)} s`, diag.last_poll_age_s != null && diag.last_poll_age_s > 1.0 ? "warning" : "success"); + setValue("diag-cmd-age", diag.last_command_age_s == null ? "--" : `${diag.last_command_age_s.toFixed(2)} s`); + setValue("diag-poll-errors", String(diag.poll_error_count || 0), (diag.poll_error_count || 0) > 0 ? "danger" : "success"); + setValue("diag-api-errors", String(diag.api_error_count || 0), (diag.api_error_count || 0) > 0 ? "warning" : "success"); + setValue("diag-overruns", `${diag.runtime_overrun_count || 0} / max ${(diag.runtime_overrun_max_ms || 0).toFixed(1)} ms`, (diag.runtime_overrun_count || 0) > 0 ? "warning" : "success"); + setValue("diag-policy-stale", `${diag.runtime_policy_stale_count || 0} / max ${(diag.runtime_policy_stale_max_ms || 0).toFixed(1)} ms`, (diag.runtime_policy_stale_count || 0) > 0 ? "warning" : "success"); + const profile = state?.loop_profile || diag.last_loop_profile || {}; + const slowest = Object.entries(profile) + .filter(([k]) => k !== "total_ms") + .sort((a, b) => (b[1] || 0) - (a[1] || 0))[0]; + const totalMs = profile.total_ms ?? state?.loop_dt_ms ?? 0; + setValue("diag-loop-profile", slowest ? `${totalMs.toFixed(1)} ms, slow ${slowest[0]}=${(slowest[1] || 0).toFixed(1)}` : "--"); + const targetInfo = state?.latest_target || diag.latest_target || {}; + if (targetInfo.source) { + setValue("diag-target", `${targetInfo.source} age ${(targetInfo.age_ms || 0).toFixed(0)}ms d ${(targetInfo.delta_max || 0).toFixed(3)}`, (targetInfo.age_ms || 0) > 100 ? "warning" : "success"); + } else { + setValue("diag-target", "--"); + } + const stand = state?.stand_balance || diag.stand_balance || {}; + if (stand.enabled) { + setValue( + "diag-stand", + `pitch ${((stand.pitch_deg) || 0).toFixed(2)}deg corr ${((stand.pitch_corr) || 0).toFixed(3)} ${stand.pitch_compensation_enabled ? "pitch-on" : "pitch-off"}`, + stand.pitch_compensation_enabled ? "warning" : "success", + ); + } else { + setValue("diag-stand", "disabled", "warning"); + } + const obsAbs = state?.obs_abs_max; + const rawAbs = state?.raw_abs_max; + const scaledAbs = state?.scaled_abs_max; + const signalText = obsAbs == null ? "--" : `obs ${obsAbs.toFixed(2)} raw ${(rawAbs || 0).toFixed(2)} scaled ${(scaledAbs || 0).toFixed(2)}`; + setValue("diag-signal", signalText, obsAbs > 80 || rawAbs > 9.5 ? "warning" : "success"); + const freshCount = state?.motor_fresh_count; + const byCount = state?.motor_fresh_by_update_count; + const byValue = state?.motor_fresh_by_value_change; + setValue("diag-motor-fresh", freshCount == null ? "--" : `${freshCount}/16 (cnt ${byCount || 0}, val ${byValue || 0})`, freshCount === 16 ? "success" : freshCount >= 12 ? "warning" : "danger"); + const odom = state?.odom; + if (odom) { + const p = odom.local_pos || odom.pos || [0, 0, 0]; + const yaw = odom.local_yaw == null ? 0 : odom.local_yaw; + const jump = odom.jump_detected ? " JUMP" : ""; + setValue("diag-odom", `${odom.type || "ODOM"} ${((odom.age_ms || 0)).toFixed(0)}ms x=${p[0].toFixed(2)} y=${p[1].toFixed(2)} yaw=${yaw.toFixed(2)}${jump}`, (odom.age_ms || 0) > 500 || odom.jump_detected ? "warning" : "success"); + } else { + setValue("diag-odom", "not available", "warning"); + } + setValue("diag-suppression", String(diag.zero_cmd_suppression), diag.zero_cmd_suppression ? "warning" : "success"); + const pathEl = $("diag-policy"); + if (pathEl) pathEl.textContent = diag.policy_path || "--"; +} + +function renderFault(status) { + const faultBox = $("fault-box"); + const faultText = $("fault-text"); + const traceText = $("traceback-text"); + if (!faultBox || !faultText || !traceText) return; + if (!status.fault_reason && !status.last_error) { + faultBox.classList.add("hidden"); + faultText.textContent = ""; + traceText.textContent = ""; + return; + } + faultBox.classList.remove("hidden"); + faultText.textContent = status.fault_reason || status.last_error || ""; + traceText.textContent = status.last_traceback || ""; +} + +function applyStatus(status) { + if (!status) return; + CURRENT_STATUS = { ...(CURRENT_STATUS || {}), ...status }; + const merged = CURRENT_STATUS; + if (merged.stage) setStage(merged.stage, merged.detail || ""); + if (merged.busy !== undefined && $("busy")) $("busy").textContent = merged.busy ? " [BUSY]" : ""; + if (merged.log_dir && $("logdir")) $("logdir").textContent = merged.log_dir; + updateButtons(merged); + renderFault(merged); + if (merged.last_state !== undefined) { + const now = performance.now(); + if (now - LAST_RENDER_TS > 80) { + renderState(merged.last_state); + renderDiagnostics(merged.diagnostics || {}, merged.last_state); + if (window.viewer3d && window.viewer3d._isLoaded && merged.last_state.joint_pos) { + window.viewer3d.updateJoints(merged.last_state.joint_pos); + } + updateMotorsGrid(merged.last_state); + addPlotData(merged.last_state); + LAST_RENDER_TS = now; + } + } +} + +async function refreshDebug() { + try { + const debug = await api("/api/debug"); + if (debug.status) { + applyStatus(debug.status); + } + renderDiagnostics(debug.status?.diagnostics || {}, debug.status?.last_state || null); + renderFault(debug.status || {}); + const diagJson = $("debug-json"); + if (diagJson) diagJson.textContent = JSON.stringify(debug.status?.diagnostics || {}, null, 2); + } catch (err) { + appendEvent({ kind: "DEBUG_FETCH_ERROR", error: err.message }); + } +} + +function connectSSE() { + if (SSE_CONN) { + SSE_CONN.close(); + SSE_CONN = null; + } + if (SSE_RECONNECT_TIMER) { + clearTimeout(SSE_RECONNECT_TIMER); + SSE_RECONNECT_TIMER = null; + } + const es = new EventSource("/events"); + SSE_CONN = es; + es.onmessage = (event) => { + const ev = JSON.parse(event.data); + if (ev.kind === "STATUS_FULL" || ev.kind === "PULSE" || ev.kind === "STATUS") { + applyStatus(ev); + if (ev.fault_reason) appendEvent({ t: ev.t, kind: "FAULT_REASON", reason: ev.fault_reason }); + } else { + appendEvent(ev); + } + }; + es.onerror = () => { + if (SSE_CONN) { + SSE_CONN.close(); + SSE_CONN = null; + } + if (!SSE_RECONNECT_TIMER) { + SSE_RECONNECT_TIMER = setTimeout(() => { + SSE_RECONNECT_TIMER = null; + connectSSE(); + }, 1500); + } + }; +} + +window.jog = async (leg, joint, dir) => { + const delta = parseFloat($("jt-delta").value) * dir; + const kp = parseFloat($("jt-kp").value); + const kd = parseFloat($("jt-kd").value); + const duration = parseFloat($("jt-dur").value); + try { + await api("/api/test_motor", { leg, joint, delta_rad: delta, kp, kd, duration_s: duration }); + appendEvent({ kind: "JOG_SENT", leg, joint, delta }); + } catch (err) { + appendEvent({ kind: "JOG_ERROR", error: err.message, leg, joint }); + } +}; + +function initMotorsGrid() { + const grid = $("motors-grid"); + if (!grid) return; + const abbr = { hip_abduction: "H_ABD", hip_pitch: "H_PIT", knee: "KNEE", wheel: "WHEEL" }; + grid.innerHTML = SIM_JOINT_ORDER.map(([leg, joint], i) => ` +
+ ● + ${leg.toUpperCase()}_${abbr[joint]} + 0.00 + 0.00 + 0.00 + 0°C + OK +
+ + +
+
+ `).join(""); +} + +function updateMotorsGrid(state) { + if (!state || !state.joint_pos) return; + const positions = state.joint_pos; + const velocities = state.joint_vel || []; + const torques = state.joint_torque || []; + const stale = state.per_motor_stale || []; + const temps = state.motor_temperatures || []; + const faults = state.motor_fault_codes || []; + for (let i = 0; i < 16; i += 1) { + const row = $("mi-" + i); + if (!row) continue; + const dot = $("ms-" + i); + if (dot) { + const count = stale[i] ?? 99; + if (count <= 0) { + dot.style.color = "#4ade80"; + dot.title = "online"; + } else if (count < 5) { + dot.style.color = "#facc15"; + dot.title = `stale(${count})`; + } else { + dot.style.color = "#ef4444"; + dot.title = `offline(${count})`; + } + } + row.children[2].textContent = (positions[i] || 0).toFixed(2); + row.children[3].textContent = (velocities[i] || 0).toFixed(2); + const tau = torques[i] || 0; + row.children[4].textContent = tau.toFixed(2); + row.children[4].style.color = Math.abs(tau) > 16.0 ? "var(--color-danger)" : ""; + row.children[4].style.fontWeight = Math.abs(tau) > 16.0 ? "bold" : ""; + + const temp = temps[i] ?? 0.0; + row.children[5].textContent = temp.toFixed(0) + "°C"; + row.children[5].style.color = temp > 60.0 ? "#ff453a" : temp > 45.0 ? "#ffd60a" : ""; + row.children[5].style.fontWeight = temp > 45.0 ? "bold" : ""; + + const fault = faults[i] ?? 0; + if (fault === 0) { + row.children[6].textContent = "OK"; + row.children[6].style.color = "#30d158"; + row.children[6].style.fontWeight = ""; + } else { + row.children[6].textContent = "E" + fault.toString(16).toUpperCase(); + row.children[6].style.color = "#ff453a"; + row.children[6].style.fontWeight = "bold"; + } + } +} + +function initPlots() { + const colors12 = ["#ff453a", "#ff9f0a", "#ffd60a", "#32ade6", "#0a84ff", "#5e5ce6", "#ff375f", "#bf5af2", "#30d158", "#66d4cf", "#8e8e93", "#c7c7cc"]; + const specs = [ + { id: "plot-pos", title: "Leg Pos (12)", nCh: 12, colors: colors12 }, + { id: "plot-vel", title: "Wheel Vel (4)", nCh: 4, colors: ["#ff453a", "#32ade6", "#30d158", "#ffd60a"] }, + { id: "plot-imu", title: "IMU (gyro+gz)", nCh: 4, colors: ["#ff453a", "#30d158", "#0a84ff", "#ffd60a"] }, + { id: "plot-diag", title: "Diag (dt+age)", nCh: 2, colors: ["#ff453a", "#30d158"] }, + ]; + const maxPts = 150; + specs.forEach((spec) => { + const canvas = $(spec.id); + if (!canvas) return; + canvas.width = canvas.parentElement.clientWidth; + canvas.height = 80; + PLOTS[spec.id] = { + ctx: canvas.getContext("2d"), + title: spec.title, + nCh: spec.nCh, + colors: spec.colors, + data: Array.from({ length: spec.nCh }, () => new Array(maxPts).fill(0)), + yMin: Array(spec.nCh).fill(Infinity), + yMax: Array(spec.nCh).fill(-Infinity), + maxPts, + }; + }); +} + +function addPlotData(state) { + if (!state) return; + const channels = [ + ["plot-pos", (state.joint_pos || []).slice(0, 12)], + ["plot-vel", (state.joint_vel || []).slice(12, 16)], + ["plot-imu", [...(state.gyro || [0, 0, 0]), (state.proj_gravity || [0, 0, -1])[2]]], + ["plot-diag", [state.loop_dt_ms || 0, state.imu_age_ms || 0]], + ]; + channels.forEach(([id, values]) => { + const plot = PLOTS[id]; + if (!plot) return; + for (let i = 0; i < plot.nCh && i < values.length; i += 1) { + const data = plot.data[i]; + data.push(values[i]); + if (data.length > plot.maxPts) data.shift(); + if (values[i] < plot.yMin[i]) plot.yMin[i] = values[i]; + if (values[i] > plot.yMax[i]) plot.yMax[i] = values[i]; + } + drawPlot(plot); + }); +} + +function drawPlot(plot) { + const { ctx, data, colors, yMin, yMax, title, maxPts } = plot; + const canvas = ctx.canvas; + const width = canvas.width; + const height = canvas.height; + ctx.clearRect(0, 0, width, height); + ctx.fillStyle = "rgba(255,255,255,0.5)"; + ctx.font = "10px monospace"; + ctx.fillText(title, 4, 12); + const margin = { l: 30, r: 4, t: 16, b: 4 }; + const plotW = width - margin.l - margin.r; + const plotH = height - margin.t - margin.b; + if (plotW <= 0 || plotH <= 0) return; + for (let i = 0; i < data.length; i += 1) { + if (yMin[i] === Infinity) { + yMin[i] = -1; + yMax[i] = 1; + } + const curMin = Math.min(...data[i]); + const curMax = Math.max(...data[i]); + yMin[i] = yMin[i] * 0.99 + curMin * 0.01; + yMax[i] = yMax[i] * 0.99 + curMax * 0.01; + } + const globalMin = Math.min(...yMin); + const globalMax = Math.max(...yMax); + const range = globalMax - globalMin || 1; + data.forEach((series, i) => { + if (series.length < 2) return; + ctx.strokeStyle = colors[i] || "#8e8e93"; + ctx.lineWidth = 1.0; + ctx.beginPath(); + series.forEach((value, j) => { + const x = margin.l + (j / maxPts) * plotW; + const y = margin.t + plotH - ((value - globalMin) / range) * plotH; + if (j === 0) ctx.moveTo(x, y); + else ctx.lineTo(x, y); + }); + ctx.stroke(); + }); + ctx.fillStyle = "rgba(255,255,255,0.4)"; + ctx.font = "9px monospace"; + ctx.fillText(globalMax.toFixed(1), 2, margin.t + 8); + ctx.fillText(globalMin.toFixed(1), 2, margin.t + plotH - 2); +} + +async function refreshLogs() { + try { + const result = await api("/api/logs"); + const tbody = document.querySelector("#logs-table tbody"); + if (!tbody) return; + tbody.innerHTML = result.sessions.map((s) => ` + + ${s.id.slice(-8)} + ${s.state_csv ? `CSV` : "—"} + ${s.events_jsonl ? `JSONL` : "—"} + ${s.size_kb} KB + + `).join(""); + } catch (err) { + appendEvent({ kind: "LOG_REFRESH_ERROR", error: err.message }); + } +} + +let cmdTimer = null; +function sendCmd() { + if (cmdTimer) return; + cmdTimer = setTimeout(() => { + cmdTimer = null; + api("/api/cmd", { + vx: parseFloat($("cmd-vx").value), + vy: parseFloat($("cmd-vy").value), + yaw: parseFloat($("cmd-yaw").value), + }).catch((err) => appendEvent({ kind: "CMD_ERROR", error: err.message })); + }, 50); +} + +function bind() { + $("btn-connect").onclick = () => api("/api/connect", { dry_run: $("dry-run").checked }).catch((err) => appendEvent({ kind: "CONNECT_ERROR", error: err.message })); + $("btn-disconnect").onclick = () => api("/api/disconnect", {}).catch((err) => appendEvent({ kind: "DISCONNECT_ERROR", error: err.message })); + $("btn-enable").onclick = () => api("/api/enable", {}).catch((err) => appendEvent({ kind: "ENABLE_ERROR", error: err.message })); + $("btn-disable").onclick = () => api("/api/disable", {}).catch((err) => appendEvent({ kind: "DISABLE_ERROR", error: err.message })); + $("btn-startup").onclick = () => api("/api/startup", {}).catch((err) => appendEvent({ kind: "STARTUP_ERROR", error: err.message })); + $("btn-runtime-start").onclick = () => api("/api/runtime/start", { policy_path: $("policy-path").value || null }).catch((err) => appendEvent({ kind: "RUNTIME_START_ERROR", error: err.message })); + $("btn-runtime-stop").onclick = () => api("/api/runtime/stop", {}).catch((err) => appendEvent({ kind: "RUNTIME_STOP_ERROR", error: err.message })); + $("btn-remote-release").onclick = () => api("/api/remote_takeover", { enabled: true }).catch((err) => appendEvent({ kind: "REMOTE_TAKEOVER_ENABLE_ERROR", error: err.message })); + $("btn-remote-reclaim").onclick = () => api("/api/remote_takeover", { enabled: false }).catch((err) => appendEvent({ kind: "REMOTE_TAKEOVER_DISABLE_ERROR", error: err.message })); + $("btn-estop").onclick = () => api("/api/estop", {}).catch((err) => appendEvent({ kind: "ESTOP_ERROR", error: err.message })); + $("btn-reset-estop").onclick = () => api("/api/reset_estop", {}).catch((err) => appendEvent({ kind: "RESET_ESTOP_ERROR", error: err.message })); + $("btn-refresh-debug").onclick = () => refreshDebug(); + + ["vx", "vy", "yaw"].forEach((key) => { + const el = $("cmd-" + key); + el.oninput = () => { + $("cmd-" + key + "-v").textContent = parseFloat(el.value).toFixed(2); + sendCmd(); + }; + }); + $("btn-cmd-zero").onclick = () => { + ["vx", "vy", "yaw"].forEach((key) => { + const el = $("cmd-" + key); + el.value = 0; + $("cmd-" + key + "-v").textContent = "0.00"; + }); + sendCmd(); + }; + + const jtSlider = $("jt-delta"); + jtSlider.oninput = () => { $("jt-delta-v").textContent = parseFloat(jtSlider.value).toFixed(2); }; + + $("btn-show-logs").onclick = () => { + refreshLogs(); + $("logs-modal").classList.remove("hidden"); + }; + $("btn-close-logs").onclick = () => $("logs-modal").classList.add("hidden"); +} + +window.addEventListener("DOMContentLoaded", () => { + initMotorsGrid(); + bind(); + initPlots(); + connectSSE(); + refreshLogs(); + refreshDebug(); + updateButtons({ stage: "DISCONNECTED", busy: false }); + setInterval(refreshLogs, 10000); + setInterval(refreshDebug, 5000); +}); + +window.addEventListener("resize", () => { + Object.values(PLOTS).forEach((plot) => { + plot.ctx.canvas.width = plot.ctx.canvas.parentElement.clientWidth; + }); +}); diff --git a/05_software/real/sim2real_v2/web/static/index.html b/05_software/real/sim2real_v2/web/static/index.html new file mode 100644 index 0000000..e4a2792 --- /dev/null +++ b/05_software/real/sim2real_v2/web/static/index.html @@ -0,0 +1,208 @@ + + + + + + sim2real 控制台 + + + + +
+ +
加载中...
+
+ +
+
+

sim2real

+ DISCONNECTED + + +
+
+ + + +
+ + +
+
+ + + +
+
+ +
+
+

控制流程

+
+ +
+ +
+ + +
+
+ + +
+
+
+
+ +
+

实时状态

+
+
+ +
+
+

Motors / Jog Test

+ POS | VEL | TAU | TEMP | FAULT +
+
+ Kp + Kd + Time + Δ(rad) + 0.10 +
+
+
+
+ +
+
+
+

Diagnostics

+ +
+
Obs NormalizationAligned
+
Control Latency-- ms
+
Tracking Error-- rad
+
Runtime--
+
Runtime Age--
+
Poll Age--
+
Cmd Age--
+
Poll Errors0
+
API Errors0
+
Loop Overruns0
+
Policy Stale0
+
Loop Profile--
+
Latest Target--
+
Stand Balance--
+
Obs / Action--
+
Motor Fresh--
+
Odin Odom--
+
Zero-Cmd Suppression--
+
Policy--
+
+ + + +
+

Command

+
+
输入源WEB
+
遥控可接管NO
+
遥控软急停OFF
+
遥控端口--
+
+
+
+ vx + + 0.00 +
+
+ vy + + 0.00 +
+
+ yaw + + 0.00 +
+ +
+
+ +
+

事件流

+
+
+ +
+

Debug JSON

+ 

+    

+
+    

+      
实时曲线

+      

+        
+        
+        
+        
+      

+    

+  

+
+  
+
+  
+  
+
+  
+  
+
+
diff --git a/05_software/real/sim2real_v2/web/static/style.css b/05_software/real/sim2real_v2/web/static/style.css
new file mode 100644
index 0000000..5ae8572
--- /dev/null
+++ b/05_software/real/sim2real_v2/web/static/style.css
@@ -0,0 +1,399 @@
+/* Apple Glass Design System for sim2real */
+:root {
+  --bg-primary: #000000;
+  --glass-bg: rgba(20, 20, 22, 0.65);
+  --glass-border: rgba(255, 255, 255, 0.12);
+  --glass-shadow: 0 8px 32px rgba(0, 0, 0, 0.25);
+  --text-primary: #ffffff;
+  --text-secondary: #ebebf5;
+  --text-tertiary: #8e8e93;
+  --accent: #0a84ff;
+  --accent-hover: #409cff;
+  --success: #30d158;
+  --warning: #ffd60a;
+  --danger: #ff453a;
+  --blur-amount: 24px;
+  --saturation: 180%;
+  --spring: cubic-bezier(0.4, 0, 0.2, 1);
+  --panel-radius: 16px;
+  --font-family: -apple-system, BlinkMacSystemFont, 'SF Pro Display', 'PingFang SC', sans-serif;
+}
+
+[data-theme="light"] {
+  --bg-primary: #f5f5f7;
+  --glass-bg: rgba(245, 245, 245, 0.75);
+  --glass-border: rgba(0, 0, 0, 0.15);
+  --glass-shadow: 0 8px 32px rgba(0, 0, 0, 0.12);
+  --text-primary: #1d1d1f;
+  --text-secondary: #424245;
+  --text-tertiary: #86868b;
+}
+
+* {
+  box-sizing: border-box;
+  margin: 0;
+  padding: 0;
+}
+
+body {
+  font-family: var(--font-family);
+  overflow: hidden;
+  background: var(--bg-primary);
+  color: var(--text-primary);
+  -webkit-font-smoothing: antialiased;
+  transition: background 0.3s var(--spring);
+}
+
+/* 3D Canvas Background */
+#canvas-container {
+  position: fixed;
+  top: 0; left: 0; right: 0; bottom: 0;
+  z-index: 0;
+  background: radial-gradient(circle at center, #1a1a24 0%, #000000 100%);
+}
+#viewer-canvas {
+  width: 100%;
+  height: 100%;
+  display: block;
+  cursor: grab;
+}
+#viewer-canvas:active {
+  cursor: grabbing;
+}
+.viewer-overlay {
+  position: absolute;
+  top: 50%; left: 50%;
+  transform: translate(-50%, -50%);
+  color: var(--text-tertiary);
+  font-size: 14px;
+  pointer-events: none;
+}
+
+.viewer-count-indicator {
+  position: fixed;
+  bottom: 20px;
+  right: 20px;
+  font-size: 11px;
+  color: var(--text-tertiary);
+  z-index: 10;
+  font-family: monospace;
+}
+
+/* Glass Panels */
+.glass-panel {
+  background: var(--glass-bg);
+  backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
+  -webkit-backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
+  border: 0.5px solid var(--glass-border);
+  box-shadow: var(--glass-shadow);
+  z-index: 50;
+}
+
+/* Top Bar */
+.top-bar {
+  position: fixed;
+  top: 16px;
+  left: 50%;
+  transform: translateX(-50%);
+  display: flex;
+  align-items: center;
+  justify-content: space-between;
+  padding: 8px 16px;
+  border-radius: 24px;
+  width: 96%;
+  max-width: 1400px;
+  gap: 16px;
+}
+
+.top-bar-left, .top-bar-center, .top-bar-right {
+  display: flex;
+  align-items: center;
+  gap: 12px;
+}
+.top-bar-center {
+  flex: 1;
+  justify-content: center;
+}
+
+.top-bar h1 {
+  font-size: 16px;
+  font-weight: 600;
+  margin: 0;
+  background: -webkit-linear-gradient(45deg, #fff, #8e8e93);
+  -webkit-background-clip: text;
+  -webkit-text-fill-color: transparent;
+}
+
+.divider {
+  width: 1px;
+  height: 24px;
+  background: var(--glass-border);
+  margin: 0 4px;
+}
+
+/* Side Panels */
+.side-panel {
+  position: fixed;
+  top: 80px;
+  bottom: 20px;
+  width: 340px;
+  border-radius: var(--panel-radius);
+  display: flex;
+  flex-direction: column;
+  overflow: hidden;
+}
+
+.left-panel { left: 2%; }
+.right-panel { right: 2%; }
+
+.panel-section {
+  padding: 16px;
+  border-bottom: 0.5px solid var(--glass-border);
+  display: flex;
+  flex-direction: column;
+}
+.panel-section:last-child {
+  border-bottom: none;
+}
+.flex-1 { flex: 1; min-height: 0; }
+
+.panel-title {
+  font-size: 12px;
+  font-weight: 700;
+  color: var(--text-tertiary);
+  text-transform: uppercase;
+  letter-spacing: 0.5px;
+  margin-bottom: 12px;
+}
+.panel-title-row {
+  display: flex; justify-content: space-between; align-items: center;
+}
+
+/* Typography & Badges */
+.stage {
+  padding: 4px 10px;
+  border-radius: 12px;
+  font-size: 11px;
+  font-weight: 700;
+  text-transform: uppercase;
+  letter-spacing: 0.5px;
+  background: rgba(255,255,255,0.1);
+  color: var(--text-secondary);
+}
+.stage.DISCONNECTED { background: rgba(142,142,147,0.3); }
+.stage.CONNECTED { background: rgba(10,132,255,0.3); color: #82c4ff; }
+.stage.ENABLED { background: rgba(48,209,88,0.3); color: #8deda7; }
+.stage.FAULTED { background: rgba(255,69,58,0.3); color: #ff8b86; }
+.stage.ESTOPPED { background: rgba(255,69,58,0.5); color: #ff8b86; box-shadow: 0 0 8px rgba(255,69,58,0.4); }
+
+/* Buttons */
+.btn {
+  background: rgba(255, 255, 255, 0.08);
+  border: 1px solid rgba(255, 255, 255, 0.1);
+  border-radius: 8px;
+  color: var(--text-primary);
+  font-size: 12px;
+  font-weight: 500;
+  padding: 6px 12px;
+  cursor: pointer;
+  transition: all 0.2s var(--spring);
+  font-family: inherit;
+  display: inline-flex;
+  align-items: center;
+  justify-content: center;
+}
+.btn:hover:not(:disabled) {
+  background: rgba(255, 255, 255, 0.15);
+  transform: translateY(-1px);
+}
+.btn:active:not(:disabled) {
+  transform: translateY(1px);
+}
+.btn:disabled {
+  opacity: 0.5;
+  cursor: not-allowed;
+}
+.btn-primary { background: var(--accent); border-color: var(--accent); color: white; }
+.btn-primary:hover:not(:disabled) { background: var(--accent-hover); }
+.btn-success { background: rgba(48,209,88,0.8); border-color: transparent; color: white; }
+.btn-warning { background: rgba(255,214,10,0.8); border-color: transparent; color: black; }
+.btn-danger { background: rgba(255,69,58,0.8); border-color: transparent; color: white; }
+.btn-icon { width: 28px; height: 28px; padding: 0; border-radius: 50%; }
+.full-width { width: 100%; }
+.mt-2 { margin-top: 8px; }
+
+.btn-group-vertical {
+  display: flex; flex-direction: column; gap: 8px;
+}
+.btn-row {
+  display: flex; gap: 8px;
+}
+
+/* Inputs */
+.glass-input, .glass-select {
+  background: rgba(0,0,0,0.2);
+  border: 1px solid var(--glass-border);
+  border-radius: 6px;
+  padding: 6px 10px;
+  color: var(--text-primary);
+  font-size: 12px;
+  font-family: inherit;
+  outline: none;
+  transition: border-color 0.2s;
+}
+.glass-input:focus, .glass-select:focus {
+  border-color: var(--accent);
+}
+.glass-input.small { width: 60px; }
+.glass-input.mini { width: 45px; padding: 4px 6px; }
+
+.control-row {
+  display: flex; align-items: center; gap: 8px; margin-bottom: 8px;
+}
+.label { font-size: 11px; color: var(--text-tertiary); }
+
+.compact-grid {
+  margin-bottom: 10px;
+}
+
+.toggle-switch {
+  display: flex; align-items: center; gap: 8px; cursor: pointer;
+}
+.toggle-switch input { display: none; }
+.toggle-switch .slider {
+  position: relative; width: 32px; height: 18px;
+  background: rgba(255,255,255,0.2); border-radius: 18px;
+  transition: 0.3s;
+}
+.toggle-switch .slider::before {
+  content: ""; position: absolute;
+  width: 14px; height: 14px; border-radius: 50%;
+  background: white; top: 2px; left: 2px; transition: 0.3s;
+}
+.toggle-switch input:checked + .slider { background: var(--accent); }
+.toggle-switch input:checked + .slider::before { transform: translateX(14px); }
+.toggle-switch .label { font-size: 12px; color: var(--text-secondary); }
+
+/* Range Sliders */
+.slider-row {
+  display: flex; align-items: center; gap: 8px; margin-bottom: 8px;
+}
+.slider-label {
+  font-size: 12px; width: 30px; color: var(--text-secondary); font-family: monospace;
+}
+.slider-val {
+  font-size: 12px; width: 36px; text-align: right; color: var(--accent); font-family: monospace;
+}
+.glass-slider {
+  flex: 1; -webkit-appearance: none; height: 4px; border-radius: 2px;
+  background: rgba(255,255,255,0.2); outline: none;
+}
+.glass-slider::-webkit-slider-thumb {
+  -webkit-appearance: none; width: 14px; height: 14px;
+  border-radius: 50%; background: white; cursor: pointer;
+  box-shadow: 0 2px 4px rgba(0,0,0,0.5);
+}
+.glass-slider:active::-webkit-slider-thumb { transform: scale(1.2); }
+
+/* Motors List (Jog & Status) */
+.motors-grid-list {
+  display: flex; flex-direction: column; gap: 2px; overflow-y: auto; padding-right: 4px;
+}
+.motor-row {
+  display: flex; align-items: center; justify-content: space-between;
+  padding: 2px 6px; background: rgba(0,0,0,0.25); border-radius: 6px;
+  border: 1px solid rgba(255,255,255,0.03);
+}
+.motor-row .name { font-size: 11px; color: var(--text-secondary); width: 65px; font-weight: 500; font-family: monospace; }
+.motor-row .m-status { font-size: 8px; color: #ef4444; flex-shrink: 0; width: 12px; text-align: center; transition: color 0.3s; }
+.motor-row .val { font-size: 10px; font-family: monospace; text-align: right; width: 35px; }
+.motor-row .val.pos { color: #0a84ff; }
+.motor-row .val.vel { color: #30d158; }
+.motor-row .val.tau { color: #ff9f0a; }
+.motor-row .val.temp { color: var(--text-secondary); width: 32px; }
+.motor-row .val.fault { width: 38px; text-align: center; font-weight: 500; font-family: monospace; }
+
+.m-jog { display: flex; gap: 2px; }
+.btn-jog { 
+  background: rgba(255,255,255,0.1); border: none; border-radius: 4px; 
+  color: white; font-family: monospace; font-size: 11px; padding: 2px 6px;
+  cursor: pointer; min-width: 24px; text-align: center;
+}
+.btn-jog:hover { background: rgba(255,255,255,0.25); }
+
+/* State Grid */
+.state-grid {
+  display: grid; grid-template-columns: 1fr 1fr; gap: 6px;
+  overflow-y: auto;
+}
+.state-item {
+  display: flex; justify-content: space-between; align-items: center;
+  padding: 4px 6px; background: rgba(0,0,0,0.2); border-radius: 4px;
+}
+.state-item .k { font-size: 10px; color: var(--text-tertiary); text-transform: uppercase; }
+.state-item .v { font-size: 11px; font-family: monospace; color: var(--text-primary); }
+.state-item .v.warn { color: var(--warning); }
+.state-item .v.bad { color: var(--danger); }
+
+/* ==== Plots & Logs ==== */
+.log-section { flex: 1; display: flex; flex-direction: column; min-height: 150px; }
+.log {
+  flex: 1; background: rgba(0,0,0,0.4); border-radius: 6px; padding: 8px;
+  font-family: monospace; font-size: 11px; overflow-y: auto; color: var(--text-secondary);
+  border: 1px solid rgba(255,255,255,0.05);
+}
+.log div { margin-bottom: 2px; line-height: 1.3; }
+.plots-section { margin-top: auto; }
+.plots-container {
+  display: flex; flex-direction: column; gap: 4px; overflow-y: auto; padding-right: 4px;
+}
+.plots-container canvas {
+  width: 100% !important; height: 50px !important; background: rgba(0,0,0,0.2); border-radius: 4px;
+}
+
+/* ==== Diagnostics ==== */
+.diag-row {
+  display: flex; justify-content: space-between; align-items: center;
+  padding: 4px 6px; background: rgba(0,0,0,0.2); border-radius: 4px;
+  margin-bottom: 4px; font-family: monospace; font-size: 12px;
+}
+.diag-label { color: var(--text-secondary); }
+.diag-value { color: var(--text-primary); font-weight: bold; }
+.diag-value.success { color: var(--color-success); }
+.diag-value.warning { color: var(--color-warning); }
+.diag-value.danger { color: var(--color-danger); }
+.plots-container::-webkit-scrollbar { width: 4px; }
+.plots-container::-webkit-scrollbar-thumb { background: rgba(255,255,255,0.2); border-radius: 2px; }
+
+/* Modal & Floating BTN */
+.floating-btn {
+  position: fixed; bottom: 20px; left: 20px; width: 40px; height: 40px;
+  border-radius: 50%; font-size: 18px; z-index: 100;
+  box-shadow: var(--glass-shadow);
+}
+.glass-modal {
+  position: fixed; top: 0; left: 0; right: 0; bottom: 0;
+  background: rgba(0,0,0,0.5); backdrop-filter: blur(4px);
+  display: flex; align-items: center; justify-content: center;
+  z-index: 1000; transition: opacity 0.3s;
+}
+.glass-modal.hidden { opacity: 0; pointer-events: none; }
+.modal-content {
+  width: 80%; max-width: 600px; max-height: 80vh;
+  border-radius: var(--panel-radius); display: flex; flex-direction: column;
+}
+.modal-header {
+  padding: 16px; border-bottom: 0.5px solid var(--glass-border);
+  display: flex; justify-content: space-between; align-items: center;
+}
+.btn-close {
+  background: transparent; border: none; color: var(--text-tertiary);
+  font-size: 20px; cursor: pointer;
+}
+.btn-close:hover { color: var(--text-primary); }
+.modal-body { padding: 16px; overflow-y: auto; }
+table { width: 100%; border-collapse: collapse; font-size: 12px; }
+table th { color: var(--text-tertiary); text-align: left; padding: 8px; border-bottom: 1px solid var(--glass-border); }
+table td { padding: 8px; border-bottom: 1px solid rgba(255,255,255,0.05); }
+table a { color: var(--accent); text-decoration: none; }
+table a:hover { text-decoration: underline; }
diff --git a/05_software/real/sim2real_v2/web/static/viewer/MJCFAdapter.js b/05_software/real/sim2real_v2/web/static/viewer/MJCFAdapter.js
new file mode 100644
index 0000000..8140e14
--- /dev/null
+++ b/05_software/real/sim2real_v2/web/static/viewer/MJCFAdapter.js
@@ -0,0 +1,2090 @@
+/**
+ * MJCF Adapter
+ * Parses MJCF XML and converts to unified model
+ */
+import { UnifiedRobotModel, Link, Joint, JointLimits, VisualGeometry, CollisionGeometry, InertialProperties, GeometryType, Constraint } from './UnifiedRobotModel.js';
+import * as THREE from 'three';
+import { loadMeshFile, ensureMeshHasPhongMaterial, getLoaders } from './MeshLoader.js';
+
+export class MJCFAdapter {
+    /**
+     * Process include tags in MJCF XML
+     * Replaces  with the content of the referenced file
+     * @param {string} xmlContent - MJCF XML content
+     * @param {Map} fileMap - File map for loading included files
+     * @param {string} basePath - Base path for resolving relative paths
+     * @returns {Promise} Processed XML content
+     */
+    static async processIncludes(xmlContent, fileMap = null, basePath = null) {
+        const parser = new DOMParser();
+        const doc = parser.parseFromString(xmlContent, 'text/xml');
+
+        // Check for parse errors
+        const parseError = doc.querySelector('parsererror');
+        if (parseError) {
+            // If there's a parse error, return original content
+            console.warn('Initial XML parse error, skipping include processing:', parseError.textContent);
+            return xmlContent;
+        }
+
+        // Find all include elements
+        const includes = doc.querySelectorAll('include');
+
+        if (includes.length === 0) {
+            return xmlContent;
+        }
+
+        console.log(`Processing ${includes.length} include tag(s)...`);
+
+        // Process each include tag
+        for (const includeEl of includes) {
+            const filePath = includeEl.getAttribute('file');
+
+            if (!filePath) {
+                console.warn('Include tag missing file attribute');
+                includeEl.remove();
+                continue;
+            }
+
+            // Try to find the included file in fileMap
+            let includedContent = null;
+
+            if (fileMap) {
+                // Try different path variations
+                const pathVariations = [
+                    filePath,
+                    basePath ? basePath + '/' + filePath : filePath,
+                    filePath.startsWith('/') ? filePath : '/' + filePath
+                ];
+
+                for (const path of pathVariations) {
+                    // Try exact match first
+                    if (fileMap.has(path)) {
+                        const file = fileMap.get(path);
+                        try {
+                            includedContent = await file.text();
+                            console.log(`Found included file: ${path}`);
+                            break;
+                        } catch (e) {
+                            console.warn(`Failed to read included file ${path}:`, e);
+                        }
+                    }
+
+                    // Try case-insensitive match
+                    for (const [key, value] of fileMap) {
+                        if (key.toLowerCase() === path.toLowerCase()) {
+                            try {
+                                includedContent = await value.text();
+                                console.log(`Found included file (case-insensitive): ${key}`);
+                                break;
+                            } catch (e) {
+                                console.warn(`Failed to read included file ${key}:`, e);
+                            }
+                        }
+                    }
+                    if (includedContent) break;
+                }
+            }
+
+            if (!includedContent) {
+                console.warn(`Could not find included file: ${filePath}`);
+                includeEl.remove();
+                continue;
+            }
+
+            // Parse the included content
+            const includedDoc = parser.parseFromString(includedContent, 'text/xml');
+            const includedParseError = includedDoc.querySelector('parsererror');
+
+            if (includedParseError) {
+                console.warn(`Failed to parse included file ${filePath}:`, includedParseError.textContent);
+                includeEl.remove();
+                continue;
+            }
+
+            // Get the mujoco root element from included file
+            const includedRoot = includedDoc.querySelector('mujoco');
+
+            if (!includedRoot) {
+                console.warn(`Included file ${filePath} has no mujoco root element`);
+                includeEl.remove();
+                continue;
+            }
+
+            // Move all child elements from included mujoco to current document
+            // Insert them before the include element
+            const childNodes = Array.from(includedRoot.childNodes);
+
+            for (const child of childNodes) {
+                // Skip text nodes and comment nodes
+                if (child.nodeType === Node.TEXT_NODE ||
+                    (child.nodeType === Node.COMMENT_NODE) ||
+                    (child.nodeType === Node.PROCESSING_INSTRUCTION_NODE)) {
+                    continue;
+                }
+
+                // Clone the node to avoid removing from included doc
+                const importedNode = doc.importNode(child, true);
+
+                // Insert before the include element
+                includeEl.parentNode.insertBefore(importedNode, includeEl);
+            }
+
+            console.log(`Successfully included content from: ${filePath}`);
+
+            // Remove the include element
+            includeEl.remove();
+        }
+
+        // Serialize the modified document back to string
+        const serializer = new XMLSerializer();
+        return serializer.serializeToString(doc);
+    }
+
+    /**
+     * Parse MJCF XML content and convert to unified model
+     * @param {string} xmlContent - MJCF XML content
+     * @param {Map} fileMap - File map (optional), for loading mesh files
+     * @param {string} basePath - Base path for resolving relative include paths (optional)
+     * @returns {Promise}
+     */
+    static async parse(xmlContent, fileMap = null, basePath = null) {
+        // Process include tags first
+        const processedContent = await this.processIncludes(xmlContent, fileMap, basePath);
+
+        const parser = new DOMParser();
+        const doc = parser.parseFromString(processedContent, 'text/xml');
+
+        // Check parse errors
+        const parseError = doc.querySelector('parsererror');
+        if (parseError) {
+            throw new Error('MJCF XML parsing failed: ' + parseError.textContent);
+        }
+
+        const model = new UnifiedRobotModel();
+        model.name = 'mujoco_model';
+
+        // Parse default values and class definitions in default tags first
+        // (needed for mesh scale inheritance)
+        const { classDefaults, rootDefaults } = this.parseDefaults(doc);
+
+        // Parse mesh definitions in asset tags (build mesh name to file path mapping)
+        // Pass classDefaults and rootDefaults to inherit mesh scale
+        const meshMap = this.parseAssets(doc, classDefaults, rootDefaults);
+
+        // Parse material definitions in material tags
+        const materialMap = this.parseMaterials(doc);
+
+        // Get worldbody (root node)
+        const worldbody = doc.querySelector('worldbody');
+        if (!worldbody) {
+            throw new Error('MJCF file missing worldbody element');
+        }
+
+        // Parse geoms directly in worldbody (not inside any body element)
+        // These geoms belong to a special "worldbody" link
+        const worldbodyGeoms = worldbody.querySelectorAll(':scope > geom');
+        if (worldbodyGeoms.length > 0) {
+            const worldbodyLink = new Link('worldbody');
+            worldbodyLink.userData.isWorldbody = true;
+            const seenMeshes = new Set();
+
+            worldbodyGeoms.forEach((geomEl, geomIndex) => {
+                // Get inherited properties from default class
+                const inheritedProps = this.getGeomInheritedProperties(geomEl, classDefaults, rootDefaults);
+
+                const group = geomEl.getAttribute('group');
+                // Use inherited group if not explicitly defined
+                const groupNum = group !== null ? parseInt(group) : 
+                    (inheritedProps.group !== null ? inheritedProps.group : 0);
+                const geomName = (geomEl.getAttribute('name') || '').toLowerCase();
+                const hasRgba = geomEl.hasAttribute('rgba') || inheritedProps.rgba !== null;
+                const meshRef = geomEl.getAttribute('mesh');
+                
+                // Use inherited contype/conaffinity if not explicitly defined
+                const contype = geomEl.getAttribute('contype');
+                const conaffinity = geomEl.getAttribute('conaffinity');
+                const density = geomEl.getAttribute('density');
+                const contypeNum = contype !== null ? parseInt(contype) : 
+                    (inheritedProps.contype !== null ? inheritedProps.contype : null);
+                const conaffinityNum = conaffinity !== null ? parseInt(conaffinity) : 
+                    (inheritedProps.conaffinity !== null ? inheritedProps.conaffinity : null);
+                const densityNum = density !== null ? parseFloat(density) : 
+                    (inheritedProps.density !== null ? inheritedProps.density : null);
+
+                // Determine if collision or visual (same logic as in parseBodies)
+                let isCollisionGeom = false;
+                if (!meshRef) {
+                    isCollisionGeom = true;
+                } else {
+                    if (contypeNum === 0 && conaffinityNum === 0) {
+                        isCollisionGeom = false;
+                    } else if (groupNum === 3) {
+                        // group=3 is collision in MuJoCo convention
+                        isCollisionGeom = true;
+                    } else if (groupNum === 2 || groupNum === 1) {
+                        // group=1,2 are visual
+                        isCollisionGeom = false;
+                    } else if (geomName.includes('collision')) {
+                        isCollisionGeom = true;
+                    } else if (seenMeshes.has(meshRef)) {
+                        if (hasRgba || (contypeNum === 0 && conaffinityNum === 0)) {
+                            return; // Skip duplicate visual
+                        } else {
+                            isCollisionGeom = true;
+                        }
+                    } else if (densityNum === 0 && groupNum === 1) {
+                        isCollisionGeom = false;
+                    } else if (hasRgba) {
+                        isCollisionGeom = false;
+                    } else {
+                        isCollisionGeom = false;
+                    }
+                }
+
+                const geom = this.parseGeom(geomEl, meshMap);
+                if (geom) {
+                    if (isCollisionGeom) {
+                        const collision = new CollisionGeometry();
+                        collision.geometry = geom;
+                        collision.name = geomEl.getAttribute('name') || `worldbody_collision_${geomIndex}`;
+                        collision.origin = this.parseOrigin(geomEl);
+                        worldbodyLink.collisions.push(collision);
+                    } else {
+                        if (meshRef) {
+                            seenMeshes.add(meshRef);
+                        }
+                        const visual = new VisualGeometry();
+                        visual.geometry = geom;
+                        visual.name = geomEl.getAttribute('name') || `worldbody_geom_${geomIndex}`;
+                        visual.origin = this.parseOrigin(geomEl);
+
+                        // Parse rgba (priority: explicit > inherited)
+                        let rgba = null;
+                        if (geomEl.hasAttribute('rgba')) {
+                            const rgbaStr = geomEl.getAttribute('rgba');
+                            const rgbaVals = rgbaStr.split(' ').map(parseFloat);
+                            if (rgbaVals.length >= 3) {
+                                rgba = {
+                                    r: rgbaVals[0],
+                                    g: rgbaVals[1],
+                                    b: rgbaVals[2],
+                                    a: rgbaVals.length >= 4 ? rgbaVals[3] : 1.0
+                                };
+                            }
+                        } else if (inheritedProps.rgba) {
+                            rgba = inheritedProps.rgba;
+                        }
+
+                        visual.userData = {
+                            group: groupNum,
+                            hasRgba: hasRgba || !!rgba,
+                            rgba: rgba,
+                            meshRef: meshRef,
+                            geomType: geomEl.getAttribute('type') || (meshRef ? 'mesh' : 'box')
+                        };
+                        worldbodyLink.visuals.push(visual);
+                    }
+                }
+            });
+
+            // Only add worldbody link if it has geometries
+            if (worldbodyLink.visuals.length > 0 || worldbodyLink.collisions.length > 0) {
+                model.addLink(worldbodyLink);
+            }
+        }
+
+        // Parse all bodies (links), pass meshMap, materialMap, classDefaults and rootDefaults
+        const bodyMap = new Map();
+        this.parseBodies(worldbody, null, bodyMap, model, null, meshMap, null, materialMap, classDefaults, rootDefaults);
+
+        // Parse all joints
+        this.parseJoints(worldbody, bodyMap, model, null, classDefaults);
+
+        // Parse equality constraints (closed-chain constraints for parallel mechanisms)
+        this.parseEquality(doc, model);
+
+        // Find root body
+        // Priority: worldbody link > bodies without parent joints > first link
+        const worldbodyLink = model.links.get('worldbody');
+        if (worldbodyLink) {
+            model.rootLink = 'worldbody';
+        } else {
+            const rootBodies = Array.from(model.links.keys()).filter(
+                name => !Array.from(model.joints.values()).some(j => j.child === name)
+            );
+            if (rootBodies.length > 0) {
+                model.rootLink = rootBodies[0];
+            } else if (model.links.size > 0) {
+                model.rootLink = Array.from(model.links.keys())[0];
+            }
+        }
+
+        // Create Three.js objects (asynchronously load mesh files)
+        await this.createThreeObject(model, fileMap, meshMap);
+
+        return model;
+    }
+
+    /**
+     * Parse mesh definitions in asset tags
+     * @param {Document} doc - XML document
+     * @param {Map} classDefaults - Class default properties map (optional)
+     * @param {object} rootDefaults - Root default properties (optional)
+     * @returns {Map} Mapping from mesh names to mesh data
+     * Mesh data can be: { type: 'file', path: string, scale: [x,y,z] } or { type: 'vertex', vertices: Float32Array, scale: [x,y,z] }
+     */
+    static parseAssets(doc, classDefaults = null, rootDefaults = null) {
+        const meshMap = new Map();
+        const asset = doc.querySelector('asset');
+        if (!asset) {
+            return meshMap;
+        }
+
+        const meshes = asset.querySelectorAll('mesh');
+        meshes.forEach((meshEl, index) => {
+            let name = meshEl.getAttribute('name');
+            const file = meshEl.getAttribute('file');
+            const vertex = meshEl.getAttribute('vertex');
+            const scale = meshEl.getAttribute('scale');
+            const meshClass = meshEl.getAttribute('class');
+
+            // Parse scale (priority: direct attribute > class inheritance > root defaults > [1,1,1])
+            let scaleVec = [1, 1, 1];
+            
+            // First check direct scale attribute
+            if (scale) {
+                const scaleValues = scale.trim().split(/\s+/).map(parseFloat);
+                if (scaleValues.length === 1) {
+                    scaleVec = [scaleValues[0], scaleValues[0], scaleValues[0]];
+                } else if (scaleValues.length === 3) {
+                    scaleVec = scaleValues;
+                }
+            } else if (meshClass && classDefaults && classDefaults.has(meshClass)) {
+                // Try to inherit scale from class defaults
+                const classDefault = classDefaults.get(meshClass);
+                if (classDefault.mesh && classDefault.mesh.scale) {
+                    scaleVec = classDefault.mesh.scale;
+                }
+            } else if (rootDefaults && rootDefaults.mesh && rootDefaults.mesh.scale) {
+                // Fall back to root defaults (e.g., robotis_op3)
+                scaleVec = rootDefaults.mesh.scale;
+            }
+
+            // If has vertex attribute, it's an inline-defined mesh
+            if (vertex) {
+                if (!name) {
+                    name = `inline_mesh_${index}`;
+                }
+
+                // Parse vertex data
+                const vertexValues = vertex.trim().split(/\s+/).map(parseFloat);
+                const vertices = new Float32Array(vertexValues);
+
+                meshMap.set(name, {
+                    type: 'vertex',
+                    vertices: vertices,
+                    scale: scaleVec
+                });
+            }
+            // If has file attribute, it's an external file
+            else if (file) {
+                // If no name, extract filename from file (remove path and extension)
+                if (!name) {
+                    // Extract filename from path: "path/to/wheel.stl" -> "wheel"
+                    const fileName = file.split('/').pop().split('\\').pop(); // Support / and \ path separators
+                    name = fileName.split('.')[0]; // Remove extension
+                }
+
+                meshMap.set(name, {
+                    type: 'file',
+                    path: file,
+                    scale: scaleVec
+                });
+            } else {
+                console.warn('MJCF mesh element missing file or vertex attribute, skipping');
+                return;
+            }
+        });
+
+        return meshMap;
+    }
+
+    /**
+     * Parse material definitions in asset tags
+     * @param {Document} doc - XML document
+     * @returns {Map} Mapping from material names to material properties
+     */
+    static parseMaterials(doc) {
+        const materialMap = new Map();
+        const asset = doc.querySelector('asset');
+        if (!asset) {
+            return materialMap;
+        }
+
+        const materials = asset.querySelectorAll('material');
+        materials.forEach((matEl) => {
+            const name = matEl.getAttribute('name');
+            if (!name) return;
+
+            const material = {};
+
+            // Parse rgba
+            const rgba = matEl.getAttribute('rgba');
+            if (rgba) {
+                const vals = rgba.split(' ').map(parseFloat);
+                if (vals.length >= 3) {
+                    material.rgba = {
+                        r: vals[0],
+                        g: vals[1],
+                        b: vals[2],
+                        a: vals.length >= 4 ? vals[3] : 1.0
+                    };
+                }
+            }
+
+            // Parse other material properties
+            const specular = matEl.getAttribute('specular');
+            if (specular) {
+                const vals = specular.split(' ').map(parseFloat);
+                material.specular = vals[0] || 0.5;
+            }
+
+            const shininess = matEl.getAttribute('shininess');
+            if (shininess) {
+                material.shininess = parseFloat(shininess);
+            }
+
+            materialMap.set(name, material);
+        });
+
+        return materialMap;
+    }
+
+    /**
+     * Parse default values and class definitions in default tags
+     * @param {Document} doc - XML document
+     * @returns {object} Object containing classDefaults Map and rootDefaults object
+     */
+    static parseDefaults(doc) {
+        const classDefaults = new Map();
+        let rootDefaults = {};
+
+        // Recursively parse default tags
+        const parseDefaultElement = (defaultEl, parentDefaults = {}) => {
+            const className = defaultEl.getAttribute('class');
+
+            // Start from parent defaults, deep copy to avoid reference issues
+            const defaults = JSON.parse(JSON.stringify(parentDefaults || {}));
+
+            // Parse mesh default values
+            const meshEl = defaultEl.querySelector(':scope > mesh');
+            if (meshEl) {
+                if (!defaults.mesh) {
+                    defaults.mesh = {};
+                }
+
+                // Parse scale
+                const scale = meshEl.getAttribute('scale');
+                if (scale) {
+                    const scaleVals = scale.trim().split(/\s+/).map(parseFloat);
+                    if (scaleVals.length === 1) {
+                        defaults.mesh.scale = [scaleVals[0], scaleVals[0], scaleVals[0]];
+                    } else if (scaleVals.length === 3) {
+                        defaults.mesh.scale = scaleVals;
+                    }
+                }
+            }
+
+            // Parse joint default values
+            const jointEl = defaultEl.querySelector(':scope > joint');
+            if (jointEl) {
+                // If parent has joint defaults, inherit first
+                if (!defaults.joint) {
+                    defaults.joint = {};
+                }
+
+                // Parse axis (if axis defined, completely replace parent axis)
+                const axis = jointEl.getAttribute('axis');
+                if (axis) {
+                    const axisVals = axis.split(' ').map(parseFloat);
+                    defaults.joint.axis = [axisVals[0] || 0, axisVals[1] || 0, axisVals[2] || 0];
+                }
+
+                // Parse range
+                const range = jointEl.getAttribute('range');
+                if (range) {
+                    const rangeVals = range.split(' ').map(parseFloat);
+                    defaults.joint.range = rangeVals;
+                }
+
+                // Parse damping
+                const damping = jointEl.getAttribute('damping');
+                if (damping) {
+                    defaults.joint.damping = parseFloat(damping);
+                }
+            }
+
+            // Parse geom default values
+            const geomEl = defaultEl.querySelector(':scope > geom');
+            if (geomEl) {
+                if (!defaults.geom) {
+                    defaults.geom = {};
+                }
+
+                // Parse contype
+                const contype = geomEl.getAttribute('contype');
+                if (contype !== null) {
+                    defaults.geom.contype = parseInt(contype);
+                }
+
+                // Parse conaffinity
+                const conaffinity = geomEl.getAttribute('conaffinity');
+                if (conaffinity !== null) {
+                    defaults.geom.conaffinity = parseInt(conaffinity);
+                }
+
+                // Parse group
+                const group = geomEl.getAttribute('group');
+                if (group !== null) {
+                    defaults.geom.group = parseInt(group);
+                }
+
+                // Parse rgba
+                const rgba = geomEl.getAttribute('rgba');
+                if (rgba) {
+                    const rgbaVals = rgba.split(' ').map(parseFloat);
+                    if (rgbaVals.length >= 3) {
+                        defaults.geom.rgba = {
+                            r: rgbaVals[0],
+                            g: rgbaVals[1],
+                            b: rgbaVals[2],
+                            a: rgbaVals.length >= 4 ? rgbaVals[3] : 1.0
+                        };
+                    }
+                }
+
+                // Parse material
+                const material = geomEl.getAttribute('material');
+                if (material) {
+                    defaults.geom.material = material;
+                }
+
+                // Parse type
+                const type = geomEl.getAttribute('type');
+                if (type) {
+                    defaults.geom.type = type;
+                }
+
+                // Parse density
+                const density = geomEl.getAttribute('density');
+                if (density !== null) {
+                    defaults.geom.density = parseFloat(density);
+                }
+            }
+
+            // If has class name, save to class map
+            if (className) {
+                classDefaults.set(className, defaults);
+            } else {
+                // No class name means this is a root default (inherits to all)
+                // Store the final computed defaults as rootDefaults
+                Object.assign(rootDefaults, defaults);
+            }
+
+            // Recursively process nested default tags
+            const nestedDefaults = defaultEl.querySelectorAll(':scope > default');
+            nestedDefaults.forEach(nested => {
+                parseDefaultElement(nested, defaults);
+            });
+        };
+
+        // Start parsing from root default tags
+        const rootDefaultElements = doc.querySelectorAll('mujoco > default');
+        rootDefaultElements.forEach(defaultEl => {
+            parseDefaultElement(defaultEl);
+        });
+
+        return { classDefaults, rootDefaults };
+    }
+
+    /**
+     * Get inherited geom properties from default class
+     * @param {Element} geomEl - geom element
+     * @param {Map} classDefaults - Class default properties map
+     * @param {object} rootDefaults - Root default properties
+     * @returns {object} Inherited properties object
+     */
+    static getGeomInheritedProperties(geomEl, classDefaults, rootDefaults) {
+        const inherited = {
+            contype: null,
+            conaffinity: null,
+            group: null,
+            rgba: null,
+            material: null,
+            type: null,
+            density: null
+        };
+
+        // First apply root defaults
+        if (rootDefaults && rootDefaults.geom) {
+            Object.assign(inherited, rootDefaults.geom);
+        }
+
+        // Then apply class defaults (if geom has class attribute)
+        const className = geomEl.getAttribute('class');
+        if (className && classDefaults && classDefaults.has(className)) {
+            const classDefault = classDefaults.get(className);
+            if (classDefault.geom) {
+                Object.assign(inherited, classDefault.geom);
+            }
+        }
+
+        return inherited;
+    }
+
+    /**
+     * Recursively parse body elements, record parent-child relationships
+     */
+    static parseBodies(element, parentName, bodyMap, model, parentLinkRef = null, meshMap = null, stats = null, materialMap = null, classDefaults = null, rootDefaults = null) {
+        // Initialize stats object (only on root call)
+        if (!stats) {
+            stats = { totalGeoms: 0, skippedCollisionGeoms: 0, visualGeoms: 0 };
+        }
+
+        const bodies = element.querySelectorAll(':scope > body');
+
+        bodies.forEach(bodyEl => {
+            const linkName = bodyEl.getAttribute('name') || `body_${bodyMap.size}`;
+            const link = new Link(linkName);
+
+            // Record parent link relationship (for building hierarchy later)
+            if (parentName) {
+                link.userData.parentName = parentName;
+            }
+
+            // Parse body's pos and quat (body's own position)
+            const bodyOrigin = this.parseOrigin(bodyEl);
+            link.userData.bodyOrigin = bodyOrigin;
+
+            // Parse geometries (geom)
+            const geoms = bodyEl.querySelectorAll(':scope > geom');
+            const seenMeshes = new Set(); // Track added meshes to avoid duplicates
+
+            geoms.forEach((geomEl, geomIndex) => {
+                stats.totalGeoms++;
+
+                // Get inherited properties from default class
+                const inheritedProps = this.getGeomInheritedProperties(geomEl, classDefaults, rootDefaults);
+
+                const group = geomEl.getAttribute('group');
+                // Use inherited group if not explicitly defined
+                const groupNum = group !== null ? parseInt(group) : 
+                    (inheritedProps.group !== null ? inheritedProps.group : 0);
+                const geomName = (geomEl.getAttribute('name') || '').toLowerCase();
+                const hasRgba = geomEl.hasAttribute('rgba') || inheritedProps.rgba !== null;
+                const meshRef = geomEl.getAttribute('mesh');
+                // Use inherited type if not explicitly defined
+                const geomType = geomEl.getAttribute('type') || inheritedProps.type || (meshRef ? 'mesh' : 'box');
+
+                // Check collision-related attributes (use inherited if not explicitly defined)
+                const contype = geomEl.getAttribute('contype');
+                const conaffinity = geomEl.getAttribute('conaffinity');
+                const density = geomEl.getAttribute('density');
+                const contypeNum = contype !== null ? parseInt(contype) : 
+                    (inheritedProps.contype !== null ? inheritedProps.contype : null);
+                const conaffinityNum = conaffinity !== null ? parseInt(conaffinity) : 
+                    (inheritedProps.conaffinity !== null ? inheritedProps.conaffinity : null);
+                const densityNum = density !== null ? parseFloat(density) : 
+                    (inheritedProps.density !== null ? inheritedProps.density : null);
+
+                // Determine geom type: visual or collision
+                let isCollisionGeom = false;
+                let skipReason = '';
+
+                // [Key Strategy]: Distinguish visual and collision geoms
+                // Basic geometries (box, cylinder, sphere) are usually simplified shapes for collision
+                if (!meshRef) {
+                    // No mesh reference, basic geometry, treat as collision
+                    isCollisionGeom = true;
+                } else {
+                    // Has mesh reference, check if should be collision
+
+                    // Strategy 1: Explicitly disabled collision (contype="0" conaffinity="0") = visual only
+                    if (contypeNum === 0 && conaffinityNum === 0) {
+                        // This is explicitly marked as visual-only (no collision)
+                        isCollisionGeom = false;
+                    }
+                    // Strategy 2: group=2 is visual, group=3 is collision
+                    // MuJoCo convention: group 0=default, 1=visual1, 2=visual2, 3=collision
+                    else if (groupNum === 3) {
+                        isCollisionGeom = true;
+                    } else if (groupNum === 2 || groupNum === 1) {
+                        isCollisionGeom = false;
+                    }
+                    // Strategy 3: Name contains collision (indicates collision-specific)
+                    else if (geomName.includes('collision')) {
+                        isCollisionGeom = true;
+                    }
+                    // Strategy 4: If same mesh already added as visual
+                    else if (seenMeshes.has(meshRef)) {
+                        // If current geom also has visual markers (rgba or contype="0"), skip duplicate visual
+                        if (hasRgba || (contypeNum === 0 && conaffinityNum === 0)) {
+                            stats.skippedCollisionGeoms++;
+                            return;
+                        } else {
+                            // Same mesh, but current geom has no visual markers - treat as collision
+                            isCollisionGeom = true;
+                        }
+                    }
+                    // Strategy 5: If density="0" and group="1", likely visual-only (common pattern in MJCF)
+                    else if (densityNum === 0 && groupNum === 1) {
+                        // This pattern (density="0" group="1") is often used for visual-only geoms
+                        isCollisionGeom = false;
+                    }
+                    // Strategy 6: Default: if has rgba, treat as visual
+                    else if (hasRgba) {
+                        isCollisionGeom = false;
+                    }
+                    // Strategy 7: Default for mesh: treat as visual (for display purposes)
+                    else {
+                        // No explicit markers, but it's a mesh - default to visual for display
+                        // (collision might be handled by a separate geom with same mesh)
+                        isCollisionGeom = false;
+                    }
+                }
+
+                const geom = this.parseGeom(geomEl, meshMap);
+                if (geom) {
+                    if (isCollisionGeom) {
+                        // Add to collision list
+                        const collision = new CollisionGeometry();
+                        collision.geometry = geom;
+                        collision.name = geomEl.getAttribute('name') || `collision_${geomIndex}`;
+                        collision.origin = this.parseOrigin(geomEl);
+                        link.collisions.push(collision);
+                    } else {
+                    // Add to visual list
+                        stats.visualGeoms++;
+
+                        // Record added mesh
+                        if (meshRef) {
+                            seenMeshes.add(meshRef);
+                        }
+
+                        const visual = new VisualGeometry();
+                        visual.geometry = geom;
+                        visual.name = geomEl.getAttribute('name') || `geom_${geomIndex}`;
+                        visual.origin = this.parseOrigin(geomEl);
+
+                        // Parse MJCF rgba color (priority: geom rgba > inherited rgba > material rgba)
+                        let rgba = null;
+                        let materialName = null;
+
+                        // 1. First check geom's own rgba, then inherited rgba
+                        if (geomEl.hasAttribute('rgba')) {
+                            const rgbaStr = geomEl.getAttribute('rgba');
+                            const rgbaVals = rgbaStr.split(' ').map(parseFloat);
+                            if (rgbaVals.length >= 3) {
+                                rgba = {
+                                    r: rgbaVals[0],
+                                    g: rgbaVals[1],
+                                    b: rgbaVals[2],
+                                    a: rgbaVals.length >= 4 ? rgbaVals[3] : 1.0
+                                };
+                            }
+                        }
+
+                        // 2. If geom has no explicit rgba, check inherited rgba
+                        if (!rgba && inheritedProps.rgba) {
+                            rgba = inheritedProps.rgba;
+                        }
+
+                        // 3. If still no rgba, check if references material (explicit or inherited)
+                        if (!rgba && materialMap) {
+                            materialName = geomEl.getAttribute('material') || inheritedProps.material;
+                            if (materialName && materialMap.has(materialName)) {
+                                const mat = materialMap.get(materialName);
+                                if (mat.rgba) {
+                                    rgba = mat.rgba;
+                                }
+                            }
+                        }
+
+                        visual.userData = {
+                            group: groupNum,
+                            hasRgba: hasRgba || !!rgba,
+                            rgba: rgba,
+                            materialName: materialName,
+                            meshRef: meshRef,
+                            geomType: geomType
+                        };
+                        link.visuals.push(visual);
+                    }
+                }
+            });
+
+            // Parse inertial properties
+            const inertialEl = bodyEl.querySelector(':scope > inertial');
+            if (inertialEl) {
+                link.inertial = this.parseInertial(inertialEl);
+            }
+
+            model.addLink(link);
+            bodyMap.set(linkName, { link, element: bodyEl, parentName });
+
+            // Recursively parse child bodies
+            this.parseBodies(bodyEl, linkName, bodyMap, model, link, meshMap, stats, materialMap, classDefaults, rootDefaults);
+        });
+    }
+
+    /**
+     * Parse geom element
+     * @param {Element} geomEl - geom element
+     * @param {Map} meshMap - Mapping from mesh names to file paths
+     */
+    static parseGeom(geomEl, meshMap = null) {
+        // In MJCF, if geom has mesh attribute, type should be mesh
+        const meshAttr = geomEl.getAttribute('mesh');
+        let type = geomEl.getAttribute('type');
+
+        // If has mesh attribute but no explicit type declaration, auto-set to mesh
+        if (meshAttr && !type) {
+            type = 'mesh';
+        }
+
+        // If no type attribute and no mesh attribute, default to sphere
+        if (!type) {
+            type = 'sphere';
+        }
+
+        const geometry = new GeometryType(type);
+
+        switch (type) {
+            case 'box':
+                const size = geomEl.getAttribute('size');
+                if (size) {
+                    const sizes = size.split(' ').map(parseFloat);
+                    // MJCF size is half-size, multiply by 2 to convert to full size
+                    geometry.size = sizes.length === 1
+                        ? { x: sizes[0] * 2, y: sizes[0] * 2, z: sizes[0] * 2 }
+                        : { x: (sizes[0] || 0.05) * 2, y: (sizes[1] || 0.05) * 2, z: (sizes[2] || 0.05) * 2 };
+                } else {
+                    geometry.size = { x: 0.1, y: 0.1, z: 0.1 };
+                }
+                break;
+
+            case 'sphere':
+                // MJCF sphere size is radius
+                const radius = parseFloat(geomEl.getAttribute('size') || '0.1');
+                geometry.size = { radius };
+                break;
+
+            case 'cylinder':
+            case 'capsule':
+                // Handle fromto attribute for capsule/cylinder
+                const fromto = geomEl.getAttribute('fromto');
+                const radiusAttr = geomEl.getAttribute('size');
+                
+                if (fromto) {
+                    const ft = fromto.split(' ').map(parseFloat);
+                    if (ft.length >= 6) {
+                        const p1 = new THREE.Vector3(ft[0], ft[1], ft[2]);
+                        const p2 = new THREE.Vector3(ft[3], ft[4], ft[5]);
+                        const center = new THREE.Vector3().addVectors(p1, p2).multiplyScalar(0.5);
+                        const height = p1.distanceTo(p2);
+                        
+                        // Calculate rotation to align cylinder/capsule with the fromto vector
+                        const direction = new THREE.Vector3().subVectors(p2, p1).normalize();
+                        const defaultDir = new THREE.Vector3(0, 1, 0); // Default cylinder axis is Y
+                        const quaternion = new THREE.Quaternion().setFromUnitVectors(defaultDir, direction);
+                        const euler = new THREE.Euler().setFromQuaternion(quaternion);
+                        
+                        // Store fromto data
+                        geometry.fromto = {
+                            p1: [ft[0], ft[1], ft[2]],
+                            p2: [ft[3], ft[4], ft[5]],
+                            center: [center.x, center.y, center.z],
+                            height: height,
+                            rpy: [euler.x, euler.y, euler.z]
+                        };
+                        
+                        // Parse radius - for fromto, size is just radius
+                        const radiusVal = parseFloat(radiusAttr || '0.01');
+                        geometry.size = { radius: radiusVal, height: height };
+                    }
+                } else if (radiusAttr) {
+                    const radii = radiusAttr.split(' ').map(parseFloat);
+                    // MJCF cylinder/capsule size is [radius, half-height], height needs to be multiplied by 2
+                    geometry.size = {
+                        radius: radii[0] || 0.1,
+                        height: (radii[1] || 0.1) * 2  // Multiply by 2 to get full height
+                    };
+                } else {
+                    geometry.size = { radius: 0.01, height: 0.1 };
+                }
+                break;
+
+            case 'mesh':
+                let meshRef = geomEl.getAttribute('mesh');
+                // If meshMap exists, try to find data corresponding to mesh name
+                if (meshMap && meshMap.has(meshRef)) {
+                    const meshData = meshMap.get(meshRef);
+                    if (meshData.type === 'file') {
+                        // External file mesh
+                        geometry.filename = meshData.path;
+                        // Apply mesh scale from asset definition (class inheritance)
+                        if (meshData.scale) {
+                            geometry.meshScale = meshData.scale;
+                        }
+                    } else if (meshData.type === 'vertex') {
+                        // Inline vertex mesh, store vertex data
+                        geometry.inlineVertices = meshData.vertices;
+                        geometry.inlineScale = meshData.scale;
+                    }
+                } else {
+                    // Otherwise directly use mesh attribute value (may be file path)
+                    geometry.filename = meshRef;
+                    if (meshMap && meshMap.size > 0) {
+                        console.warn(`⚠️ mesh "${meshRef}" not defined in assets`);
+                    }
+                }
+                geometry.size = null;
+                break;
+        }
+
+        return geometry;
+    }
+
+    /**
+     * Parse origin attribute (pos + quat or xyz + rpy)
+     */
+    static parseOrigin(element) {
+        const origin = { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+
+        // Check pos attribute
+        const pos = element.getAttribute('pos');
+        if (pos) {
+            const xyz = pos.split(' ').map(parseFloat);
+            origin.xyz = [xyz[0] || 0, xyz[1] || 0, xyz[2] || 0];
+        }
+
+        // Check quat attribute (quaternion, needs to be converted to rpy)
+        const quat = element.getAttribute('quat');
+        if (quat) {
+            const q = quat.split(' ').map(parseFloat);
+            // MJCF uses wxyz order
+            const qw = q[0], qx = q[1], qy = q[2], qz = q[3];
+
+            // Save original quaternion (for inertia visualization)
+            origin.quat = { w: qw, x: qx, y: qy, z: qz };
+
+            // Convert to Euler angles
+            origin.rpy = this.quaternionToEuler(qw, qx, qy, qz);
+        } else {
+            // Check euler attribute
+            const euler = element.getAttribute('euler');
+            if (euler) {
+                const rpy = euler.split(' ').map(parseFloat);
+                origin.rpy = [rpy[0] || 0, rpy[1] || 0, rpy[2] || 0];
+            }
+        }
+
+        return origin;
+    }
+
+    /**
+     * Convert quaternion to Euler angles (simplified version)
+     */
+    static quaternionToEuler(w, x, y, z) {
+        // Normalize quaternion first (MJCF may use non-normalized quaternions)
+        const norm = Math.sqrt(w * w + x * x + y * y + z * z);
+        if (norm > 0) {
+            w = w / norm;
+            x = x / norm;
+            y = y / norm;
+            z = z / norm;
+        }
+
+        // Simplified conversion (using standard formula)
+        const sinr_cosp = 2 * (w * x + y * z);
+        const cosr_cosp = 1 - 2 * (x * x + y * y);
+        const roll = Math.atan2(sinr_cosp, cosr_cosp);
+
+        const sinp = 2 * (w * y - z * x);
+        const pitch = Math.abs(sinp) >= 1
+            ? Math.sign(sinp) * Math.PI / 2
+            : Math.asin(sinp);
+
+        const siny_cosp = 2 * (w * z + x * y);
+        const cosy_cosp = 1 - 2 * (y * y + z * z);
+        const yaw = Math.atan2(siny_cosp, cosy_cosp);
+
+        return [roll, pitch, yaw];
+    }
+
+    /**
+     * Parse inertial element
+     *
+     * MJCF inertia is defined in inertial frame, needs:
+     * 1. Transform to body frame via quat rotation
+     * 2. Then perform MJCF to Three.js coordinate system conversion
+     */
+    static parseInertial(inertialEl) {
+        const inertial = new InertialProperties();
+
+        const mass = inertialEl.getAttribute('mass');
+        if (mass) inertial.mass = parseFloat(mass);
+
+        const origin = this.parseOrigin(inertialEl);
+        inertial.origin = origin;
+
+        // Parse inertia matrix
+        const diaginertia = inertialEl.getAttribute('diaginertia');
+        const fullinertia = inertialEl.getAttribute('fullinertia');
+
+        let mjcf_ixx = 0, mjcf_iyy = 0, mjcf_izz = 0;
+        let mjcf_ixy = 0, mjcf_ixz = 0, mjcf_iyz = 0;
+
+        if (diaginertia) {
+            const values = diaginertia.split(' ').map(parseFloat);
+            mjcf_ixx = values[0] || 0;
+            mjcf_iyy = values[1] || 0;
+            mjcf_izz = values[2] || 0;
+        }
+
+        if (fullinertia) {
+            const values = fullinertia.split(' ').map(parseFloat);
+            mjcf_ixx = values[0] || 0;
+            mjcf_iyy = values[1] || 0;
+            mjcf_izz = values[2] || 0;
+            mjcf_ixy = values[3] || 0;
+            mjcf_ixz = values[4] || 0;
+            mjcf_iyz = values[5] || 0;
+        }
+
+        // Save original diagonal inertia values (for visualization)
+        // These are principal moments of inertia in inertial frame
+        inertial.diagonalInertia = {
+            ixx: mjcf_ixx,
+            iyy: mjcf_iyy,
+            izz: mjcf_izz
+        };
+
+        // If quat exists, need to rotate inertia tensor
+        if (origin.quat) {
+            const rotated = this.rotateInertiaTensor(
+                mjcf_ixx, mjcf_iyy, mjcf_izz,
+                mjcf_ixy, mjcf_ixz, mjcf_iyz,
+                origin.quat
+            );
+            mjcf_ixx = rotated.ixx;
+            mjcf_iyy = rotated.iyy;
+            mjcf_izz = rotated.izz;
+            mjcf_ixy = rotated.ixy;
+            mjcf_ixz = rotated.ixz;
+            mjcf_iyz = rotated.iyz;
+        }
+
+        // Coordinate system conversion: MJCF -> Three.js
+        // On top of quat rotation, need to rotate 180 degrees around Y-axis (split into two 90-degree rotations)
+        // This is the correct transformation from MJCF coordinate system (X-forward, Y-left, Z-up) to Three.js coordinate system (X-right, Y-up, Z-forward)
+        const coordRotated1 = this.rotateInertiaAroundAxis(
+            mjcf_ixx, mjcf_iyy, mjcf_izz,
+            mjcf_ixy, mjcf_ixz, mjcf_iyz,
+            'Y', 90
+        );
+
+        const coordRotated2 = this.rotateInertiaAroundAxis(
+            coordRotated1.ixx, coordRotated1.iyy, coordRotated1.izz,
+            coordRotated1.ixy, coordRotated1.ixz, coordRotated1.iyz,
+            'Y', 90
+        );
+
+        inertial.ixx = coordRotated2.ixx;
+        inertial.iyy = coordRotated2.iyy;
+        inertial.izz = coordRotated2.izz;
+        inertial.ixy = coordRotated2.ixy;
+        inertial.ixz = coordRotated2.ixz;
+        inertial.iyz = coordRotated2.iyz;
+
+        return inertial;
+    }
+
+    /**
+     * Rotate inertia tensor around specified axis
+     * @param {string} axis - 'X', 'Y', or 'Z'
+     * @param {number} degrees - Rotation angle (degrees)
+     */
+    static rotateInertiaAroundAxis(ixx, iyy, izz, ixy, ixz, iyz, axis, degrees) {
+        const rad = degrees * Math.PI / 180;
+        const c = Math.cos(rad);
+        const s = Math.sin(rad);
+
+        let R;
+        if (axis === 'X') {
+            R = [
+                [1, 0, 0],
+                [0, c, -s],
+                [0, s, c]
+            ];
+        } else if (axis === 'Y') {
+            R = [
+                [c, 0, s],
+                [0, 1, 0],
+                [-s, 0, c]
+            ];
+        } else if (axis === 'Z') {
+            R = [
+                [c, -s, 0],
+                [s, c, 0],
+                [0, 0, 1]
+            ];
+        }
+
+        // Inertia matrix
+        const I = [
+            [ixx, ixy, ixz],
+            [ixy, iyy, iyz],
+            [ixz, iyz, izz]
+        ];
+
+        // Calculate R * I
+        const RI = [
+            [0, 0, 0],
+            [0, 0, 0],
+            [0, 0, 0]
+        ];
+
+        for (let i = 0; i < 3; i++) {
+            for (let j = 0; j < 3; j++) {
+                for (let k = 0; k < 3; k++) {
+                    RI[i][j] += R[i][k] * I[k][j];
+                }
+            }
+        }
+
+        // Calculate (R * I) * R^T
+        const result = [
+            [0, 0, 0],
+            [0, 0, 0],
+            [0, 0, 0]
+        ];
+
+        for (let i = 0; i < 3; i++) {
+            for (let j = 0; j < 3; j++) {
+                for (let k = 0; k < 3; k++) {
+                    result[i][j] += RI[i][k] * R[j][k]; // R^T[k][j] = R[j][k]
+                }
+            }
+        }
+
+        return {
+            ixx: result[0][0],
+            iyy: result[1][1],
+            izz: result[2][2],
+            ixy: result[0][1],
+            ixz: result[0][2],
+            iyz: result[1][2]
+        };
+    }
+
+    /**
+     * Rotate inertia tensor: I_rotated = R * I * R^T
+     */
+    static rotateInertiaTensor(ixx, iyy, izz, ixy, ixz, iyz, quat) {
+        const {w, x, y, z} = quat;
+
+        // Build rotation matrix R (from quaternion)
+        const r11 = 1 - 2*(y*y + z*z);
+        const r12 = 2*(x*y - w*z);
+        const r13 = 2*(x*z + w*y);
+        const r21 = 2*(x*y + w*z);
+        const r22 = 1 - 2*(x*x + z*z);
+        const r23 = 2*(y*z - w*x);
+        const r31 = 2*(x*z - w*y);
+        const r32 = 2*(y*z + w*x);
+        const r33 = 1 - 2*(x*x + y*y);
+
+        // Inertia matrix
+        const I = [
+            [ixx, ixy, ixz],
+            [ixy, iyy, iyz],
+            [ixz, iyz, izz]
+        ];
+
+        // Calculate R * I
+        const RI = [
+            [0, 0, 0],
+            [0, 0, 0],
+            [0, 0, 0]
+        ];
+
+        RI[0][0] = r11*I[0][0] + r12*I[1][0] + r13*I[2][0];
+        RI[0][1] = r11*I[0][1] + r12*I[1][1] + r13*I[2][1];
+        RI[0][2] = r11*I[0][2] + r12*I[1][2] + r13*I[2][2];
+
+        RI[1][0] = r21*I[0][0] + r22*I[1][0] + r23*I[2][0];
+        RI[1][1] = r21*I[0][1] + r22*I[1][1] + r23*I[2][1];
+        RI[1][2] = r21*I[0][2] + r22*I[1][2] + r23*I[2][2];
+
+        RI[2][0] = r31*I[0][0] + r32*I[1][0] + r33*I[2][0];
+        RI[2][1] = r31*I[0][1] + r32*I[1][1] + r33*I[2][1];
+        RI[2][2] = r31*I[0][2] + r32*I[1][2] + r33*I[2][2];
+
+        // Calculate (R * I) * R^T
+        const result = {
+            ixx: RI[0][0]*r11 + RI[0][1]*r12 + RI[0][2]*r13,
+            iyy: RI[1][0]*r21 + RI[1][1]*r22 + RI[1][2]*r23,
+            izz: RI[2][0]*r31 + RI[2][1]*r32 + RI[2][2]*r33,
+            ixy: RI[0][0]*r21 + RI[0][1]*r22 + RI[0][2]*r23,
+            ixz: RI[0][0]*r31 + RI[0][1]*r32 + RI[0][2]*r33,
+            iyz: RI[1][0]*r31 + RI[1][1]*r32 + RI[1][2]*r33
+        };
+
+        return result;
+    }
+
+    /**
+     * Parse joint element
+     */
+    static parseJoints(element, bodyMap, model, parentBodyName = null, defaultsMap = null) {
+        const joints = element.querySelectorAll(':scope > joint');
+
+        joints.forEach(jointEl => {
+            const jointName = jointEl.getAttribute('name') || `joint_${model.joints.size}`;
+            const jointType = jointEl.getAttribute('type') || 'hinge';
+
+            // Map MJCF joint types to URDF types
+            let urdfType = 'revolute';
+            if (jointType === 'slide') urdfType = 'prismatic';
+            else if (jointType === 'free') urdfType = 'continuous';
+            else if (jointType === 'ball' || jointType === 'hinge') urdfType = 'revolute';
+
+            const joint = new Joint(jointName, urdfType);
+
+            // Joint types that don't require axis attribute
+            const jointTypesWithoutAxis = ['free', 'ball'];
+            const requiresAxis = !jointTypesWithoutAxis.includes(jointType);
+
+            // [Critical fix] In MJCF, joint is defined inside body, representing the connection relationship between this body and its parent body
+            // So: parent is parent body, child is current body
+            const currentBody = jointEl.parentElement;
+            const currentBodyName = currentBody.getAttribute('name');
+
+            // parent is the passed parent body name (or worldbody)
+            if (parentBodyName) {
+                joint.parent = parentBodyName;
+            } else {
+                // If no parent body, parent is worldbody
+                joint.parent = 'worldbody';
+            }
+
+            // child is current body
+            if (currentBodyName) {
+                joint.child = currentBodyName;
+            }
+
+
+            // [Important] Parse axis, consider class inheritance
+            let axisVals = null;
+            let axisSource = '';
+
+            // First try to get axis from joint element itself
+            const axis = jointEl.getAttribute('axis');
+            if (axis) {
+                axisVals = axis.split(' ').map(parseFloat);
+                axisSource = 'directly defined';
+            } else {
+                // If not, inherit from class or childclass
+                let className = jointEl.getAttribute('class');
+
+                // If joint has no class, check parent body's childclass
+                if (!className) {
+                    className = currentBody.getAttribute('childclass');
+                }
+
+                if (className && defaultsMap) {
+                    const defaults = defaultsMap.get(className);
+                    if (defaults && defaults.joint && defaults.joint.axis) {
+                        axisVals = defaults.joint.axis;
+                        axisSource = `inherited from class="${className}"`;
+                    }
+                }
+
+                // Only warn if axis is required for this joint type
+                if (!axisVals && requiresAxis) {
+                    console.warn(`  ⚠️ Joint "${jointName}" (type="${jointType}") has no axis attribute (class="${className || 'none'}")`);
+                }
+            }
+
+            // Set axis
+            if (axisVals) {
+                joint.axis = { xyz: [axisVals[0] || 0, axisVals[1] || 0, axisVals[2] || 0] };
+            }
+
+            // [Important] Parse limits, consider class inheritance
+            let rangeVals = null;
+
+            // First try to get range from joint element itself
+            const range = jointEl.getAttribute('range');
+            if (range) {
+                rangeVals = range.split(' ').map(parseFloat);
+            } else {
+                // If not, inherit from class or childclass
+                let className = jointEl.getAttribute('class');
+
+                // If joint has no class, check parent body's childclass
+                if (!className) {
+                    className = currentBody.getAttribute('childclass');
+                }
+
+                if (className && defaultsMap) {
+                    const defaults = defaultsMap.get(className);
+                    if (defaults && defaults.joint && defaults.joint.range) {
+                        rangeVals = defaults.joint.range;
+                    }
+                }
+            }
+
+            // Set limits
+            if (rangeVals && rangeVals.length >= 2) {
+                const limits = new JointLimits();
+                limits.lower = rangeVals[0];
+                limits.upper = rangeVals[1];
+                joint.limits = limits;
+            }
+            // If no range definition, joint.limits remains null (indicating unlimited/continuous)
+
+            // Parse joint's own origin (if any)
+            // joint's pos defines the offset of joint in this body's coordinate system
+            joint.origin = this.parseOrigin(jointEl);
+
+            model.addJoint(joint);
+        });
+
+        // Process freejoint elements (free-floating joints)
+        const freejoints = element.querySelectorAll(':scope > freejoint');
+        freejoints.forEach((freejointEl, index) => {
+            const freejointName = freejointEl.getAttribute('name') || `freejoint_${model.joints.size}`;
+            
+            // Create a 'free' type joint (maps to continuous/floating in URDF terms)
+            const joint = new Joint(freejointName, 'continuous');
+            joint.type = 'free'; // Mark as free joint type
+            
+            // Get parent body
+            const currentBody = freejointEl.parentElement;
+            const currentBodyName = currentBody.getAttribute('name');
+            
+            // Parent is worldbody for freejoints
+            if (parentBodyName) {
+                joint.parent = parentBodyName;
+            } else {
+                joint.parent = 'worldbody';
+            }
+            
+            // Child is current body
+            if (currentBodyName) {
+                joint.child = currentBodyName;
+            }
+            
+            // Parse origin
+            joint.origin = this.parseOrigin(freejointEl);
+            
+            model.addJoint(joint);
+        });
+
+        // Recursively process child bodies
+        // Find direct child bodies (use :scope > body to ensure only direct children are selected)
+        const bodies = element.querySelectorAll(':scope > body');
+        const currentElementName = element.getAttribute('name'); // Name of current body or worldbody
+
+        bodies.forEach(body => {
+            // Child body's parent body name is current element's name
+            // Note: worldbody has no name attribute, so first level body's parent is null or 'worldbody'
+            this.parseJoints(body, bodyMap, model, currentElementName || 'worldbody', defaultsMap);
+        });
+    }
+
+    /**
+     * Parse equality constraints (closed-chain constraints for parallel mechanisms)
+     */
+    static parseEquality(doc, model) {
+        const equality = doc.querySelector('equality');
+        if (!equality) {
+            return; // No equality tag, skip
+        }
+
+        // Parse connect constraints (connect two bodies)
+        const connects = equality.querySelectorAll('connect');
+        connects.forEach((connectEl, index) => {
+            const name = connectEl.getAttribute('name') || `connect_${index}`;
+            const constraint = new Constraint(name, 'connect');
+
+            constraint.body1 = connectEl.getAttribute('body1');
+            constraint.body2 = connectEl.getAttribute('body2');
+
+            const anchor = connectEl.getAttribute('anchor');
+            if (anchor) {
+                constraint.anchor = anchor.trim().split(/\s+/).map(parseFloat);
+            }
+
+            const torquescale = connectEl.getAttribute('torquescale');
+            if (torquescale) {
+                constraint.torquescale = parseFloat(torquescale);
+            }
+
+            constraint.userData = {
+                body1: constraint.body1,
+                body2: constraint.body2,
+                anchor: constraint.anchor
+            };
+
+            model.addConstraint(constraint);
+        });
+
+        // Parse weld constraints (weld two bodies)
+        const welds = equality.querySelectorAll('weld');
+        welds.forEach((weldEl, index) => {
+            const name = weldEl.getAttribute('name') || `weld_${index}`;
+            const constraint = new Constraint(name, 'weld');
+
+            constraint.body1 = weldEl.getAttribute('body1');
+            constraint.body2 = weldEl.getAttribute('body2');
+
+            const anchor = weldEl.getAttribute('anchor');
+            if (anchor) {
+                constraint.anchor = anchor.trim().split(/\s+/).map(parseFloat);
+            }
+
+            const torquescale = weldEl.getAttribute('torquescale');
+            if (torquescale) {
+                constraint.torquescale = parseFloat(torquescale);
+            }
+
+            constraint.userData = {
+                body1: constraint.body1,
+                body2: constraint.body2,
+                anchor: constraint.anchor
+            };
+
+            model.addConstraint(constraint);
+        });
+
+        // Parse joint constraints (joint coupling)
+        const joints = equality.querySelectorAll('joint');
+        joints.forEach((jointEl, index) => {
+            const name = jointEl.getAttribute('name') || `joint_constraint_${index}`;
+            const constraint = new Constraint(name, 'joint');
+
+            constraint.joint1 = jointEl.getAttribute('joint1');
+            constraint.joint2 = jointEl.getAttribute('joint2');
+
+            const polycoef = jointEl.getAttribute('polycoef');
+            if (polycoef) {
+                constraint.polycoef = polycoef.trim().split(/\s+/).map(parseFloat);
+            } else {
+                constraint.polycoef = [0, 1]; // Default 1:1
+            }
+
+            constraint.userData = {
+                joint1: constraint.joint1,
+                joint2: constraint.joint2,
+                polycoef: constraint.polycoef
+            };
+
+            model.addConstraint(constraint);
+        });
+
+        // Parse distance constraints
+        const distances = equality.querySelectorAll('distance');
+        distances.forEach((distanceEl, index) => {
+            const name = distanceEl.getAttribute('name') || `distance_${index}`;
+            const constraint = new Constraint(name, 'distance');
+
+            constraint.body1 = distanceEl.getAttribute('body1');
+            constraint.body2 = distanceEl.getAttribute('body2');
+
+            constraint.userData = {
+                body1: constraint.body1,
+                body2: constraint.body2
+            };
+
+            model.addConstraint(constraint);
+        });
+    }
+
+    /**
+     * Create Three.js objects (recursively build hierarchy)
+     * @param {UnifiedRobotModel} model
+     * @param {Map} fileMap - File map for loading mesh files
+     * @param {Map} meshMap - Mesh name to file path mapping (optional)
+     */
+    static async createThreeObject(model, fileMap = null, meshMap = null) {
+        // Preload loaders
+        await getLoaders();
+
+        const rootGroup = new THREE.Group();
+        rootGroup.name = model.name;
+
+        // Create Three.js objects for all links (but don't add to scene yet)
+        const linkObjects = new Map();
+
+        // Collect all unique mesh file paths (only need visual, as MJCF doesn't create collision separately)
+        const uniqueMeshFiles = new Set();
+        for (const [name, link] of model.links) {
+            for (const visual of link.visuals) {
+                if (visual.geometry.type === 'mesh' && visual.geometry.filename) {
+                    uniqueMeshFiles.add(visual.geometry.filename);
+                }
+            }
+        }
+
+        // Load all unique mesh files in parallel
+        const meshPromises = Array.from(uniqueMeshFiles).map(filename =>
+            this.loadMeshFile(filename, fileMap).catch(err => {
+                console.error(`Failed to load mesh: ${filename}`, err);
+                return null;
+            })
+        );
+
+        // Wait for all mesh loading to complete
+        const meshResults = await Promise.all(meshPromises);
+        const meshCache = new Map();
+
+        // Build mesh cache (filename -> geometry)
+        let index = 0;
+        for (const filename of uniqueMeshFiles) {
+            const result = meshResults[index++];
+            meshCache.set(filename, result);
+        }
+
+        // Create link groups
+        let totalVisuals = 0;
+        for (const [name, link] of model.links) {
+            const linkGroup = new THREE.Group();
+            linkGroup.name = name;
+            linkGroup.isURDFLink = true; // Mark as link for JointDragControls recognition
+            linkGroup.type = 'URDFLink'; // Set type
+
+            // [Critical] Do not apply body.pos on linkGroup!
+            // body.pos should be applied on the jointGroup that connects it
+            // linkGroup only needs to contain geometry, position is determined by jointGroup
+
+            let linkVisualCount = 0;
+            let linkCollisionCount = 0;
+
+            // Create visual geometry
+            for (const visual of link.visuals) {
+                const mesh = await this.createGeometryMesh(visual.geometry, fileMap, meshCache);
+                if (mesh) {
+                    // Apply origin transformation
+                    // Check if this geom has fromto data (for capsule/cylinder)
+                    if (visual.geometry && visual.geometry.fromto) {
+                        // Use fromto center position
+                        mesh.position.set(...visual.geometry.fromto.center);
+                        // Apply fromto rotation plus any explicit rotation
+                        const fromtoRpy = visual.geometry.fromto.rpy;
+                        mesh.rotation.set(
+                            fromtoRpy[0] + visual.origin.rpy[0],
+                            fromtoRpy[1] + visual.origin.rpy[1],
+                            fromtoRpy[2] + visual.origin.rpy[2]
+                        );
+                    } else {
+                        mesh.position.set(...visual.origin.xyz);
+                        mesh.rotation.set(...visual.origin.rpy);
+                    }
+                    mesh.name = visual.name || 'visual';
+
+                    // If MJCF defines rgba color, apply to mesh
+                    if (visual.userData && visual.userData.rgba) {
+                        const rgba = visual.userData.rgba;
+                        const color = new THREE.Color(rgba.r, rgba.g, rgba.b);
+
+                        mesh.traverse((child) => {
+                            if (child.isMesh && child.material) {
+                                // Handle material arrays and single materials
+                                if (Array.isArray(child.material)) {
+                                    child.material = child.material.map(mat => {
+                                        const clonedMat = mat.clone();
+                                        clonedMat.color = color;
+                                        if (rgba.a < 1.0) {
+                                            clonedMat.transparent = true;
+                                            clonedMat.opacity = rgba.a;
+                                        }
+                                        // Save original properties before enhancing (for lighting toggle)
+                                        if (clonedMat.isMeshPhongMaterial || clonedMat.isMeshStandardMaterial) {
+                                            if (clonedMat.userData.originalShininess === undefined) {
+                                                clonedMat.userData.originalShininess = clonedMat.shininess !== undefined ? clonedMat.shininess : 30;
+                                                // Save original specular - if material had no specular, save null
+                                                if (!clonedMat.specular) {
+                                                    clonedMat.userData.originalSpecular = null;
+                                                } else if (clonedMat.specular.isColor) {
+                                                    const spec = clonedMat.specular;
+                                                    if (spec.r < 0.1 && spec.g < 0.1 && spec.b < 0.1) {
+                                                        clonedMat.userData.originalSpecular = null; // Likely default
+                                                    } else {
+                                                        clonedMat.userData.originalSpecular = spec.clone();
+                                                    }
+                                                } else if (typeof clonedMat.specular === 'number') {
+                                                    if (clonedMat.specular === 0x111111 || clonedMat.specular < 0x111111) {
+                                                        clonedMat.userData.originalSpecular = null;
+                                                    } else {
+                                                        clonedMat.userData.originalSpecular = new THREE.Color(clonedMat.specular);
+                                                    }
+                                                } else {
+                                                    clonedMat.userData.originalSpecular = null;
+                                                }
+                                            }
+                                            // Enhance material for better lighting (MuJoCo style) - default enabled
+                                            if (clonedMat.shininess === undefined || clonedMat.shininess < 50) {
+                                                clonedMat.shininess = 50;
+                                            }
+                                            if (!clonedMat.specular ||
+                                                (clonedMat.specular.isColor && clonedMat.specular.r < 0.2) ||
+                                                (typeof clonedMat.specular === 'number' && clonedMat.specular < 0x333333)) {
+                                                clonedMat.specular = new THREE.Color(0.3, 0.3, 0.3);
+                                            }
+                                        }
+                                        return clonedMat;
+                                    });
+                                } else {
+                                    // Clone material to avoid affecting other instances
+                                    child.material = child.material.clone();
+                                    child.material.color = color;
+                                    if (rgba.a < 1.0) {
+                                        child.material.transparent = true;
+                                        child.material.opacity = rgba.a;
+                                    }
+                                    // Save original properties before enhancing (for lighting toggle)
+                                    if (child.material.isMeshPhongMaterial || child.material.isMeshStandardMaterial) {
+                                        if (child.material.userData.originalShininess === undefined) {
+                                            child.material.userData.originalShininess = child.material.shininess !== undefined ? child.material.shininess : 30;
+                                            // Save original specular - if material had no specular, save null
+                                            if (!child.material.specular) {
+                                                child.material.userData.originalSpecular = null;
+                                            } else if (child.material.specular.isColor) {
+                                                const spec = child.material.specular;
+                                                if (spec.r < 0.1 && spec.g < 0.1 && spec.b < 0.1) {
+                                                    child.material.userData.originalSpecular = null; // Likely default
+                                                } else {
+                                                    child.material.userData.originalSpecular = spec.clone();
+                                                }
+                                            } else if (typeof child.material.specular === 'number') {
+                                                if (child.material.specular === 0x111111 || child.material.specular < 0x111111) {
+                                                    child.material.userData.originalSpecular = null;
+                                                } else {
+                                                    child.material.userData.originalSpecular = new THREE.Color(child.material.specular);
+                                                }
+                                            } else {
+                                                child.material.userData.originalSpecular = null;
+                                            }
+                                        }
+                                        // Enhance material for better lighting (MuJoCo style) - default enabled
+                                        if (child.material.shininess === undefined || child.material.shininess < 50) {
+                                            child.material.shininess = 50;
+                                        }
+                                        if (!child.material.specular ||
+                                            (child.material.specular.isColor && child.material.specular.r < 0.2) ||
+                                            (typeof child.material.specular === 'number' && child.material.specular < 0x333333)) {
+                                            child.material.specular = new THREE.Color(0.3, 0.3, 0.3);
+                                        }
+                                    }
+                                }
+                            }
+                        });
+                    }
+
+                    linkGroup.add(mesh);
+                    visual.threeObject = mesh;
+                    totalVisuals++;
+                    linkVisualCount++;
+                }
+            }
+
+            // Create collision geometry
+            for (const collision of link.collisions) {
+                const mesh = await this.createGeometryMesh(collision.geometry, fileMap, meshCache);
+                if (mesh) {
+                    // Apply origin transformation
+                    // Check if this geom has fromto data (for capsule/cylinder)
+                    if (collision.geometry && collision.geometry.fromto) {
+                        // Use fromto center position
+                        mesh.position.set(...collision.geometry.fromto.center);
+                        // Apply fromto rotation plus any explicit rotation
+                        const fromtoRpy = collision.geometry.fromto.rpy;
+                        mesh.rotation.set(
+                            fromtoRpy[0] + collision.origin.rpy[0],
+                            fromtoRpy[1] + collision.origin.rpy[1],
+                            fromtoRpy[2] + collision.origin.rpy[2]
+                        );
+                    } else {
+                        mesh.position.set(...collision.origin.xyz);
+                        mesh.rotation.set(...collision.origin.rpy);
+                    }
+                    mesh.name = collision.name || 'collision';
+
+                    // Create collision body container (similar to URDF handling)
+                    const colliderGroup = new THREE.Group();
+                    colliderGroup.name = `${name}_collider_${linkCollisionCount}`;
+                    colliderGroup.isURDFCollider = true; // Mark as collision body
+                    colliderGroup.add(mesh);
+
+                    linkGroup.add(colliderGroup);
+                    collision.threeObject = colliderGroup;
+                    linkCollisionCount++;
+                }
+            }
+
+            link.threeObject = linkGroup;
+            linkObjects.set(name, linkGroup);
+        }
+
+
+        // Build hierarchy based on body parent-child relationships (MJCF bodies are nested)
+        const bodyMap = new Map();
+        for (const [name, link] of model.links) {
+            bodyMap.set(name, { link, parentName: link.userData.parentName });
+        }
+
+        // Find root body (body without parent)
+        const rootLinks = Array.from(model.links.keys()).filter(
+            name => !bodyMap.get(name).parentName
+        );
+
+        // Recursively build hierarchy
+        function buildHierarchy(linkName, parentGroup) {
+            const linkGroup = linkObjects.get(linkName);
+            if (!linkGroup) return;
+
+            // Add current link to parent group
+            parentGroup.add(linkGroup);
+
+            // Find all joints with this link as parent
+            const childJoints = Array.from(model.joints.values()).filter(
+                j => j.parent === linkName && j.child
+            );
+
+            // Process child joints and child bodies
+            childJoints.forEach(joint => {
+                const childLinkName = joint.child;
+                if (!childLinkName) return;
+
+                // Get child link's body origin (in MJCF, body.pos defines connection position)
+                const childLink = model.links.get(childLinkName);
+                const bodyOrigin = childLink.userData.bodyOrigin || { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+
+                // Create joint transformation group
+                const jointGroup = new THREE.Group();
+                jointGroup.name = joint.name || `joint_${childLinkName}`;
+                jointGroup.isURDFJoint = true; // Mark as joint for JointDragControls recognition
+                jointGroup.type = 'URDFJoint'; // Set type
+                jointGroup.jointType = joint.type; // Set joint type
+
+                // Store joint axis information (for JointDragControls use)
+                if (joint.axis && joint.axis.xyz) {
+                    const mjcfAxis = joint.axis.xyz;
+                    jointGroup.axis = new THREE.Vector3(mjcfAxis[0], mjcfAxis[1], mjcfAxis[2]).normalize();
+                } else {
+                    // If no axis defined, use default value (0, 1, 0)
+                    jointGroup.axis = new THREE.Vector3(0, 1, 0);
+                }
+
+                // [Critical] Apply body.pos + joint.pos as jointGroup position
+                // body.pos defines body position relative to parent body (i.e., connection position)
+                // joint.pos defines joint offset in body coordinate system (usually 0)
+                jointGroup.position.set(
+                    bodyOrigin.xyz[0] + joint.origin.xyz[0],
+                    bodyOrigin.xyz[1] + joint.origin.xyz[1],
+                    bodyOrigin.xyz[2] + joint.origin.xyz[2]
+                );
+                jointGroup.rotation.set(...bodyOrigin.rpy);
+
+                // Recursively build child link
+                buildHierarchy(childLinkName, jointGroup);
+
+                linkGroup.add(jointGroup);
+                joint.threeObject = jointGroup;
+            });
+
+            // Process direct child bodies (find via bodyMap)
+            for (const [childName, bodyData] of bodyMap.entries()) {
+                if (bodyData.parentName === linkName) {
+                    // Check if joint connection already exists
+                    const hasJoint = Array.from(model.joints.values()).some(
+                        j => j.parent === linkName && j.child === childName
+                    );
+                    if (!hasJoint) {
+                        // If no joint, create fixed connection group to apply body position and rotation
+                        const childLink = model.links.get(childName);
+                        const childBodyOrigin = childLink.userData.bodyOrigin || { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+
+                        // Mark this as fixed-connected child body (for structure graph display)
+                        childLink.userData.isFixedConnection = true;
+
+                        // Create fixed connection group
+                        const fixedGroup = new THREE.Group();
+                        fixedGroup.position.set(...childBodyOrigin.xyz);
+                        fixedGroup.rotation.set(...childBodyOrigin.rpy);
+
+                        // Recursively build child body and add to fixed group
+                        buildHierarchy(childName, fixedGroup);
+
+                        linkGroup.add(fixedGroup);
+                    }
+                }
+            }
+        }
+
+        // Start building from root link
+        if (rootLinks.length > 0) {
+            rootLinks.forEach(rootName => {
+                // Root link needs to apply its own body.pos (because it has no parent joint)
+                const rootLink = model.links.get(rootName);
+                const rootLinkGroup = linkObjects.get(rootName);
+                if (rootLink.userData.bodyOrigin) {
+                    rootLinkGroup.position.set(...rootLink.userData.bodyOrigin.xyz);
+                    rootLinkGroup.rotation.set(...rootLink.userData.bodyOrigin.rpy);
+                }
+                buildHierarchy(rootName, rootGroup);
+            });
+        } else if (model.links.size > 0) {
+            // If no root link found, use first link
+            const firstLink = Array.from(model.links.keys())[0];
+            const firstLinkObj = model.links.get(firstLink);
+            const firstLinkGroup = linkObjects.get(firstLink);
+            if (firstLinkObj.userData.bodyOrigin) {
+                firstLinkGroup.position.set(...firstLinkObj.userData.bodyOrigin.xyz);
+                firstLinkGroup.rotation.set(...firstLinkObj.userData.bodyOrigin.rpy);
+            }
+            buildHierarchy(firstLink, rootGroup);
+        }
+
+        model.threeObject = rootGroup;
+
+        // Mark model type as MJCF (also set on model)
+        if (!rootGroup.userData) rootGroup.userData = {};
+        rootGroup.userData.type = 'mjcf';
+
+        if (!model.userData) model.userData = {};
+        model.userData.type = 'mjcf';
+    }
+
+    /**
+     * Create Three.js Mesh based on geometry type
+     * @param {GeometryType} geometry
+     * @param {Map} fileMap - File map for loading mesh files
+     * @param {Map} meshCache - Cache of loaded meshes (optional)
+     * @returns {Promise}
+     */
+    static async createGeometryMesh(geometry, fileMap = null, meshCache = null) {
+        let threeGeometry = null;
+
+        switch (geometry.type) {
+            case 'box':
+                if (geometry.size) {
+                    threeGeometry = new THREE.BoxGeometry(
+                        geometry.size.x,
+                        geometry.size.y,
+                        geometry.size.z
+                    );
+                }
+                break;
+
+            case 'sphere':
+                if (geometry.size && geometry.size.radius) {
+                    threeGeometry = new THREE.SphereGeometry(geometry.size.radius, 32, 32);
+                }
+                break;
+
+            case 'cylinder':
+                if (geometry.size) {
+                    // Three.js CylinderGeometry defaults to Y-axis
+                    threeGeometry = new THREE.CylinderGeometry(
+                        geometry.size.radius,
+                        geometry.size.radius,
+                        geometry.size.height,
+                        32
+                    );
+                    // MJCF cylinder defaults to Z-axis, Three.js Cylinder is Y-axis aligned
+                    // Rotate to align with Z-axis
+                    threeGeometry.rotateX(Math.PI / 2);
+                    
+                    // If fromto is defined, the mesh will be positioned and rotated by fromto data
+                    // in the calling code
+                }
+                break;
+
+            case 'capsule':
+                if (geometry.size) {
+                    // Three.js doesn't have native CapsuleGeometry in older versions
+                    // Use a combination of cylinder and spheres, or CapsuleGeometry if available
+                    const { radius, height } = geometry.size;
+                    
+                    // Check if CapsuleGeometry is available (Three.js r133+)
+                    if (typeof THREE.CapsuleGeometry !== 'undefined') {
+                        threeGeometry = new THREE.CapsuleGeometry(radius, height, 4, 16);
+                        // CapsuleGeometry is Y-axis aligned, MJCF capsule is Z-axis aligned
+                        threeGeometry.rotateX(Math.PI / 2);
+                    } else {
+                        // Fallback: create a cylinder with sphere caps
+                        const cylinderHeight = Math.max(0, height - 2 * radius);
+                        const cylinder = new THREE.CylinderGeometry(radius, radius, cylinderHeight, 16);
+                        cylinder.rotateX(Math.PI / 2); // Align with Z-axis
+                        threeGeometry = cylinder;
+                    }
+                }
+                break;
+
+            case 'mesh':
+                // Load mesh file
+                if (geometry.filename) {
+                    let cachedMesh = null;
+
+                    // If already cached, get it
+                    if (meshCache && meshCache.has(geometry.filename)) {
+                        cachedMesh = meshCache.get(geometry.filename);
+                    } else if (fileMap) {
+                        cachedMesh = await this.loadMeshFile(geometry.filename, fileMap);
+                    }
+
+                    if (!cachedMesh) {
+                        console.error(`❌ Cannot load mesh file: ${geometry.filename}`);
+                        return null;
+                    }
+
+                    // loadMeshFile may return Group/Scene (OBJ/DAE/GLTF) or BufferGeometry (STL)
+                    // If Group/Scene, need to clone (because Three.js objects can only have one parent)
+                    if (cachedMesh.isGroup || cachedMesh.isObject3D) {
+                        threeGeometry = cachedMesh.clone(true); // Deep clone (including materials)
+
+                        // Apply mesh scale from MJCF class inheritance (e.g., scale="0.001 0.001 0.001")
+                        if (geometry.meshScale) {
+                            const [sx, sy, sz] = geometry.meshScale;
+                            threeGeometry.scale.set(sx, sy, sz);
+                        }
+
+                        // Check cloned mesh material situation
+                        let meshCount = 0;
+                        let materialCount = 0;
+                        threeGeometry.traverse((child) => {
+                            if (child.isMesh) {
+                                meshCount++;
+                                if (child.material) {
+                                    materialCount++;
+                                }
+                            }
+                        });
+
+                        // Ensure mesh uses lighting-compatible material
+                        ensureMeshHasPhongMaterial(threeGeometry);
+                        return threeGeometry;
+                    }
+                    // If BufferGeometry (e.g., STL), create a mesh and apply scale
+                    if (geometry.meshScale) {
+                        const [sx, sy, sz] = geometry.meshScale;
+                        // Scale the geometry directly
+                        threeGeometry = cachedMesh.clone();
+                        threeGeometry.scale(sx, sy, sz);
+                    } else {
+                        threeGeometry = cachedMesh;
+                    }
+                } else {
+                    console.warn('⚠️ Mesh type geometry missing filename');
+                    return null;
+                }
+                break;
+        }
+
+        if (!threeGeometry) return null;
+
+        // Create default material for BufferGeometry (basic geometries: box, sphere, cylinder, stl, etc.)
+        // Enhanced for better lighting (MuJoCo style) with reflections
+        const envMap = typeof window !== 'undefined' && window.app?.sceneManager?.environmentManager?.getEnvironmentMap();
+        const material = new THREE.MeshPhongMaterial({
+            color: 0xf0f0f0,  // Near white
+            shininess: 50,  // Increased for better highlights
+            specular: new THREE.Color(0.3, 0.3, 0.3),  // Enhanced specular reflection
+            envMap: envMap || null,
+            reflectivity: envMap ? 0.3 : 0
+        });
+        // Save original properties for lighting toggle
+        material.userData.originalShininess = 30;
+        material.userData.originalSpecular = null; // New material, no original specular
+        return new THREE.Mesh(threeGeometry, material);
+    }
+
+    /**
+     * Load mesh file from fileMap (using universal loader)
+     */
+    static async loadMeshFile(meshPath, fileMap) {
+        return loadMeshFile(meshPath, fileMap);
+    }
+
+    /**
+     * Set joint angle
+     */
+    static setJointAngle(joint, angle) {
+        joint.currentValue = angle;
+
+        if (joint.threeObject) {
+            // Rotate based on joint type and axis
+            if (joint.type === 'revolute' || joint.type === 'continuous') {
+                // Use axis stored on threeObject (already converted), if not available convert from joint.axis
+                let axis;
+                if (joint.threeObject.axis) {
+                    axis = joint.threeObject.axis.clone().normalize();
+                } else if (joint.axis && joint.axis.xyz) {
+                    // If no pre-stored axis, need coordinate system conversion
+                    const mjcfAxis = joint.axis.xyz;
+                    axis = new THREE.Vector3(mjcfAxis[0], mjcfAxis[2], -mjcfAxis[1]).normalize();
+                } else {
+                    console.warn('Joint has no axis definition:', joint.name);
+                    return;
+                }
+
+                // Save initial rotation (only save on first call)
+                if (!joint.threeObject.userData.initialQuaternion) {
+                    joint.threeObject.userData.initialQuaternion = joint.threeObject.quaternion.clone();
+                }
+
+                // Set rotation using quaternion: initial rotation * joint rotation
+                const rotationQuat = new THREE.Quaternion();
+                rotationQuat.setFromAxisAngle(axis, angle);
+
+                // Combine rotations: apply initial rotation first, then joint rotation
+                joint.threeObject.quaternion.copy(joint.threeObject.userData.initialQuaternion);
+                joint.threeObject.quaternion.multiply(rotationQuat);
+
+                // Update matrix
+                joint.threeObject.updateMatrixWorld(true);
+            } else if (joint.type === 'prismatic') {
+                // Use axis stored on threeObject (already converted) or convert from joint.axis
+                let axis;
+                if (joint.threeObject.axis) {
+                    axis = joint.threeObject.axis.clone().normalize();
+                } else if (joint.axis && joint.axis.xyz) {
+                    // If no pre-stored axis, need coordinate system conversion
+                    const mjcfAxis = joint.axis.xyz;
+                    axis = new THREE.Vector3(mjcfAxis[0], mjcfAxis[2], -mjcfAxis[1]).normalize();
+                } else {
+                    console.warn('Joint has no axis definition:', joint.name);
+                    return;
+                }
+
+                // Save initial position (only save on first call)
+                if (!joint.threeObject.userData.initialPosition) {
+                    joint.threeObject.userData.initialPosition = joint.threeObject.position.clone();
+                }
+
+                // Translate joint: initial position + move along axis
+                joint.threeObject.position.copy(joint.threeObject.userData.initialPosition);
+                joint.threeObject.position.addScaledVector(axis, angle);
+
+                // Update matrix
+                joint.threeObject.updateMatrixWorld(true);
+            }
+        }
+    }
+}
+
diff --git a/05_software/real/sim2real_v2/web/static/viewer/MeshLoader.js b/05_software/real/sim2real_v2/web/static/viewer/MeshLoader.js
new file mode 100644
index 0000000..7505935
--- /dev/null
+++ b/05_software/real/sim2real_v2/web/static/viewer/MeshLoader.js
@@ -0,0 +1,106 @@
+/**
+ * Adapted MeshLoader for sim2real web console.
+ * Supports both fileMap-based loading (original robot_viewer API) and URL-based
+ * fetching from the sim2real HTTP server at /meshes/.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}
+ */
+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 };
diff --git a/05_software/real/sim2real_v2/web/static/viewer/RobotViewer3D.js b/05_software/real/sim2real_v2/web/static/viewer/RobotViewer3D.js
new file mode 100644
index 0000000..3189de9
--- /dev/null
+++ b/05_software/real/sim2real_v2/web/static/viewer/RobotViewer3D.js
@@ -0,0 +1,181 @@
+/**
+ * RobotViewer3D — sim2real 3D 可视化（基于 robot_viewer 的 MJCFAdapter + Three.js）
+ *
+ * 加载 wheelleg.xml → MJCFAdapter.parse → Three.js 场景树
+ * 建立 jointName → THREE.Object3D 映射，通过 updateJoints(pos16) 实时更新。
+ * 支持 OrbitControls 旋转/缩放/平移。
+ *
+ * 用法：
+ *   const viewer = new RobotViewer3D(canvasElement);
+ *   await viewer.load('/mjcf/wheelleg.xml');
+ *   viewer.updateJoints(jointPositions16);
+ */
+import * as THREE from 'three';
+import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
+import { MJCFAdapter } from './MJCFAdapter.js';
+import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
+
+// 16 关节的标准顺序（与 motor_mapping.py:SIM_JOINT_ORDER 对齐）
+const JOINT_ORDER = [
+    'fl_hip_abduction_joint', 'fl_hip_pitch_joint', 'fl_knee_joint',
+    'fr_hip_abduction_joint', 'fr_hip_pitch_joint', 'fr_knee_joint',
+    'rl_hip_abduction_joint', 'rl_hip_pitch_joint', 'rl_knee_joint',
+    'rr_hip_abduction_joint', 'rr_hip_pitch_joint', 'rr_knee_joint',
+    'fl_wheel_joint', 'fr_wheel_joint', 'rl_wheel_joint', 'rr_wheel_joint',
+];
+
+// MJCF → Three.js 坐标轴转换：让 MJCF 的 Z 轴(向上) 映射到 Three.js 的 Y 轴(向上)
+const MJCF_TO_THREE = new THREE.Matrix4().makeRotationX(-Math.PI / 2);
+// 或直接用 euler: (0, PI, 0)
+
+export class RobotViewer3D {
+    /**
+     * @param {HTMLCanvasElement} canvas
+     * @param {object} [opts]
+     * @param {string} [opts.meshBaseUrl='/meshes/']  STL mesh 文件的 HTTP 路径前缀
+     * @param {string} [opts.mjcfUrl='/mjcf/wheelleg.xml']
+     * @param {string} [opts.backgroundColor='#1a1d24']
+     */
+    constructor(canvas, opts = {}) {
+        this.canvas = canvas;
+        this.meshBaseUrl = opts.meshBaseUrl || '/meshes/';
+        this.mjcfUrl = opts.mjcfUrl || '/mjcf/wheelleg.xml';
+
+        // Three.js 核心
+        const w = canvas.clientWidth, h = canvas.clientHeight;
+        this.scene = new THREE.Scene();
+        // 移除背景色，使用透明背景，由 CSS 控制
+        // this.scene.background = new THREE.Color(opts.backgroundColor || '#1a1d24');
+
+        this.camera = new THREE.PerspectiveCamera(55, w / h, 0.05, 50);
+        this.camera.position.set(0.5, 0.35, 0.65);
+        this.camera.lookAt(0.2, 0, 0);
+
+        this.renderer = new THREE.WebGLRenderer({ canvas, antialias: true, alpha: true });
+        this.renderer.setSize(w, h);
+        this.renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2));
+        this.renderer.shadowMap.enabled = true;
+
+        // OrbitControls
+        this.controls = new OrbitControls(this.camera, canvas);
+        this.controls.target.set(0.15, 0.08, 0.0);
+        this.controls.enableDamping = true;
+        this.controls.dampingFactor = 0.12;
+        this.controls.update();
+
+        // 灯光
+        this._setupLights();
+
+        // 地面
+        const grid = new THREE.GridHelper(2, 20, 0x444444, 0x222222);
+        grid.position.y = -0.35;
+        this.scene.add(grid);
+
+        // 状态
+        this.model = null;
+        this.rootGroup = null;
+        this.jointMap = new Map();           // jointName → { joint, group }
+        this._isLoaded = false;
+        this._rafId = null;
+        this._stlCache = new Map();          // filename → BufferGeometry
+    }
+
+    _setupLights() {
+        const ambient = new THREE.AmbientLight(0x606060, 1.5);
+        this.scene.add(ambient);
+
+        const dir1 = new THREE.DirectionalLight(0xffffff, 2.5);
+        dir1.position.set(2, 3, 2);
+        this.scene.add(dir1);
+
+        const dir2 = new THREE.DirectionalLight(0x8899cc, 1.0);
+        dir2.position.set(-1, 1, -1);
+        this.scene.add(dir2);
+
+        const hemi = new THREE.HemisphereLight(0x8899cc, 0x334455, 1.2);
+        this.scene.add(hemi);
+    }
+
+    // ---- 加载模型 ----
+    async load(mjcfUrlOverride) {
+        const url = mjcfUrlOverride || this.mjcfUrl;
+        console.log('[RobotViewer3D] loading MJCF:', url);
+        const resp = await fetch(url);
+        if (!resp.ok) throw new Error(`MJCF 404: ${url}`);
+        const xmlText = await resp.text();
+
+        // 用 MJCFAdapter 解析 → UnifiedRobotModel
+        // fileMap 为空时不传；MeshLoader 会自动 fallback 到 URL 加载
+        const model = await MJCFAdapter.parse(xmlText, null);
+        this.model = model;
+        console.log('[RobotViewer3D] parsed:', model.links.size, 'links,', model.joints.size, 'joints');
+
+        // 取 rootGroup（MJCFAdapter.createThreeObject 已构建完整 hierarchy）
+        this.rootGroup = model.threeObject;
+        // 坐标轴转换：MJCF → Three.js
+        this.rootGroup.applyMatrix4(MJCF_TO_THREE);
+        this.scene.add(this.rootGroup);
+
+        // 遍历 joints，建立索引
+        this.jointMap.clear();
+        for (const [jointName, joint] of model.joints) {
+            if (joint.threeObject) {
+                this.jointMap.set(jointName, joint);
+            }
+        }
+        // 已建立映射的关节列表
+        const mapped = Array.from(this.jointMap.keys()).sort();
+        console.log('[RobotViewer3D] joint map:', mapped.length, 'joints');
+
+        this._isLoaded = true;
+        this._startRenderLoop();
+    }
+
+    // ---- 渲染循环（按需 + 持续） ----
+    _startRenderLoop() {
+        if (this._rafId) return;
+        const loop = () => {
+            this.controls.update();
+            this.renderer.render(this.scene, this.camera);
+            this._rafId = requestAnimationFrame(loop);
+        };
+        loop();
+    }
+
+    // ---- 实时更新关节角度 ----
+    /**
+     * @param {Float64Array|number[]} pos16 — 16 关节角度 (rad)，顺序同 SIM_JOINT_ORDER
+     *   索引 0-11: 腿关节 (fl_abd,fl_pitch,fl_knee,fr...,rl...,rr...)
+     *   索引 12-15: 轮子关节 (fl_wheel,fr_wheel,rl_wheel,rr_wheel)
+     */
+    updateJoints(pos16) {
+        if (!this._isLoaded) return;
+        for (let i = 0; i < JOINT_ORDER.length && i < pos16.length; i++) {
+            const name = JOINT_ORDER[i];
+            const joint = this.jointMap.get(name);
+            if (joint) {
+                MJCFAdapter.setJointAngle(joint, pos16[i]);
+            }
+        }
+    }
+
+    // ---- 重置相机 ----
+    resetCamera() {
+        this.camera.position.set(0.5, 0.35, 0.65);
+        this.controls.target.set(0.15, 0.08, 0.0);
+        this.controls.update();
+    }
+
+    // ---- 调整大小 ----
+    resize() {
+        const w = this.canvas.clientWidth, h = this.canvas.clientHeight;
+        this.camera.aspect = w / h;
+        this.camera.updateProjectionMatrix();
+        this.renderer.setSize(w, h);
+    }
+
+    dispose() {
+        if (this._rafId) cancelAnimationFrame(this._rafId);
+        this.renderer.dispose();
+    }
+}
diff --git a/05_software/real/sim2real_v2/web/static/viewer/UnifiedRobotModel.js b/05_software/real/sim2real_v2/web/static/viewer/UnifiedRobotModel.js
new file mode 100644
index 0000000..1ef44f8
--- /dev/null
+++ b/05_software/real/sim2real_v2/web/static/viewer/UnifiedRobotModel.js
@@ -0,0 +1,181 @@
+/**
+ * Unified robot model data interface
+ * All formats (URDF, MJCF, USD) are converted to this unified format
+ */
+export class UnifiedRobotModel {
+    constructor() {
+        this.name = '';
+        this.links = new Map(); // Map
+        this.joints = new Map(); // Map
+        this.materials = new Map(); // Map
+        this.constraints = new Map(); // Map - 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 = {};
+    }
+}
+