s100 cpp sdk更新

.gitignore

@@ -1 +1,30 @@
-logs/
+logs/
+__pycache__/
+*.pyc
+.firecrawl/
+deploy_45dim_rl_gym/bpu_quantization/.hb_check/
+deploy_45dim_rl_gym/bpu_quantization/calibration_data/
+deploy_45dim_rl_gym/bpu_quantization/calibration_data_fast*/
+deploy_45dim_rl_gym/bpu_quantization/calibration_data_*_gym_fast*/
+deploy_45dim_rl_gym/bpu_quantization/calibration_data_*_gym_fast*_metadata.json
+deploy_45dim_rl_gym/bpu_quantization/calibration_data_*_robotlab_fast*/
+deploy_45dim_rl_gym/bpu_quantization/calibration_data_*_robotlab_fast*_metadata.json
+deploy_45dim_rl_gym/bpu_quantization/mapper_output/
+deploy_45dim_rl_gym/bpu_quantization/mapper_output_gemm/
+deploy_45dim_rl_gym/bpu_quantization/mapper_output_*_gemm/
+deploy_45dim_rl_gym/bpu_quantization/mapper_output_6500_gemm/
+deploy_45dim_rl_gym/bpu_quantization/hb_mapper_*.log*
+deploy_45dim_rl_gym/bpu_quantization/policy_[0-9]*_actions.onnx
+deploy_45dim_rl_gym/bpu_quantization/policy_[0-9]*_bpu4d.onnx
+deploy_45dim_rl_gym/bpu_quantization/policy_[0-9]*_bpu4d_gemm.onnx
+deploy_45dim_rl_gym/bpu_quantization/policy_[0-9]*_int16_gemm.yaml
+deploy_45dim_rl_gym/bpu_quantization/policy_[0-9]*_int8_gemm.yaml
+deploy_45dim_rl_gym/bpu_quantization/policy_[0-9]*_opset11.onnx
+deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_bpu4d.onnx
+deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_bpu4d_gemm.onnx
+deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_opset11.onnx
+deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_6500_bpu4d.onnx
+deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_6500_bpu4d_gemm.onnx
+deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_6500_opset11.onnx
+deploy_45dim_rl_gym/bpu_deploy_x5/cpp/*.so
+deploy_45dim_rl_gym/bpu_deploy_x5/cpp/bpu_dnn_bench

deploy_45dim_rl_gym/README.md

@@ -5,6 +5,10 @@
 - `policy_15k.onnx`：RoboGauge Go1 RL-Gym 的 ONNX 策略
 - `deploy_go1_onnx_mujoco.py`：MuJoCo 调试脚本
 - `deploy_go1_rlgym_pro_sdk.py`：真机 Go1 PRO 低层部署脚本
+- `deploy_go1_rlgym_pro_sdk_fastcpp.py`：真机 Go1 PRO 低层部署脚本，独立使用 C++ LowCmd 构包/加密后端
+- `deploy_go1_rlgym_pro_sdk_lab.py`：RobotLab 策略真机部署脚本
+- `deploy_go1_rlgym_pro_sdk_lab_fastcpp.py`：RobotLab 策略真机部署脚本，独立使用 C++ LowCmd 构包/加密后端
+- `deploy_go1_rlgym_bpu_x5_fastcpp.py`：Gym 5 帧 BPU 真机部署脚本，使用 C++ LowCmd/LowState 热路径
 
 该 ONNX 策略使用 45 维单帧观测，以及 5 帧、共 225 维的历史输入，按观测项分组堆叠：
 
@@ -14,6 +18,49 @@
 
 关节顺序为 `FR, FL, RR, RL`，与 `go1_pro_sdk` 的电机顺序一致。
 
+## Python / C++ 构包版本隔离
+
+默认真机入口仍然走 Python SDK 构包和 Blowfish：
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py ...
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab.py ...
+```
+
+C++ 低层构包测试使用独立入口，不会改动默认 Python 版本：
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_fastcpp.py ...
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab_fastcpp.py ...
+```
+
+C++ 入口启动时会打印：
+
+```text
+[INFO] LowCmd backend: cpp_fast_lowcmd
+```
+
+日志 `metadata.json` 里也会写入：
+
+```json
+"lowcmd_backend": "cpp_fast_lowcmd"
+```
+
+如果 C++ 扩展没有编译，fastcpp 入口会在连接机器人前直接报错，不会回退到 Python 构包。板端编译命令：
+
+```bash
+cd /root/go1_pro_sdk/fast_lowcmd_cpp
+PYTHONPATH=/root/go1_pro_sdk python3 setup.py build_ext --inplace
+```
+
+Gym 5 帧 BPU 部署入口默认走 `policy_35k` 对应的 `mapper_output_35k_gemm/policy_35k_int16_gemm.bin`，
+也可以显式切换：
+
+```bash
+PYTHONPATH=/root/go1_pro_deploy python3 deploy_45dim_rl_gym/deploy_go1_rlgym_bpu_x5_fastcpp.py --bpu-round 35k
+PYTHONPATH=/root/go1_pro_deploy python3 deploy_45dim_rl_gym/deploy_go1_rlgym_bpu_x5_fastcpp.py --bpu-round 30k
+```
+
 ## 安全说明
 
 直接低层控电机是危险操作。

deploy_45dim_rl_gym/bpu_deploy_s100/README.md

@@ -0,0 +1,239 @@
+# S100 BPU 部署测试
+
+这个目录是 S100 平台的隔离部署路径，不覆盖现有 X5 BPU 脚本。
+
+当前默认模型：
+
+```text
+deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/policy_robotlab_26000_s100_int16_gemm.hbm
+```
+
+S100 量化参数：
+
+- 原始模型：`deploy_45dim_rl_gym/policy_robotlab_26000.onnx`
+- 历史长度：RobotLab 10 帧
+- 输入：`obs_4d [1, 1, 1, 450]`，float32 featuremap
+- 输出：`actions [1, 12, 1, 1]`
+- `march`：`nash-e`
+- Docker 镜像：`registry.d-robotics.cc/deliver/ai_toolchain_ubuntu_22_s100_s600_cpu:v3.7.0`
+
+## 本机量化
+
+在 Mac 的仓库根目录执行：
+
+```bash
+cd /Users/chenyouyuan/cyy_ws/deploy_go1_pro
+bash deploy_45dim_rl_gym/bpu_quantization/quantize_policy_s100.sh
+```
+
+等价显式命令：
+
+```bash
+cd /Users/chenyouyuan/cyy_ws/deploy_go1_pro
+bash deploy_45dim_rl_gym/bpu_quantization/quantize_policy_s100.sh \
+  --policy ../policy_robotlab_26000.onnx \
+  --round 26000 \
+  --name policy_robotlab_26000 \
+  --history-len 10 \
+  --samples 64 \
+  --min-samples 32 \
+  --log-prefix robotlab_go1_deploy \
+  --cal-tag robotlab \
+  --march nash-e
+```
+
+输出文件：
+
+```text
+deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/policy_robotlab_26000_s100_int16_gemm.hbm
+```
+
+## 同步到 S100
+
+S100 板端地址：
+
+```text
+root@192.168.11.144
+```
+
+如果仓库已经通过 git 同步，直接在板端拉取即可。如果只同步产物，可以从 Mac 执行；Docker 只在 Mac 上用于量化，S100 板端不运行 Docker：
+
+```bash
+scp \
+  deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/policy_robotlab_26000_s100_int16_gemm.hbm \
+  root@192.168.11.144:/root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/
+```
+
+同时确保校准输入存在，离线测速会用到：
+
+```bash
+scp \
+  deploy_45dim_rl_gym/bpu_quantization/calibration_data_26000_robotlab_fast64/00000.bin \
+  root@192.168.11.144:/root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/calibration_data_26000_robotlab_fast64/
+```
+
+## 板端安装 hbm_runtime
+
+```bash
+ssh root@192.168.11.144
+cd /usr/hobot/lib/hbm_runtime
+./build.sh install
+```
+
+S100 使用官方 `hbm_runtime` Python 绑定加载 `.hbm`，不复用 X5 的
+`/usr/include/dnn/hb_dnn.h` C++ wrapper。
+
+## 离线推理测速
+
+先用官方 `hrt_model_exec` 看模型信息：
+
+```bash
+cd /root/go1_pro_deploy
+/usr/hobot/bin/hrt_model_exec model_info \
+  --model_file deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/policy_robotlab_26000_s100_int16_gemm.hbm
+```
+
+官方 `hrt_model_exec` 稳态测速：
+
+```bash
+cd /root/go1_pro_deploy
+/usr/hobot/bin/hrt_model_exec perf \
+  --model_file deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/policy_robotlab_26000_s100_int16_gemm.hbm \
+  --model_name policy_robotlab_26000_s100_int16_gemm \
+  --input_file deploy_45dim_rl_gym/bpu_quantization/calibration_data_26000_robotlab_fast64/00000.bin \
+  --frame_count 1000 \
+  --thread_num 1
+```
+
+当前板端 `root@192.168.11.144` 已验证：
+
+```text
+Average latency: 0.394 ms
+FPS: 2442.456
+```
+
+部署脚本使用的 Python `hbm_runtime` wrapper：
+
+```bash
+cd /root/go1_pro_deploy
+PYTHONPATH=/root/go1_pro_sdk:/root/go1_pro_deploy \
+python3 deploy_45dim_rl_gym/bpu_deploy_s100/test_bpu_policy.py \
+  --repeat 1000
+```
+
+当前板端结果：
+
+```text
+Backend: hbm_runtime_s100
+Input: obs_4d (1, 1, 1, 450)
+Output: actions (1, 12)
+repeat=1000 avg_ms=0.733020
+```
+
+纯 C++ BPU wrapper/bench，不经过 Python 推理路径：
+
+```bash
+cd /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_deploy_s100/cpp
+bash build_board.sh
+
+./s100_bpu_bench \
+  /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/policy_robotlab_26000_s100_int16_gemm.hbm \
+  /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/calibration_data_26000_robotlab_fast64/00000.bin \
+  1000 \
+  -1
+```
+
+参数含义：
+
+- 第 1 个参数：S100 `.hbm` 模型。
+- 第 2 个参数：float32 输入样本，当前 RobotLab 10 帧模型应为 450 个 float。
+- 第 3 个参数：重复推理次数。
+- 第 4 个参数：BPU core，`-1` 表示自动选择，`0..3` 表示固定单核。
+
+这个 C++ wrapper 只做离线推理：模型加载和 tensor 内存分配只初始化一次，循环里只做输入拷贝、cache flush、`hbDNNInferV2`、`hbUCPSubmitTask`、等待和输出拷贝。它不会连接机器人，也不会发送电机指令。
+
+当前板端纯 C++ 结果：
+
+```text
+backend=cpp_dnn_api_s100
+input_floats=450 output_floats=12 bpu_core=0
+action [0.544585 -1.108296 0.916061 -0.837005 0.074006 -0.434765 -0.858212 -2.766010 1.127831 0.422954 1.424179 -0.016353]
+action_max_abs 2.766010
+repeat=5000 cpp_avg_ms=0.426015
+```
+
+## 离线推理检查
+
+这一步会连接 MCU 读取状态，但不会发送电机指令：
+
+```bash
+cd /root/go1_pro_deploy
+PYTHONPATH=/root/go1_pro_sdk:/root/go1_pro_deploy \
+python3 deploy_45dim_rl_gym/bpu_deploy_s100/deploy_go1_robotlab_bpu_s100_fastcpp.py \
+  --infer-check \
+  --log-dir logs \
+  --print-every 50 \
+  --max-steps 500
+```
+
+## 悬空状态机测试
+
+先不要加 `--enable-rl`，确认 R2 只能推进到 `INFER_TEST`：
+
+```bash
+cd /root/go1_pro_deploy
+PYTHONPATH=/root/go1_pro_sdk:/root/go1_pro_deploy \
+python3 deploy_45dim_rl_gym/bpu_deploy_s100/deploy_go1_robotlab_bpu_s100_fastcpp.py \
+  --kill-sport \
+  --log-dir logs \
+  --kp 28 --kd 0.7 \
+  --kp-cal 20 --kd-cal 1.0 \
+  --power-factor 7 \
+  --position-protect-limit 0.0 \
+  --action-clip 5.0 \
+  --action-trip-limit 8.0 \
+  --action-hard-trip-limit 16.0 \
+  --max-target-step 0.025 \
+  --max-roll-deg 35 \
+  --max-pitch-deg 35 \
+  --swap-vy-yaw \
+  --rc-vx-scale 0.3 \
+  --rc-vy-scale 0.3 \
+  --rc-wz-scale 0.6 \
+  --log-timing
+```
+
+## 实际 RL 启动
+
+只有悬空测试正常后，再启用 RL：
+
+```bash
+cd /root/go1_pro_deploy
+PYTHONPATH=/root/go1_pro_sdk:/root/go1_pro_deploy \
+python3 deploy_45dim_rl_gym/bpu_deploy_s100/deploy_go1_robotlab_bpu_s100_fastcpp.py \
+  --kill-sport \
+  --enable-rl \
+  --log-dir logs \
+  --kp 28 --kd 0.7 \
+  --kp-cal 20 --kd-cal 1.0 \
+  --power-factor 7 \
+  --position-protect-limit 0.0 \
+  --action-clip 5.0 \
+  --action-trip-limit 8.0 \
+  --action-hard-trip-limit 16.0 \
+  --max-target-step 0.025 \
+  --max-roll-deg 35 \
+  --max-pitch-deg 35 \
+  --swap-vy-yaw \
+  --rc-vx-scale 0.3 \
+  --rc-vy-scale 0.3 \
+  --rc-wz-scale 0.6 \
+  --log-timing
+```
+
+如果要临时指定其它 S100 `.hbm`：
+
+```bash
+python3 deploy_45dim_rl_gym/bpu_deploy_s100/deploy_go1_robotlab_bpu_s100_fastcpp.py \
+  --bpu-model /absolute/path/to/model.hbm
+```

deploy_45dim_rl_gym/bpu_deploy_s100/bpu_policy.py

@@ -0,0 +1,77 @@
+#!/usr/bin/env python3
+"""S100 HBM policy runtime wrapper."""
+
+from pathlib import Path
+
+import numpy as np
+
+
+class BpuInferLibPolicy:
+    """S100 backend: official hbm_runtime Python binding."""
+
+    backend_name = "hbm_runtime_s100"
+
+    def __init__(self, model_path, priority=0, bpu_cores=(0,), cpp_lib=None):
+        del cpp_lib
+        self.model_path = Path(model_path).expanduser().resolve()
+        if not self.model_path.exists():
+            raise FileNotFoundError(f"S100 HBM model not found: {self.model_path}")
+
+        try:
+            from hbm_runtime import HB_HBMRuntime
+        except ImportError as exc:
+            raise RuntimeError(
+                "hbm_runtime is required on S100. Install it on the board with: "
+                "cd /usr/hobot/lib/hbm_runtime && ./build.sh install"
+            ) from exc
+
+        self.priority = int(priority)
+        self.bpu_cores = tuple(int(core) for core in bpu_cores)
+        self.runtime = HB_HBMRuntime(str(self.model_path))
+        self.version = getattr(self.runtime, "version", "")
+
+        model_names = list(self.runtime.model_names)
+        if len(model_names) != 1:
+            raise RuntimeError(f"expected one model in {self.model_path}, got {model_names}")
+        self.model_name = model_names[0]
+
+        input_names = list(self.runtime.input_names[self.model_name])
+        output_names = list(self.runtime.output_names[self.model_name])
+        if len(input_names) != 1 or len(output_names) != 1:
+            raise RuntimeError(
+                f"expected 1 input and 1 output, got {input_names} / {output_names}"
+            )
+        self.input_name = input_names[0]
+        self.output_name = output_names[0]
+
+        self.input_shape = tuple(int(x) for x in self.runtime.input_shapes[self.model_name][self.input_name])
+        self.output_shape = tuple(int(x) for x in self.runtime.output_shapes[self.model_name][self.output_name])
+        self.input_size = int(np.prod(self.input_shape))
+        self.output_size = int(np.prod(self.output_shape))
+
+        print(f"[INFO] BPU model: {self.model_path}")
+        print(f"[INFO]   Backend: {self.backend_name}")
+        print(f"[INFO]   Runtime: {self.version}")
+        print(f"[INFO]   Input  : {self.input_name} {self.input_shape}")
+        print(f"[INFO]   Output : {self.output_name} {self.output_shape}")
+
+    def close(self):
+        self.runtime = None
+
+    def __call__(self, flat_input):
+        arr = np.asarray(flat_input, dtype=np.float32)
+        if arr.size != self.input_size:
+            raise ValueError(f"S100 policy input has {arr.size} values, expected {self.input_size}")
+        input_tensor = np.ascontiguousarray(arr.reshape(self.input_shape), dtype=np.float32)
+        outputs = self.runtime.run(input_tensor)
+        action = np.asarray(outputs[self.model_name][self.output_name], dtype=np.float32).reshape(-1)
+        if action.size != self.output_size:
+            raise RuntimeError(
+                f"S100 policy output has {action.size} values, expected {self.output_size}"
+            )
+        if not np.all(np.isfinite(action)):
+            raise RuntimeError(f"S100 policy output is not finite: {action}")
+        return action.copy()
+
+
+BpuInferLibPythonPolicy = BpuInferLibPolicy

deploy_45dim_rl_gym/bpu_deploy_s100/cpp/build_board.sh

@@ -0,0 +1,33 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+cd "$(dirname "$0")"
+
+CXX="${CXX:-g++}"
+CXXFLAGS=(
+  -O3
+  -DNDEBUG
+  -std=c++17
+  -Wall
+  -Wextra
+  -fPIC
+  -I/usr/include
+)
+LDFLAGS=(
+  -L/usr/hobot/lib
+  -ldnn
+  -lhbucp
+  -Wl,-rpath,/usr/hobot/lib
+)
+
+"${CXX}" "${CXXFLAGS[@]}" -shared s100_bpu_policy.cpp \
+  "${LDFLAGS[@]}" \
+  -o libs100_bpu_policy.so
+
+"${CXX}" "${CXXFLAGS[@]}" s100_bpu_bench.cpp \
+  -L. -ls100_bpu_policy -Wl,-rpath,'$ORIGIN' \
+  "${LDFLAGS[@]}" \
+  -o s100_bpu_bench
+
+echo "[OK] built $(pwd)/libs100_bpu_policy.so"
+echo "[OK] built $(pwd)/s100_bpu_bench"

deploy_45dim_rl_gym/bpu_deploy_s100/cpp/s100_bpu_bench.cpp

@@ -0,0 +1,129 @@
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstdlib>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <string>
+#include <vector>
+
+extern "C" {
+void *rlgym_s100_bpu_create(const char *model_path, int bpu_core, int priority,
+                            char *err, int err_len);
+int rlgym_s100_bpu_infer(void *handle, const float *input, float *output, char *err,
+                         int err_len);
+int rlgym_s100_bpu_input_floats(void *handle);
+int rlgym_s100_bpu_output_floats(void *handle);
+void rlgym_s100_bpu_destroy(void *handle);
+const char *rlgym_s100_bpu_version();
+}
+
+namespace {
+
+bool read_f32_file(const std::string &path, std::vector<float> *data) {
+  std::ifstream ifs(path, std::ios::binary);
+  if (!ifs) {
+    return false;
+  }
+  ifs.seekg(0, std::ios::end);
+  const auto size = ifs.tellg();
+  ifs.seekg(0, std::ios::beg);
+  if (size <= 0 || size % static_cast<std::streamoff>(sizeof(float)) != 0) {
+    return false;
+  }
+  data->resize(static_cast<size_t>(size) / sizeof(float));
+  ifs.read(reinterpret_cast<char *>(data->data()), size);
+  return ifs.good();
+}
+
+void print_usage(const char *argv0) {
+  std::cerr << "Usage: " << argv0 << " [model.hbm] [input.bin] [repeat] [bpu_core]\n"
+            << "  bpu_core: -1 means any core; 0..3 pins one BPU core\n";
+}
+
+}  // namespace
+
+int main(int argc, char **argv) {
+  const char *model =
+      "/root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/"
+      "mapper_output_26000_s100_gemm/policy_robotlab_26000_s100_int16_gemm.hbm";
+  const char *input =
+      "/root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/"
+      "calibration_data_26000_robotlab_fast64/00000.bin";
+  int repeat = 1000;
+  int bpu_core = -1;
+  if (argc > 1) model = argv[1];
+  if (argc > 2) input = argv[2];
+  if (argc > 3) repeat = std::atoi(argv[3]);
+  if (argc > 4) bpu_core = std::atoi(argv[4]);
+  if (argc > 5 || repeat <= 0) {
+    print_usage(argv[0]);
+    return 2;
+  }
+
+  char err[2048] = {};
+  void *handle = rlgym_s100_bpu_create(model, bpu_core, 0, err, sizeof(err));
+  if (handle == nullptr) {
+    std::cerr << err << "\n";
+    return 3;
+  }
+
+  const int input_floats = rlgym_s100_bpu_input_floats(handle);
+  const int output_floats = rlgym_s100_bpu_output_floats(handle);
+  if (input_floats <= 0 || output_floats <= 0) {
+    std::cerr << "invalid tensor sizes from S100 BPU runtime\n";
+    rlgym_s100_bpu_destroy(handle);
+    return 2;
+  }
+
+  std::vector<float> obs;
+  if (!read_f32_file(input, &obs) || static_cast<int>(obs.size()) != input_floats) {
+    std::cerr << "failed to read " << input_floats << " float32 input: " << input
+              << "\n";
+    rlgym_s100_bpu_destroy(handle);
+    return 2;
+  }
+
+  std::vector<float> out(static_cast<size_t>(output_floats), 0.0f);
+  if (rlgym_s100_bpu_infer(handle, obs.data(), out.data(), err, sizeof(err)) != 0) {
+    std::cerr << err << "\n";
+    rlgym_s100_bpu_destroy(handle);
+    return 4;
+  }
+
+  const auto t0 = std::chrono::steady_clock::now();
+  for (int i = 0; i < repeat; ++i) {
+    if (rlgym_s100_bpu_infer(handle, obs.data(), out.data(), err, sizeof(err)) != 0) {
+      std::cerr << err << "\n";
+      rlgym_s100_bpu_destroy(handle);
+      return 5;
+    }
+  }
+  const auto t1 = std::chrono::steady_clock::now();
+  const double elapsed_ms =
+      std::chrono::duration<double, std::milli>(t1 - t0).count();
+
+  const auto max_it = std::max_element(out.begin(), out.end(), [](float a, float b) {
+    return std::fabs(a) < std::fabs(b);
+  });
+
+  std::cout << "backend=" << rlgym_s100_bpu_version() << "\n";
+  std::cout << "input_floats=" << input_floats << " output_floats=" << output_floats
+            << " bpu_core=" << bpu_core << "\n";
+  std::cout << std::fixed << std::setprecision(6);
+  std::cout << "action [";
+  for (int i = 0; i < output_floats; ++i) {
+    if (i != 0) {
+      std::cout << ' ';
+    }
+    std::cout << out[static_cast<size_t>(i)];
+  }
+  std::cout << "]\n";
+  std::cout << "action_max_abs " << (max_it == out.end() ? 0.0f : std::fabs(*max_it))
+            << "\n";
+  std::cout << "repeat=" << repeat << " cpp_avg_ms=" << (elapsed_ms / repeat) << "\n";
+
+  rlgym_s100_bpu_destroy(handle);
+  return 0;
+}

deploy_45dim_rl_gym/bpu_deploy_s100/cpp/s100_bpu_policy.cpp

@@ -0,0 +1,349 @@
+#include <algorithm>
+#include <cstdint>
+#include <cstdio>
+#include <cstring>
+#include <exception>
+#include <limits>
+#include <memory>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <hobot/dnn/hb_dnn.h>
+#include <hobot/dnn/hb_dnn_status.h>
+#include <hobot/hb_ucp.h>
+#include <hobot/hb_ucp_status.h>
+
+namespace {
+
+void set_error(char *err, int err_len, const std::string &msg) {
+  if (err == nullptr || err_len <= 0) {
+    return;
+  }
+  std::snprintf(err, static_cast<size_t>(err_len), "%s", msg.c_str());
+}
+
+std::string api_error(const std::string &where, int32_t code) {
+  std::ostringstream oss;
+  oss << where << " failed: " << code;
+  const char *desc = hbDNNGetErrorDesc(code);
+  if (desc == nullptr) {
+    desc = hbUCPGetErrorDesc(code);
+  }
+  if (desc != nullptr) {
+    oss << " (" << desc << ")";
+  }
+  return oss.str();
+}
+
+void check_api(int32_t code, const std::string &where) {
+  if (code != 0) {
+    throw std::runtime_error(api_error(where, code));
+  }
+}
+
+int64_t element_count(const hbDNNTensorShape &shape) {
+  int64_t count = 1;
+  for (int i = 0; i < shape.numDimensions; ++i) {
+    if (shape.dimensionSize[i] <= 0) {
+      throw std::runtime_error("dynamic or invalid tensor shape is not supported");
+    }
+    count *= shape.dimensionSize[i];
+  }
+  return count;
+}
+
+int element_size(int tensor_type) {
+  switch (tensor_type) {
+    case HB_DNN_TENSOR_TYPE_BOOL8:
+    case HB_DNN_TENSOR_TYPE_S8:
+    case HB_DNN_TENSOR_TYPE_U8:
+      return 1;
+    case HB_DNN_TENSOR_TYPE_F16:
+    case HB_DNN_TENSOR_TYPE_S16:
+    case HB_DNN_TENSOR_TYPE_U16:
+      return 2;
+    case HB_DNN_TENSOR_TYPE_F32:
+    case HB_DNN_TENSOR_TYPE_S32:
+    case HB_DNN_TENSOR_TYPE_U32:
+      return 4;
+    case HB_DNN_TENSOR_TYPE_F64:
+    case HB_DNN_TENSOR_TYPE_S64:
+    case HB_DNN_TENSOR_TYPE_U64:
+      return 8;
+    default:
+      throw std::runtime_error("unsupported tensor type");
+  }
+}
+
+int64_t compact_tail_bytes(const int32_t *dims, int dim_count, int elem_bytes) {
+  int64_t bytes = elem_bytes;
+  for (int i = 0; i < dim_count; ++i) {
+    bytes *= dims[i];
+  }
+  return bytes;
+}
+
+void copy_compact_to_strided(char *dst, const char *src, const hbDNNTensorProperties &props,
+                             int dim, int elem_bytes) {
+  const auto &shape = props.validShape;
+  if (dim + 1 == shape.numDimensions) {
+    std::memcpy(dst, src, static_cast<size_t>(shape.dimensionSize[dim] * elem_bytes));
+    return;
+  }
+
+  const int64_t src_step =
+      compact_tail_bytes(shape.dimensionSize + dim + 1, shape.numDimensions - dim - 1,
+                         elem_bytes);
+  const int64_t dst_step = props.stride[dim];
+  for (int i = 0; i < shape.dimensionSize[dim]; ++i) {
+    copy_compact_to_strided(dst + dst_step * i, src + src_step * i, props, dim + 1,
+                            elem_bytes);
+  }
+}
+
+void copy_strided_to_compact(char *dst, const char *src, const hbDNNTensorProperties &props,
+                             int dim, int elem_bytes) {
+  const auto &shape = props.validShape;
+  if (dim + 1 == shape.numDimensions) {
+    std::memcpy(dst, src, static_cast<size_t>(shape.dimensionSize[dim] * elem_bytes));
+    return;
+  }
+
+  const int64_t dst_step =
+      compact_tail_bytes(shape.dimensionSize + dim + 1, shape.numDimensions - dim - 1,
+                         elem_bytes);
+  const int64_t src_step = props.stride[dim];
+  for (int i = 0; i < shape.dimensionSize[dim]; ++i) {
+    copy_strided_to_compact(dst + dst_step * i, src + src_step * i, props, dim + 1,
+                            elem_bytes);
+  }
+}
+
+uint64_t core_mask_from_arg(int bpu_core) {
+  if (bpu_core < 0) {
+    return HB_UCP_BPU_CORE_ANY;
+  }
+  if (bpu_core > 3) {
+    throw std::runtime_error("bpu_core must be -1 or 0..3");
+  }
+  return 1ULL << static_cast<uint64_t>(bpu_core);
+}
+
+class S100BpuPolicy {
+ public:
+  S100BpuPolicy(const char *model_path, int bpu_core, int priority)
+      : bpu_core_mask_(core_mask_from_arg(bpu_core)), priority_(priority) {
+    if (model_path == nullptr || model_path[0] == '\0') {
+      throw std::runtime_error("empty model path");
+    }
+
+    const char *model_files[] = {model_path};
+    check_api(hbDNNInitializeFromFiles(&packed_handle_, model_files, 1),
+              "hbDNNInitializeFromFiles");
+
+    const char **model_names = nullptr;
+    int32_t model_count = 0;
+    check_api(hbDNNGetModelNameList(&model_names, &model_count, packed_handle_),
+              "hbDNNGetModelNameList");
+    if (model_count <= 0 || model_names == nullptr || model_names[0] == nullptr) {
+      throw std::runtime_error("model has no names");
+    }
+    model_name_ = model_names[0];
+    check_api(hbDNNGetModelHandle(&dnn_handle_, packed_handle_, model_names[0]),
+              "hbDNNGetModelHandle");
+
+    int32_t input_count = 0;
+    int32_t output_count = 0;
+    check_api(hbDNNGetInputCount(&input_count, dnn_handle_), "hbDNNGetInputCount");
+    check_api(hbDNNGetOutputCount(&output_count, dnn_handle_), "hbDNNGetOutputCount");
+    if (input_count != 1 || output_count != 1) {
+      std::ostringstream oss;
+      oss << "expected 1 input and 1 output, got " << input_count << " inputs and "
+          << output_count << " outputs";
+      throw std::runtime_error(oss.str());
+    }
+
+    input_tensors_.resize(1);
+    output_tensors_.resize(1);
+    check_api(hbDNNGetInputTensorProperties(&input_tensors_[0].properties, dnn_handle_, 0),
+              "hbDNNGetInputTensorProperties");
+    check_api(hbDNNGetOutputTensorProperties(&output_tensors_[0].properties, dnn_handle_, 0),
+              "hbDNNGetOutputTensorProperties");
+
+    validate_float_tensor(input_tensors_[0].properties, "input");
+    validate_float_tensor(output_tensors_[0].properties, "output");
+    input_floats_ = checked_float_count(input_tensors_[0].properties, "input");
+    output_floats_ = checked_float_count(output_tensors_[0].properties, "output");
+
+    alloc_tensor_mem(input_tensors_[0]);
+    alloc_tensor_mem(output_tensors_[0]);
+  }
+
+  ~S100BpuPolicy() {
+    release_tensor_mem(input_tensors_);
+    release_tensor_mem(output_tensors_);
+    if (packed_handle_ != nullptr) {
+      hbDNNRelease(packed_handle_);
+      packed_handle_ = nullptr;
+    }
+  }
+
+  void infer(const float *input, float *output) {
+    if (input == nullptr || output == nullptr) {
+      throw std::runtime_error("null input/output pointer");
+    }
+
+    auto &input_tensor = input_tensors_[0];
+    const auto &input_props = input_tensor.properties;
+    std::memset(input_tensor.sysMem.virAddr, 0,
+                static_cast<size_t>(input_props.alignedByteSize));
+    copy_compact_to_strided(reinterpret_cast<char *>(input_tensor.sysMem.virAddr),
+                            reinterpret_cast<const char *>(input), input_props, 0,
+                            sizeof(float));
+    check_api(hbUCPMemFlush(&input_tensor.sysMem, HB_SYS_MEM_CACHE_CLEAN),
+              "hbUCPMemFlush(input)");
+
+    hbUCPTaskHandle_t task_handle = nullptr;
+    check_api(hbDNNInferV2(&task_handle, output_tensors_.data(), input_tensors_.data(),
+                           dnn_handle_),
+              "hbDNNInferV2");
+
+    hbUCPSchedParam sched_param{};
+    HB_UCP_INITIALIZE_SCHED_PARAM(&sched_param);
+    sched_param.priority = priority_;
+    sched_param.backend = bpu_core_mask_;
+    try {
+      check_api(hbUCPSubmitTask(task_handle, &sched_param), "hbUCPSubmitTask");
+      check_api(hbUCPWaitTaskDone(task_handle, 0), "hbUCPWaitTaskDone");
+
+      auto &output_tensor = output_tensors_[0];
+      check_api(hbUCPMemFlush(&output_tensor.sysMem, HB_SYS_MEM_CACHE_INVALIDATE),
+                "hbUCPMemFlush(output)");
+      copy_strided_to_compact(reinterpret_cast<char *>(output),
+                              reinterpret_cast<const char *>(output_tensor.sysMem.virAddr),
+                              output_tensor.properties, 0, sizeof(float));
+    } catch (...) {
+      hbUCPReleaseTask(task_handle);
+      throw;
+    }
+
+    check_api(hbUCPReleaseTask(task_handle), "hbUCPReleaseTask");
+  }
+
+  int input_floats() const { return input_floats_; }
+  int output_floats() const { return output_floats_; }
+  const std::string &model_name() const { return model_name_; }
+
+ private:
+  static void validate_float_tensor(const hbDNNTensorProperties &props, const char *name) {
+    if (props.tensorType != HB_DNN_TENSOR_TYPE_F32) {
+      std::ostringstream oss;
+      oss << name << " tensor type " << props.tensorType << " != "
+          << HB_DNN_TENSOR_TYPE_F32;
+      throw std::runtime_error(oss.str());
+    }
+    if (props.alignedByteSize <= 0) {
+      throw std::runtime_error(std::string(name) + " alignedByteSize <= 0");
+    }
+    if (props.validShape.numDimensions <= 0) {
+      throw std::runtime_error(std::string(name) + " has invalid dimensions");
+    }
+  }
+
+  static int checked_float_count(const hbDNNTensorProperties &props, const char *name) {
+    const int64_t floats = element_count(props.validShape);
+    const int64_t bytes = floats * element_size(props.tensorType);
+    if (bytes > props.alignedByteSize) {
+      std::ostringstream oss;
+      oss << name << " compact bytes " << bytes << " > alignedByteSize "
+          << props.alignedByteSize;
+      throw std::runtime_error(oss.str());
+    }
+    if (floats > static_cast<int64_t>(std::numeric_limits<int>::max())) {
+      throw std::runtime_error(std::string(name) + " tensor is too large");
+    }
+    return static_cast<int>(floats);
+  }
+
+  static void alloc_tensor_mem(hbDNNTensor &tensor) {
+    std::memset(&tensor.sysMem, 0, sizeof(tensor.sysMem));
+    check_api(hbUCPMallocCached(&tensor.sysMem,
+                                static_cast<uint64_t>(tensor.properties.alignedByteSize), 0),
+              "hbUCPMallocCached");
+  }
+
+  static void release_tensor_mem(std::vector<hbDNNTensor> &tensors) {
+    for (auto &tensor : tensors) {
+      if (tensor.sysMem.virAddr != nullptr) {
+        hbUCPFree(&tensor.sysMem);
+        std::memset(&tensor.sysMem, 0, sizeof(tensor.sysMem));
+      }
+    }
+  }
+
+  hbDNNPackedHandle_t packed_handle_{nullptr};
+  hbDNNHandle_t dnn_handle_{nullptr};
+  std::string model_name_;
+  std::vector<hbDNNTensor> input_tensors_;
+  std::vector<hbDNNTensor> output_tensors_;
+  int input_floats_{0};
+  int output_floats_{0};
+  uint64_t bpu_core_mask_{HB_UCP_BPU_CORE_ANY};
+  int priority_{HB_UCP_PRIORITY_LOWEST};
+};
+
+}  // namespace
+
+extern "C" {
+
+void *rlgym_s100_bpu_create(const char *model_path, int bpu_core, int priority,
+                            char *err, int err_len) {
+  try {
+    set_error(err, err_len, "");
+    return new S100BpuPolicy(model_path, bpu_core, priority);
+  } catch (const std::exception &e) {
+    set_error(err, err_len, e.what());
+    return nullptr;
+  }
+}
+
+int rlgym_s100_bpu_infer(void *handle, const float *input, float *output, char *err,
+                         int err_len) {
+  try {
+    set_error(err, err_len, "");
+    if (handle == nullptr) {
+      throw std::runtime_error("null policy handle");
+    }
+    static_cast<S100BpuPolicy *>(handle)->infer(input, output);
+    return 0;
+  } catch (const std::exception &e) {
+    set_error(err, err_len, e.what());
+    return -1;
+  }
+}
+
+int rlgym_s100_bpu_input_floats(void *handle) {
+  if (handle == nullptr) {
+    return 0;
+  }
+  return static_cast<S100BpuPolicy *>(handle)->input_floats();
+}
+
+int rlgym_s100_bpu_output_floats(void *handle) {
+  if (handle == nullptr) {
+    return 0;
+  }
+  return static_cast<S100BpuPolicy *>(handle)->output_floats();
+}
+
+void rlgym_s100_bpu_destroy(void *handle) {
+  delete static_cast<S100BpuPolicy *>(handle);
+}
+
+const char *rlgym_s100_bpu_version() {
+  return "cpp_dnn_api_s100";
+}
+
+}

deploy_45dim_rl_gym/bpu_deploy_s100/test_bpu_policy.py

@@ -0,0 +1,55 @@
+#!/usr/bin/env python3
+"""Offline BPU policy smoke test for S100.
+
+This does not connect to the robot. It loads a S100 BPU .hbm and one raw
+float32 input file, then runs the hbm_runtime backend repeatedly.
+"""
+
+import argparse
+import time
+from pathlib import Path
+
+import numpy as np
+
+from bpu_policy import BpuInferLibPolicy
+
+
+HERE = Path(__file__).parent.resolve()
+DEFAULT_MODEL = (
+    HERE.parent / "bpu_quantization" / "mapper_output_26000_s100_gemm" /
+    "policy_robotlab_26000_s100_int16_gemm.hbm"
+)
+DEFAULT_INPUT = (
+    HERE.parent / "bpu_quantization" /
+    "calibration_data_26000_robotlab_fast64" / "00000.bin"
+)
+
+def main():
+    parser = argparse.ArgumentParser(description="Offline BPU policy smoke test")
+    parser.add_argument("--bpu-model", default=str(DEFAULT_MODEL))
+    parser.add_argument("--input-bin", default=str(DEFAULT_INPUT))
+    parser.add_argument("--repeat", type=int, default=1000)
+    args = parser.parse_args()
+
+    input_path = Path(args.input_bin).expanduser().resolve()
+    data = np.fromfile(input_path, dtype=np.float32)
+    policy = BpuInferLibPolicy(args.bpu_model)
+    if data.size != policy.input_size:
+        raise ValueError(
+            f"{input_path} has {data.size} float32 values, "
+            f"but model expects {policy.input_size}"
+        )
+    action = policy(data)
+    print("action", np.array2string(action, precision=6))
+    print("action_max_abs", float(np.max(np.abs(action))))
+
+    repeats = max(1, int(args.repeat))
+    t0 = time.perf_counter()
+    for _ in range(repeats):
+        policy(data)
+    elapsed_ms = (time.perf_counter() - t0) * 1000.0
+    print(f"repeat={repeats} avg_ms={elapsed_ms / repeats:.6f}")
+
+
+if __name__ == "__main__":
+    main()

deploy_45dim_rl_gym/bpu_deploy_x5/README.md

@@ -0,0 +1,194 @@
+# RDK X5 BPU 部署测试
+
+这个目录是 RobotLab BPU `.bin` 的隔离部署路径，不覆盖现有 ONNX/fastcpp
+部署脚本。
+
+当前包含：
+
+- `bpu_policy.py`：BPU policy wrapper，默认使用 `cpp_dnn_api_x5` C++ DNN API 后端。
+- `cpp/`：C++ DNN API 推理库源码、板端编译脚本和纯 C++ benchmark。
+- `deploy_go1_robotlab_bpu_x5_fastcpp.py`：基于现有 `lab_fastcpp` 状态机的 BPU 版本，默认 26000。
+
+## 重要限制
+
+不要用 `hobot_dnn.pyeasy_dnn` 直接跑这个模型。这个 BPU 模型的输入是
+`featuremap`，板端实测 `pyeasy_dnn.forward()` 会产生明显错误的 action，
+和 `hrt_model_exec infer` 不一致。
+
+当前默认不再使用 `bpu_infer_lib` 做实机推理，而是用 C++ 直接调用 DNN API。
+部署脚本使用的 C++ 调用顺序是：
+
+```bash
+hbDNNInitializeFromFiles -> hbDNNGetModelHandle -> hbDNNInfer -> hbDNNWaitTaskDone -> hbDNNReleaseTask
+```
+
+这个路径已经用 `00000.bin` 验证，输出和 `hrt_model_exec infer` 对齐。
+
+`bpu_infer_lib.forward(False)` 在当前板端会从 C runtime 向 stdout 每帧打印
+`duplicate model infer is not supported in one batch` / `invalid model`。
+虽然输出和 `hrt_model_exec infer` 一致，但这说明 task 提交方式不干净。旧 Python
+fallback 保留为 `BpuInferLibPythonPolicy`，只用于对照，不作为默认部署路径。
+
+这个判断参考 D-Robotics RDK X5 runtime sample：
+
+- `03_misc/run_resnet50_feature.sh` 是官方 featuremap 输入模型示例，流程是
+  prepare feature tensor -> infer success -> task done。
+- 性能测量示例在循环中每帧执行 `hbDNNInfer` 后，立刻
+  `hbDNNWaitTaskDone(task_handle, 0)`，再 `hbDNNReleaseTask(task_handle)` 并把
+  task handle 置空。
+
+`bpu_infer_lib` 的 Python API 没有直接暴露 `hbDNNReleaseTask`，所以这里改成
+C++ shared library，并由 Python 通过 `ctypes` 调用。
+
+板端编译：
+
+```bash
+cd /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_deploy_x5/cpp
+bash build_board.sh
+```
+
+## 模型路径
+
+默认模型：
+
+```text
+deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_gemm/policy_robotlab_26000_int16_gemm.bin
+```
+
+输入输出：
+
+- 输入：`obs_4d [1, 1, 1, 450]`，float32 featuremap
+- 输出：`actions [1, 12, 1, 1]`，float32，脚本会 flatten 成 12 维 action
+
+## 板端准备
+
+同步仓库和模型后，在板端确认：
+
+```bash
+cd /root/go1_pro_deploy
+ls deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_gemm/policy_robotlab_26000_int16_gemm.bin
+ls deploy_45dim_rl_gym/bpu_quantization/calibration_data/00000.bin
+ls deploy_45dim_rl_gym/bpu_deploy_x5/cpp/libbpu_dnn_policy.so
+```
+
+如果 `mapper_output_26000_gemm/` 没有同步到 git，需要手动把 `.bin` 放到对应路径，
+或用 `--bpu-model` 指定绝对路径。
+
+## 纯离线 BPU 自检
+
+先不连接机器人，确认 C++ BPU 后端输出和 `hrt_model_exec infer` 的参考值一致：
+
+```bash
+cd /root/go1_pro_deploy
+PYTHONPATH=/root/go1_pro_deploy \
+python3 deploy_45dim_rl_gym/bpu_deploy_x5/test_bpu_policy.py \
+  --bpu-model deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_gemm/policy_robotlab_26000_int16_gemm.bin \
+  --input-bin deploy_45dim_rl_gym/bpu_quantization/calibration_data_26000_robotlab_fast64/00000.bin \
+  --repeat 1000
+```
+
+当前板端 `root@192.168.150.167` 已验证：
+
+```text
+repeat=1000 avg_ms=...  # Python ctypes -> C++ DNN API
+```
+
+纯 C++ benchmark：
+
+```bash
+cd /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_deploy_x5/cpp
+./bpu_dnn_bench \
+  /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_gemm/policy_robotlab_26000_int16_gemm.bin \
+  /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/calibration_data_26000_robotlab_fast64/00000.bin \
+  1000
+```
+
+当前板端结果：
+
+```text
+input_floats=450 output_floats=12
+repeat=1000 cpp_avg_ms=...
+```
+
+## 离线推理检查
+
+先不要接电机控制，只运行 BPU inference：
+
+```bash
+cd /root/go1_pro_deploy
+PYTHONPATH=/root/go1_pro_sdk:/root/go1_pro_deploy \
+python3 deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_robotlab_bpu_x5_fastcpp.py \
+  --infer-check \
+  --log-dir logs \
+  --print-every 50 \
+  --max-steps 500
+```
+
+这一步仍会连接 MCU 读取状态，但不会发送电机指令。
+
+如果要切回旧轮次：
+
+```bash
+PYTHONPATH=/root/go1_pro_deploy \
+python3 deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_robotlab_bpu_x5_fastcpp.py \
+  --bpu-round 6500
+
+PYTHONPATH=/root/go1_pro_deploy \
+python3 deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_robotlab_bpu_x5_fastcpp.py \
+  --bpu-round 15000
+```
+
+## 悬空状态机测试
+
+只有 `--infer-check` 日志确认 action 正常后，再悬空测试状态机。先不加
+`--enable-rl`，R2 只能走到 `INFER_TEST`，不会进入 RL 控制：
+
+```bash
+cd /root/go1_pro_deploy
+PYTHONPATH=/root/go1_pro_sdk:/root/go1_pro_deploy \
+python3 deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_robotlab_bpu_x5_fastcpp.py \
+  --kill-sport \
+  --log-dir logs \
+  --kp 28 --kd 0.7 \
+  --kp-cal 20 --kd-cal 1.0 \
+  --power-factor 7 \
+  --position-protect-limit 0.0 \
+  --action-clip 5.0 \
+  --action-trip-limit 8.0 \
+  --action-hard-trip-limit 16.0 \
+  --max-target-step 0.025 \
+  --max-roll-deg 35 \
+  --max-pitch-deg 35 \
+  --swap-vy-yaw \
+  --rc-vx-scale 0.5 \
+  --rc-vy-scale 0.5 \
+  --rc-wz-scale 1.0
+```
+
+确认 `OBS_TEST` 和 `INFER_TEST` 正常后，才允许加 `--enable-rl` 做悬空 RL：
+
+```bash
+cd /root/go1_pro_deploy
+PYTHONPATH=/root/go1_pro_sdk:/root/go1_pro_deploy \
+python3 deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_robotlab_bpu_x5_fastcpp.py \
+  --kill-sport \
+  --enable-rl \
+  --log-dir logs \
+  --kp 28 --kd 0.7 \
+  --kp-cal 20 --kd-cal 1.0 \
+  --power-factor 7 \
+  --position-protect-limit 0.0 \
+  --action-clip 5.0 \
+  --action-trip-limit 8.0 \
+  --action-hard-trip-limit 16.0 \
+  --max-target-step 0.025 \
+  --max-roll-deg 35 \
+  --max-pitch-deg 35 \
+  --swap-vy-yaw \
+  --rc-vx-scale 0.5 \
+  --rc-vy-scale 0.5 \
+  --rc-wz-scale 1.0
+```
+
+地面测试不要直接从 BPU 版本开始；先用同参数 ONNX 版本确认行为，再做 BPU/ONNX
+对照。

deploy_45dim_rl_gym/bpu_deploy_x5/bpu_policy.py

@@ -0,0 +1,218 @@
+#!/usr/bin/env python3
+"""RDK X5 BPU policy runtime wrappers."""
+
+from contextlib import contextmanager
+from pathlib import Path
+import ctypes
+import os
+import sys
+
+import numpy as np
+
+
+HERE = Path(__file__).parent.resolve()
+DEFAULT_CPP_LIB = HERE / "cpp" / "libbpu_dnn_policy.so"
+
+
+def flush_c_stdio():
+    try:
+        ctypes.CDLL(None).fflush(None)
+    except Exception:
+        pass
+
+
+@contextmanager
+def suppress_c_output(enabled=True):
+    if not enabled:
+        yield
+        return
+    sys.stdout.flush()
+    sys.stderr.flush()
+    flush_c_stdio()
+    saved_fds = {fd: os.dup(fd) for fd in (1, 2)}
+    devnull_fd = os.open(os.devnull, os.O_WRONLY)
+    try:
+        os.dup2(devnull_fd, 1)
+        os.dup2(devnull_fd, 2)
+        yield
+    finally:
+        flush_c_stdio()
+        for fd, saved_fd in saved_fds.items():
+            os.dup2(saved_fd, fd)
+            os.close(saved_fd)
+        os.close(devnull_fd)
+
+
+def _first_bpu_core(bpu_cores):
+    cores = tuple(int(core) for core in bpu_cores)
+    if not cores:
+        return 0
+    core = cores[0]
+    if core == 0:
+        return 1
+    if core == 1:
+        return 2
+    return 0
+
+
+class BpuInferLibPolicy:
+    """Default fast backend: direct C++ DNN API via ctypes."""
+
+    backend_name = "cpp_dnn_api_x5"
+
+    def __init__(self, model_path, priority=0, bpu_cores=(0,), cpp_lib=DEFAULT_CPP_LIB):
+        self.model_path = Path(model_path).expanduser().resolve()
+        self.cpp_lib = Path(cpp_lib).expanduser().resolve()
+        if not self.model_path.exists():
+            raise FileNotFoundError(f"BPU model not found: {self.model_path}")
+        if not self.cpp_lib.exists():
+            raise FileNotFoundError(
+                f"C++ BPU runtime library not found: {self.cpp_lib}. "
+                f"Build it on the RDK X5 board with: cd {self.cpp_lib.parent} && bash build_board.sh"
+            )
+
+        self.priority = int(priority)
+        self.bpu_cores = tuple(int(core) for core in bpu_cores)
+        self._lib = ctypes.CDLL(str(self.cpp_lib))
+        self._lib.rlgym_bpu_create.argtypes = [
+            ctypes.c_char_p,
+            ctypes.c_int,
+            ctypes.c_int,
+            ctypes.c_char_p,
+            ctypes.c_int,
+        ]
+        self._lib.rlgym_bpu_create.restype = ctypes.c_void_p
+        self._lib.rlgym_bpu_infer.argtypes = [
+            ctypes.c_void_p,
+            ctypes.POINTER(ctypes.c_float),
+            ctypes.POINTER(ctypes.c_float),
+            ctypes.c_char_p,
+            ctypes.c_int,
+        ]
+        self._lib.rlgym_bpu_infer.restype = ctypes.c_int
+        self._lib.rlgym_bpu_destroy.argtypes = [ctypes.c_void_p]
+        self._lib.rlgym_bpu_destroy.restype = None
+        self._lib.rlgym_bpu_input_floats.argtypes = [ctypes.c_void_p]
+        self._lib.rlgym_bpu_input_floats.restype = ctypes.c_int
+        self._lib.rlgym_bpu_output_floats.argtypes = [ctypes.c_void_p]
+        self._lib.rlgym_bpu_output_floats.restype = ctypes.c_int
+        self._lib.rlgym_bpu_version.argtypes = []
+        self._lib.rlgym_bpu_version.restype = ctypes.c_char_p
+
+        err = ctypes.create_string_buffer(1024)
+        self._handle = self._lib.rlgym_bpu_create(
+            str(self.model_path).encode("utf-8"),
+            _first_bpu_core(self.bpu_cores),
+            self.priority,
+            err,
+            len(err),
+        )
+        if not self._handle:
+            raise RuntimeError(err.value.decode("utf-8", errors="replace"))
+        self.input_size = int(self._lib.rlgym_bpu_input_floats(self._handle))
+        self.output_size = int(self._lib.rlgym_bpu_output_floats(self._handle))
+        if self.input_size <= 0 or self.output_size <= 0:
+            raise RuntimeError("invalid BPU tensor sizes reported by runtime")
+        self.input_shape = (1, 1, 1, self.input_size)
+        self.output_shape = (1, self.output_size, 1, 1)
+        self._output = np.empty(self.output_size, dtype=np.float32)
+        self._output_ptr = self._output.ctypes.data_as(ctypes.POINTER(ctypes.c_float))
+        self._err = ctypes.create_string_buffer(1024)
+
+        print(f"[INFO] BPU model: {self.model_path}")
+        print(f"[INFO]   Backend: {self.backend_name}")
+        print(f"[INFO]   Runtime: {self.cpp_lib}")
+        print(f"[INFO]   Input  : obs_4d {self.input_shape}")
+        print(f"[INFO]   Output : actions {self.output_shape}")
+
+    def close(self):
+        handle = getattr(self, "_handle", None)
+        if handle:
+            self._lib.rlgym_bpu_destroy(handle)
+            self._handle = None
+
+    def __del__(self):
+        try:
+            self.close()
+        except Exception:
+            pass
+
+    def __call__(self, flat_input):
+        arr = np.asarray(flat_input, dtype=np.float32)
+        if arr.size != self.input_size:
+            raise ValueError(f"BPU policy input has {arr.size} values, expected {self.input_size}")
+        input_flat = np.ascontiguousarray(arr.reshape(-1), dtype=np.float32)
+        rc = self._lib.rlgym_bpu_infer(
+            self._handle,
+            input_flat.ctypes.data_as(ctypes.POINTER(ctypes.c_float)),
+            self._output_ptr,
+            self._err,
+            len(self._err),
+        )
+        if rc != 0:
+            raise RuntimeError(self._err.value.decode("utf-8", errors="replace"))
+        if not np.all(np.isfinite(self._output)):
+            raise RuntimeError(f"BPU policy output is not finite: {self._output}")
+        return self._output.copy()
+
+
+class BpuInferLibPythonPolicy:
+    """Slower Python package backend kept only for comparison."""
+
+    backend_name = "bpu_infer_lib_x5_python"
+    input_shape = (1, 1, 1, 450)
+    output_shape = (1, 12, 1, 1)
+
+    def __init__(self, model_path, priority=0, bpu_cores=(0,), suppress_runtime_output=False):
+        self.model_path = Path(model_path).expanduser().resolve()
+        if not self.model_path.exists():
+            raise FileNotFoundError(f"BPU model not found: {self.model_path}")
+
+        try:
+            from bpu_infer_lib import Infer
+        except ImportError as exc:
+            raise RuntimeError(
+                "bpu_infer_lib is required for the fallback Python BPU backend. "
+                "Do not use hobot_dnn.pyeasy_dnn for this policy: it produced "
+                "wrong actions in local board tests."
+            ) from exc
+
+        self.priority = int(priority)
+        self.bpu_cores = tuple(int(core) for core in bpu_cores)
+        self.suppress_runtime_output = bool(suppress_runtime_output)
+        self.input_size = 450
+        self.input_shape = (1, 1, 1, self.input_size)
+        self.output_shape = (1, 12, 1, 1)
+        with suppress_c_output(self.suppress_runtime_output):
+            self.infer = Infer(False)
+            loaded = self.infer.load_model(str(self.model_path))
+        if not loaded:
+            raise RuntimeError(f"Failed to load BPU model: {self.model_path}")
+
+        print(f"[INFO] BPU model: {self.model_path}")
+        print(f"[INFO]   Backend: {self.backend_name}")
+        print(f"[INFO]   Input  : obs_4d {self.input_shape}")
+        print(f"[INFO]   Output : actions {self.output_shape}")
+
+    def __call__(self, flat_input):
+        arr = np.asarray(flat_input, dtype=np.float32)
+        if arr.size != self.input_size:
+            raise ValueError(f"BPU policy input has {arr.size} values, expected {self.input_size}")
+        input_4d = np.ascontiguousarray(arr.reshape(self.input_shape), dtype=np.float32)
+        with suppress_c_output(self.suppress_runtime_output):
+            copied = self.infer.read_numpy_arr_float32(input_4d, 0)
+            if copied:
+                self.infer.forward(True)
+                got_output = self.infer.get_output()
+                if not got_output:
+                    raise RuntimeError("bpu_infer_lib get_output() failed")
+                output = self.infer.get_infer_res_np_float32(0)
+        if not copied:
+            raise RuntimeError("Failed to copy float32 input into bpu_infer_lib")
+        action = np.asarray(output, dtype=np.float32).reshape(-1)
+        if action.size < 12:
+            raise RuntimeError(f"BPU policy output has {action.size} values, expected at least 12")
+        action = action[:12].astype(np.float32, copy=False)
+        if not np.all(np.isfinite(action)):
+            raise RuntimeError(f"BPU policy output is not finite: {action}")
+        return action

deploy_45dim_rl_gym/bpu_deploy_x5/cpp/bpu_dnn_bench.cpp

@@ -0,0 +1,119 @@
+#include <chrono>
+#include <algorithm>
+#include <cmath>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <fstream>
+#include <iostream>
+#include <string>
+#include <vector>
+
+extern "C" {
+void *rlgym_bpu_create(const char *model_path, int bpu_core, int priority,
+                       char *err, int err_len);
+int rlgym_bpu_infer(void *handle, const float *input, float *output, char *err,
+                    int err_len);
+int rlgym_bpu_input_floats(void *handle);
+int rlgym_bpu_output_floats(void *handle);
+void rlgym_bpu_destroy(void *handle);
+}
+
+namespace {
+
+constexpr float kReference[12] = {
+    0.537439f,  1.032869f,  0.042010f, -0.463015f,
+    0.515008f,  1.349627f,  0.014652f, -1.518314f,
+    1.012123f, -0.346840f,  0.086353f, -0.755527f,
+};
+
+bool read_f32_file(const std::string &path, std::vector<float> *data) {
+  std::ifstream ifs(path, std::ios::binary);
+  if (!ifs) {
+    return false;
+  }
+  ifs.seekg(0, std::ios::end);
+  const auto size = ifs.tellg();
+  ifs.seekg(0, std::ios::beg);
+  if (size <= 0 || size % static_cast<std::streamoff>(sizeof(float)) != 0) {
+    return false;
+  }
+  data->resize(static_cast<size_t>(size) / sizeof(float));
+  ifs.read(reinterpret_cast<char *>(data->data()), size);
+  return ifs.good();
+}
+
+}  // namespace
+
+int main(int argc, char **argv) {
+  const char *model =
+      "/root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/mapper_output_6500_gemm/"
+      "policy_robotlab_6500_int16_gemm.bin";
+  const char *input =
+      "/root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/calibration_data/00000.bin";
+  int repeat = 1000;
+  if (argc > 1) model = argv[1];
+  if (argc > 2) input = argv[2];
+  if (argc > 3) repeat = std::atoi(argv[3]);
+
+  char err[1024] = {};
+  void *handle = rlgym_bpu_create(model, 1, 0, err, sizeof(err));
+  if (handle == nullptr) {
+    std::cerr << err << "\n";
+    return 3;
+  }
+
+  const int input_floats = rlgym_bpu_input_floats(handle);
+  const int output_floats = rlgym_bpu_output_floats(handle);
+  if (input_floats <= 0 || output_floats <= 0) {
+    std::cerr << "invalid tensor sizes from BPU runtime\n";
+    rlgym_bpu_destroy(handle);
+    return 2;
+  }
+
+  std::vector<float> obs;
+  if (!read_f32_file(input, &obs) || static_cast<int>(obs.size()) != input_floats) {
+    std::cerr << "failed to read " << input_floats << " float32 input: " << input << "\n";
+    rlgym_bpu_destroy(handle);
+    return 2;
+  }
+
+  std::vector<float> out(static_cast<size_t>(output_floats), 0.0f);
+  if (rlgym_bpu_infer(handle, obs.data(), out.data(), err, sizeof(err)) != 0) {
+    std::cerr << err << "\n";
+    rlgym_bpu_destroy(handle);
+    return 4;
+  }
+  float max_diff = 0.0f;
+  if (input_floats == 450 && output_floats == 12) {
+    for (int i = 0; i < 12; ++i) {
+      max_diff = std::max(max_diff, std::fabs(out[static_cast<size_t>(i)] - kReference[i]));
+    }
+  }
+
+  const auto t0 = std::chrono::steady_clock::now();
+  for (int i = 0; i < repeat; ++i) {
+    if (rlgym_bpu_infer(handle, obs.data(), out.data(), err, sizeof(err)) != 0) {
+      std::cerr << err << "\n";
+      rlgym_bpu_destroy(handle);
+      return 5;
+    }
+  }
+  const auto t1 = std::chrono::steady_clock::now();
+  const double elapsed_ms =
+      std::chrono::duration<double, std::milli>(t1 - t0).count();
+
+  std::cout << "input_floats=" << input_floats << " output_floats=" << output_floats << "\n";
+  if (input_floats == 450 && output_floats == 12) {
+    std::cout << "reference_max_abs_diff " << max_diff << "\n";
+  }
+  std::cout << "repeat=" << repeat << " cpp_avg_ms=" << (elapsed_ms / repeat) << "\n";
+  std::cout << "action0=" << out[0] << " action_max_abs="
+            << *std::max_element(out.begin(), out.end(), [](float a, float b) {
+                 return std::fabs(a) < std::fabs(b);
+               })
+            << "\n";
+
+  rlgym_bpu_destroy(handle);
+  return 0;
+}

deploy_45dim_rl_gym/bpu_deploy_x5/cpp/bpu_dnn_policy.cpp

@@ -0,0 +1,239 @@
+#include <algorithm>
+#include <chrono>
+#include <cstring>
+#include <exception>
+#include <memory>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include <dnn/hb_dnn.h>
+#include <dnn/hb_dnn_ext.h>
+#include <dnn/hb_dnn_status.h>
+
+namespace {
+
+constexpr int kOutputFloats = 12;
+
+void set_error(char *err, int err_len, const std::string &msg) {
+  if (err == nullptr || err_len <= 0) {
+    return;
+  }
+  std::snprintf(err, static_cast<size_t>(err_len), "%s", msg.c_str());
+}
+
+std::string dnn_error(const std::string &where, int code) {
+  std::ostringstream oss;
+  oss << where << " failed: " << code;
+  const char *desc = hbDNNGetErrorDesc(code);
+  if (desc != nullptr) {
+    oss << " (" << desc << ")";
+  }
+  return oss.str();
+}
+
+int element_count(const hbDNNTensorShape &shape) {
+  int count = 1;
+  for (int i = 0; i < shape.numDimensions; ++i) {
+    count *= shape.dimensionSize[i];
+  }
+  return count;
+}
+
+void check_dnn(int code, const std::string &where) {
+  if (code != 0) {
+    throw std::runtime_error(dnn_error(where, code));
+  }
+}
+
+class BpuDnnPolicy {
+ public:
+  BpuDnnPolicy(const char *model_path, int bpu_core, int priority)
+      : bpu_core_(bpu_core), priority_(priority) {
+    if (model_path == nullptr || model_path[0] == '\0') {
+      throw std::runtime_error("empty model path");
+    }
+
+    const char *model_files[] = {model_path};
+    check_dnn(hbDNNInitializeFromFiles(&packed_handle_, model_files, 1),
+              "hbDNNInitializeFromFiles");
+
+    const char **model_names = nullptr;
+    int32_t model_count = 0;
+    check_dnn(hbDNNGetModelNameList(&model_names, &model_count, packed_handle_),
+              "hbDNNGetModelNameList");
+    if (model_count <= 0 || model_names == nullptr || model_names[0] == nullptr) {
+      throw std::runtime_error("model has no names");
+    }
+    model_name_ = model_names[0];
+    check_dnn(hbDNNGetModelHandle(&dnn_handle_, packed_handle_, model_names[0]),
+              "hbDNNGetModelHandle");
+
+    int32_t input_count = 0;
+    int32_t output_count = 0;
+    check_dnn(hbDNNGetInputCount(&input_count, dnn_handle_), "hbDNNGetInputCount");
+    check_dnn(hbDNNGetOutputCount(&output_count, dnn_handle_), "hbDNNGetOutputCount");
+    if (input_count != 1 || output_count != 1) {
+      std::ostringstream oss;
+      oss << "expected 1 input and 1 output, got " << input_count << " inputs and "
+          << output_count << " outputs";
+      throw std::runtime_error(oss.str());
+    }
+
+    input_tensors_.resize(1);
+    output_tensors_.resize(1);
+    check_dnn(hbDNNGetInputTensorProperties(&input_tensors_[0].properties, dnn_handle_, 0),
+              "hbDNNGetInputTensorProperties");
+    check_dnn(hbDNNGetOutputTensorProperties(&output_tensors_[0].properties, dnn_handle_, 0),
+              "hbDNNGetOutputTensorProperties");
+
+    input_floats_ = element_count(input_tensors_[0].properties.validShape);
+    output_floats_ = element_count(output_tensors_[0].properties.validShape);
+    validate_tensor(input_tensors_[0].properties, HB_DNN_TENSOR_TYPE_F32, "input");
+    validate_tensor(output_tensors_[0].properties, HB_DNN_TENSOR_TYPE_F32, "output");
+
+    alloc_tensor_mem(input_tensors_[0]);
+    alloc_tensor_mem(output_tensors_[0]);
+  }
+
+  ~BpuDnnPolicy() {
+    release_tensor_mem(input_tensors_);
+    release_tensor_mem(output_tensors_);
+    if (packed_handle_ != nullptr) {
+      hbDNNRelease(packed_handle_);
+      packed_handle_ = nullptr;
+    }
+  }
+
+  void infer(const float *input, float *output) {
+    if (input == nullptr || output == nullptr) {
+      throw std::runtime_error("null input/output pointer");
+    }
+
+    auto &input_mem = input_tensors_[0].sysMem[0];
+    std::memcpy(input_mem.virAddr, input, sizeof(float) * input_floats_);
+    int code = hbSysFlushMem(&input_mem, HB_SYS_MEM_CACHE_CLEAN);
+    if (code != 0) {
+      throw std::runtime_error(dnn_error("hbSysFlushMem(input)", code));
+    }
+
+    hbDNNInferCtrlParam ctrl;
+    HB_DNN_INITIALIZE_INFER_CTRL_PARAM(&ctrl);
+    ctrl.bpuCoreId = bpu_core_;
+    ctrl.priority = priority_;
+    ctrl.more = false;
+
+    hbDNNTaskHandle_t task_handle = nullptr;
+    hbDNNTensor *output_ptr = output_tensors_.data();
+    check_dnn(hbDNNInfer(&task_handle, &output_ptr, input_tensors_.data(), dnn_handle_, &ctrl),
+              "hbDNNInfer");
+    check_dnn(hbDNNWaitTaskDone(task_handle, 0), "hbDNNWaitTaskDone");
+    check_dnn(hbDNNReleaseTask(task_handle), "hbDNNReleaseTask");
+    task_handle = nullptr;
+
+    auto &output_mem = output_tensors_[0].sysMem[0];
+    code = hbSysFlushMem(&output_mem, HB_SYS_MEM_CACHE_INVALIDATE);
+    if (code != 0) {
+      throw std::runtime_error(dnn_error("hbSysFlushMem(output)", code));
+    }
+    std::memcpy(output, output_mem.virAddr, sizeof(float) * output_floats_);
+  }
+
+  int input_floats() const { return input_floats_; }
+  int output_floats() const { return output_floats_; }
+
+ private:
+  static void validate_tensor(const hbDNNTensorProperties &props, int expected_type,
+                              const char *name) {
+    if (props.tensorType != expected_type) {
+      std::ostringstream oss;
+      oss << name << " tensor type " << props.tensorType << " != " << expected_type;
+      throw std::runtime_error(oss.str());
+    }
+    if (props.alignedByteSize <= 0) {
+      throw std::runtime_error(std::string(name) + " alignedByteSize <= 0");
+    }
+  }
+
+  static void alloc_tensor_mem(hbDNNTensor &tensor) {
+    std::memset(tensor.sysMem, 0, sizeof(tensor.sysMem));
+    const uint32_t bytes = static_cast<uint32_t>(tensor.properties.alignedByteSize);
+    const int code = hbSysAllocCachedMem(&tensor.sysMem[0], bytes);
+    if (code != 0) {
+      throw std::runtime_error(dnn_error("hbSysAllocCachedMem", code));
+    }
+  }
+
+  static void release_tensor_mem(std::vector<hbDNNTensor> &tensors) {
+    for (auto &tensor : tensors) {
+      if (tensor.sysMem[0].virAddr != nullptr || tensor.sysMem[0].phyAddr != 0) {
+        hbSysFreeMem(&tensor.sysMem[0]);
+        std::memset(tensor.sysMem, 0, sizeof(tensor.sysMem));
+      }
+    }
+  }
+
+  hbPackedDNNHandle_t packed_handle_{nullptr};
+  hbDNNHandle_t dnn_handle_{nullptr};
+  std::string model_name_;
+  std::vector<hbDNNTensor> input_tensors_;
+  std::vector<hbDNNTensor> output_tensors_;
+  int input_floats_{0};
+  int output_floats_{0};
+  int bpu_core_{HB_BPU_CORE_ANY};
+  int priority_{HB_DNN_PRIORITY_LOWEST};
+};
+
+}  // namespace
+
+extern "C" {
+
+void *rlgym_bpu_create(const char *model_path, int bpu_core, int priority,
+                       char *err, int err_len) {
+  try {
+    set_error(err, err_len, "");
+    return new BpuDnnPolicy(model_path, bpu_core, priority);
+  } catch (const std::exception &e) {
+    set_error(err, err_len, e.what());
+    return nullptr;
+  }
+}
+
+int rlgym_bpu_infer(void *handle, const float *input, float *output, char *err,
+                    int err_len) {
+  try {
+    set_error(err, err_len, "");
+    if (handle == nullptr) {
+      throw std::runtime_error("null policy handle");
+    }
+    static_cast<BpuDnnPolicy *>(handle)->infer(input, output);
+    return 0;
+  } catch (const std::exception &e) {
+    set_error(err, err_len, e.what());
+    return -1;
+  }
+}
+
+int rlgym_bpu_input_floats(void *handle) {
+  if (handle == nullptr) {
+    return 0;
+  }
+  return static_cast<BpuDnnPolicy *>(handle)->input_floats();
+}
+
+int rlgym_bpu_output_floats(void *handle) {
+  if (handle == nullptr) {
+    return 0;
+  }
+  return static_cast<BpuDnnPolicy *>(handle)->output_floats();
+}
+
+void rlgym_bpu_destroy(void *handle) {
+  delete static_cast<BpuDnnPolicy *>(handle);
+}
+
+const char *rlgym_bpu_version() {
+  return "cpp_dnn_api_x5";
+}
+
+}

deploy_45dim_rl_gym/bpu_deploy_x5/cpp/build_board.sh

@@ -0,0 +1,20 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+cd "$(dirname "$0")"
+
+g++ -O3 -DNDEBUG -std=c++17 -fPIC -shared \
+  bpu_dnn_policy.cpp \
+  -I/usr/include \
+  -ldnn \
+  -o libbpu_dnn_policy.so
+
+g++ -O3 -DNDEBUG -std=c++17 \
+  bpu_dnn_bench.cpp \
+  -I/usr/include \
+  -L. -lbpu_dnn_policy \
+  -Wl,-rpath,'$ORIGIN' \
+  -o bpu_dnn_bench
+
+ls -lh libbpu_dnn_policy.so
+ls -lh bpu_dnn_bench

deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_gym_bpu_x5_fastcpp.py

@@ -0,0 +1,10 @@
+#!/usr/bin/env python3
+"""Compatibility wrapper for the Gym 5-frame BPU deployment entrypoint."""
+
+import runpy
+from pathlib import Path
+
+
+if __name__ == "__main__":
+    entry = Path(__file__).resolve().parents[1] / "deploy_go1_rlgym_bpu_x5_fastcpp.py"
+    runpy.run_path(str(entry), run_name="__main__")

deploy_45dim_rl_gym/bpu_deploy_x5/test_bpu_policy.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+"""Offline BPU policy smoke test for RDK X5.
+
+This does not connect to the robot. It loads a BPU .bin and one raw float32
+input file, runs the C++ DNN API backend repeatedly, and optionally checks the
+known RobotLab 00000.bin reference output captured from hrt_model_exec.
+"""
+
+import argparse
+import time
+from pathlib import Path
+
+import numpy as np
+
+from bpu_policy import BpuInferLibPolicy
+
+
+HERE = Path(__file__).parent.resolve()
+DEFAULT_MODEL = (
+    HERE.parent / "bpu_quantization" / "mapper_output_6500_gemm" /
+    "policy_robotlab_6500_int16_gemm.bin"
+)
+DEFAULT_INPUT = HERE.parent / "bpu_quantization" / "calibration_data" / "00000.bin"
+REFERENCE_00000 = np.array([
+    0.537439,
+    1.032869,
+    0.042010,
+    -0.463015,
+    0.515008,
+    1.349627,
+    0.014652,
+    -1.518314,
+    1.012123,
+    -0.346840,
+    0.086353,
+    -0.755527,
+], dtype=np.float32)
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Offline BPU policy smoke test")
+    parser.add_argument("--bpu-model", default=str(DEFAULT_MODEL))
+    parser.add_argument("--input-bin", default=str(DEFAULT_INPUT))
+    parser.add_argument("--repeat", type=int, default=1000)
+    parser.add_argument("--check-reference-00000", action="store_true")
+    args = parser.parse_args()
+
+    input_path = Path(args.input_bin).expanduser().resolve()
+    data = np.fromfile(input_path, dtype=np.float32)
+    policy = BpuInferLibPolicy(args.bpu_model)
+    if data.size != policy.input_size:
+        raise ValueError(
+            f"{input_path} has {data.size} float32 values, "
+            f"but model expects {policy.input_size}"
+        )
+    action = policy(data)
+    print("action", np.array2string(action, precision=6))
+    print("action_max_abs", float(np.max(np.abs(action))))
+
+    if args.check_reference_00000:
+        if policy.input_size != 450:
+            raise ValueError("--check-reference-00000 is only valid for the 450-dim RobotLab reference input")
+        diff = np.abs(action - REFERENCE_00000)
+        print("reference_max_abs_diff", float(diff.max()))
+        print("reference_mean_abs_diff", float(diff.mean()))
+        if diff.max() > 1e-5:
+            raise RuntimeError("BPU output does not match the known 00000.bin reference")
+
+    repeats = max(1, int(args.repeat))
+    t0 = time.perf_counter()
+    for _ in range(repeats):
+        policy(data)
+    elapsed_ms = (time.perf_counter() - t0) * 1000.0
+    print(f"repeat={repeats} avg_ms={elapsed_ms / repeats:.6f}")
+
+
+if __name__ == "__main__":
+    main()

deploy_45dim_rl_gym/bpu_quantization/README.md

@@ -0,0 +1,356 @@
+# RDK X5 BPU 量化流程
+
+这个目录用于把 Gym/RobotLab 的 ONNX 策略转成 RDK X5 可运行的 Horizon
+runtime `.bin`。量化在 Mac 上用 CPU Docker 完成，板端可以做离线测速和实机
+部署测试。
+
+参考资料：
+
+- D-Robotics 官方 X5 工具链说明：`https://developer.d-robotics.cc/rdk_x_doc/Advanced_development/toolchain_development/overview?v=3.5.0&p=RDK+X5`
+- D-Robotics 论坛 WTW/Go2/X5 流程：`https://forum.d-robotics.cc/t/topic/28338`
+
+官方工具链对 ONNX 的关键限制是：`ir_version <= 7`、`opset10/11`、固定
+4 维输入，且 N 维只能为 1。因此这里不能直接拿原始 ONNX 编译，需要先裁剪
+actions-only 输出、降级 opset，再把 `[1, D]` 输入包成固定 4D `NCHW`：
+Gym 5 帧是 `[1, 1, 1, 225]`，RobotLab 10 帧是 `[1, 1, 1, 450]`。
+
+## 当前状态
+
+新增 Gym 5 帧策略的一键量化入口，默认目标是：
+
+- 原始模型：`../policy_35k.onnx`
+- 原始输入：`obs [1, 225]`
+- BPU 编译输入：`obs_4d [1, 1, 1, 225]`
+- BPU 输出：`actions [1, 12, 1, 1]`
+- 默认输出：`mapper_output_35k_gemm/policy_35k_int16_gemm.bin`
+- 默认校准数据：`calibration_data_35k_gym_fast64/`
+
+本机 Docker 已完成一次默认量化：
+
+- 浮点 4D/Gemm 图等价对比：64 个真实样本上 `max_abs_diff = 7.15e-7`
+- `hb_mapper makertbin` 输出：`actions` cosine `0.998524`，L1 `0.014313`，L2 `0.005103`，Chebyshev `0.040004`
+- 编译估计延迟：`463.9 us`
+- 产物大小：`2.0M`
+- 产物路径：`deploy_45dim_rl_gym/bpu_quantization/mapper_output_35k_gemm/policy_35k_int16_gemm.bin`
+
+一键量化命令：
+
+```bash
+cd /Users/chenyouyuan/cyy_ws/deploy_go1_pro/deploy_45dim_rl_gym/bpu_quantization
+./quantize_policy_x5.sh
+```
+
+切换其他 Gym 轮次时直接指定模型和 round：
+
+```bash
+./quantize_policy_x5.sh --policy ../policy_30k.onnx --round 30k
+./quantize_policy_x5.sh --policy ../policy_25k.onnx --round 25k
+./quantize_policy_x5.sh --policy ../policy_15k.onnx --round 15k
+```
+
+脚本默认只抽 64 个真实 RL 样本做校准和最多 64 个样本做浮点等价对比，避免
+之前 512 样本和 batch 回退导致的量化流程过慢。需要更稳的校准时再手动加大：
+
+```bash
+./quantize_policy_x5.sh --samples 128 --compare-limit 128
+```
+
+速度优先 int8 对照：
+
+```bash
+./quantize_policy_x5.sh --quant int8
+```
+
+本次 `policy_35k` int8 结果不建议直接实机使用：
+
+- 产物：`mapper_output_35k_int8_gemm/policy_35k_int8_gemm.bin`
+- `actions` cosine：`0.981343`
+- L1/L2/Chebyshev：`0.053474 / 0.018129 / 0.113345`
+- mapper 编译估计延迟：`314.2 us + 49.7 us`
+- 板端 `hrt_model_exec perf` 实测：`1.583870 ms`
+- 板端 C++ bench：`1.56478 ms`
+
+对比当前 int16：
+
+- `actions` cosine：`0.998524`
+- 板端 `hrt_model_exec perf` 实测：`1.401526 ms`
+- 板端 C++ bench：`1.37792 ms`
+
+结论：这个模型上默认 int8 既更慢，输出误差也更大。当前继续用
+`policy_35k_int16_gemm.bin` 更合理。
+
+Gym BPU 部署入口支持快速切换轮次：
+
+```bash
+cd /root/go1_pro_deploy
+PYTHONPATH=/root/go1_pro_deploy python3 deploy_45dim_rl_gym/deploy_go1_rlgym_bpu_x5_fastcpp.py \
+  --bpu-round 35k \
+  --kill-sport \
+  --enable-rl \
+  --log-dir logs \
+  --kp 32 --kd 1.0 \
+  --kp-cal 20 --kd-cal 1.0 \
+  --power-factor 9 \
+  --position-protect-limit 0.0 \
+  --action-clip 6.5 \
+  --action-trip-limit 8.0 \
+  --action-hard-trip-limit 16.0 \
+  --max-target-step 0.0 \
+  --max-roll-deg 50 \
+  --max-pitch-deg 50 \
+  --swap-vy-yaw \
+  --rc-vx-scale 0.5 \
+  --rc-vy-scale 0.5 \
+  --rc-wz-scale 1.0 \
+  --log-timing
+```
+
+也可以直接指定 bin：
+
+```bash
+PYTHONPATH=/root/go1_pro_deploy python3 deploy_45dim_rl_gym/deploy_go1_rlgym_bpu_x5_fastcpp.py \
+  --bpu-model deploy_45dim_rl_gym/bpu_quantization/mapper_output_35k_gemm/policy_35k_int16_gemm.bin \
+  --infer-check --max-steps 1000 --log-timing
+```
+
+板端离线测速：
+
+```bash
+cd /root/go1_pro_deploy
+PYTHONPATH=/root/go1_pro_deploy python3 deploy_45dim_rl_gym/bpu_deploy_x5/test_bpu_policy.py \
+  --bpu-model deploy_45dim_rl_gym/bpu_quantization/mapper_output_35k_gemm/policy_35k_int16_gemm.bin \
+  --input-bin deploy_45dim_rl_gym/bpu_quantization/calibration_data_35k_gym_fast64/00000.bin \
+  --repeat 1000
+
+cd /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_deploy_x5/cpp
+./bpu_dnn_bench \
+  /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/mapper_output_35k_gemm/policy_35k_int16_gemm.bin \
+  /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/calibration_data_35k_gym_fast64/00000.bin \
+  1000
+```
+
+已经完成 `policy_robotlab_15000.onnx`、`policy_robotlab_6500.onnx` 和
+`policy_robotlab_26000.onnx` 的 int16 量化。RobotLab BPU 部署当前默认使用
+26000 版本：
+
+- 原始模型：`../policy_robotlab_26000.onnx`
+- 原始输入：`obs [1, 450]`
+- BPU 编译输入：`obs_4d [1, 1, 1, 450]`
+- BPU 输出：`actions [1, 12, 1, 1]`
+- Docker 镜像：`openexplorer/ai_toolchain_ubuntu_20_x5_cpu:v1.2.8`
+- `hb_mapper`：`1.24.3`
+- `hbdk`：`3.49.15`
+- 当前产物：`mapper_output_26000_gemm/policy_robotlab_26000_int16_gemm.bin`
+
+`mapper_output*/`、`.hb_check/`、校准数据、中间 ONNX、`hb_mapper` 日志都被
+`.gitignore` 忽略；需要时按下面步骤重新生成。仓库里只保留脚本和 YAML 配置。
+
+26000 量化使用 `calibration_data_26000_robotlab_fast64/` 的 64 个真实样本。原因是
+`hb_mapper` 会先尝试 calibration batch 8，但当前 4D featuremap 包装会被工具链
+内部改成固定 batch 的 reshape，batch 8 失败后会退回 batch 1；用 64 样本可以把
+校准时间压到可接受范围。
+
+如果你要回退到旧版：
+
+- `mapper_output_6500_gemm/policy_robotlab_6500_int16_gemm.bin`
+- `mapper_output_gemm/policy_robotlab_15000_int16_gemm.bin`
+
+## 1. 生成校准数据
+
+在本机 `deploy_go1_pro` 根目录执行：
+
+```bash
+/Users/chenyouyuan/miniconda/envs/free_dog_sdk/bin/python3.10 \
+  deploy_45dim_rl_gym/bpu_quantization/make_calibration_data.py \
+  --max-samples 512 \
+  --overwrite
+```
+
+脚本会从 `logs/robotlab_go1_deploy_*/steps.jsonl` 中提取 RL 状态下的真实
+`obs_single`，按部署代码相同的 10 帧历史顺序重建 450 维输入，并写成
+`float32` feature-map `.bin`。
+
+本次校准数据：
+
+- 候选 RL 输入：`62606`
+- 选中样本：`512`
+- 单个样本形状：`1x1x1x450`
+- 元数据：`deploy_45dim_rl_gym/bpu_quantization/calibration_data_metadata.json`
+
+## 2. 生成 opset11 和 4D ONNX
+
+启动 Docker Desktop 后，在 `deploy_go1_pro` 根目录执行：
+
+```bash
+docker run --rm --platform linux/amd64 \
+  -v "$PWD:/workspace/deploy_go1_pro" \
+  openexplorer/ai_toolchain_ubuntu_20_x5_cpu:v1.2.8 \
+  bash -lc 'cd /workspace/deploy_go1_pro/deploy_45dim_rl_gym/bpu_quantization &&
+    python3 downgrade_policy_to_opset11.py \
+      --input ../policy_robotlab_15000.onnx \
+      --output policy_robotlab_15000_opset11.onnx &&
+    python3 make_bpu_4d_onnx.py \
+      --input policy_robotlab_15000_opset11.onnx \
+      --output policy_robotlab_15000_bpu4d.onnx &&
+    python3 compare_4d_onnx.py \
+      --flat-onnx ../policy_robotlab_15000.onnx \
+      --bpu4d-onnx policy_robotlab_15000_bpu4d.onnx \
+      --limit 256'
+```
+
+本次对比结果：256 个真实输入上 `max_abs_diff = 0`，说明降级和 4D 包装没有
+改变浮点 ONNX 输出。
+
+## 3. 可选：替换 MoE grouped Conv
+
+原始导出的 MoE expert 末端有一个 `group=8, kernel=1` 的 `Conv1d`：
+
+```text
+[1, 2048] -> Unsqueeze -> Conv(group=8, kernel=1) -> Squeeze -> [1, 256]
+```
+
+在 X5 checker 里，这段会切到 CPU float，导致 BPU 子图被拆开。这个 Conv
+等价于一个块对角 `Gemm`，可以不重训直接改图：
+
+```bash
+docker run --rm --platform linux/amd64 \
+  -v "$PWD:/workspace/deploy_go1_pro" \
+  openexplorer/ai_toolchain_ubuntu_20_x5_cpu:v1.2.8 \
+  bash -lc 'cd /workspace/deploy_go1_pro/deploy_45dim_rl_gym/bpu_quantization &&
+    python3 replace_group_conv_with_gemm.py \
+      --input policy_robotlab_15000_bpu4d.onnx \
+      --output policy_robotlab_15000_bpu4d_gemm.onnx &&
+    python3 compare_4d_onnx.py \
+      --flat-onnx ../policy_robotlab_15000.onnx \
+      --bpu4d-onnx policy_robotlab_15000_bpu4d_gemm.onnx \
+      --limit 512'
+```
+
+本次浮点对比结果：
+
+- `max_abs_diff = 1.43e-6`
+- `max_mean_abs_diff = 3.53e-7`
+
+这是浮点舍入级误差，可以认为图变换等价。
+
+注意：这里没有把 `ReduceL2/Pow/Reshape/Sqrt` 这类 norm 算子挪到后处理，
+因为它们在 actor 之前，属于策略中间计算，不能当成输出后处理裁掉。当前
+`hb_mapper makertbin` 已经能把这些 norm 节点放到 BPU 上。
+
+## 4. checker 检查
+
+```bash
+docker run --rm --platform linux/amd64 \
+  -v "$PWD:/workspace/deploy_go1_pro" \
+  openexplorer/ai_toolchain_ubuntu_20_x5_cpu:v1.2.8 \
+  bash -lc 'cd /workspace/deploy_go1_pro/deploy_45dim_rl_gym/bpu_quantization &&
+    hb_mapper checker \
+      --model policy_robotlab_15000_bpu4d_gemm.onnx \
+      --model-type onnx \
+      --march bayes-e \
+      --input-shape obs_4d 1x1x1x450'
+```
+
+checker 可以通过。原始模型中 MoE expert 的 `Elu/Reshape/Conv` CPU fallback
+会被消掉；checker 阶段仍可能显示 gating `Softmax` 或输出 reshape 的 CPU
+适配，正式 int16 编译时 `Softmax` 会被量化拆成 BPU 子算子。
+
+## 5. 编译 int16 `.bin`
+
+```bash
+docker run --rm --platform linux/amd64 \
+  -v "$PWD:/workspace/deploy_go1_pro" \
+  openexplorer/ai_toolchain_ubuntu_20_x5_cpu:v1.2.8 \
+  bash -lc 'cd /workspace/deploy_go1_pro/deploy_45dim_rl_gym/bpu_quantization &&
+    hb_mapper makertbin \
+      --config policy_robotlab_15000_int16_gemm.yaml \
+      --model-type onnx'
+```
+
+优化版 `hb_mapper` 输出的量化精度：
+
+```text
+Output   Cosine Similarity   L1 Distance   L2 Distance   Chebyshev Distance
+actions  0.999910            0.009500      0.003393      0.024229
+```
+
+优化版正式 int16 编译后，日志中列出的策略节点全部在同一个 BPU 子图
+`id(0)` 上，原始模型的 CPU `Elu/Reshape/Conv` fallback 已消失。
+
+优化版 BPU 子图编译估计延迟：
+
+- subgraph0：`424.5 us`
+
+原始版 BPU 子图估计延迟是 `284.3 us + 80.8 us`，但带 CPU/hybrid 切换。
+优化版单子图的编译估计延迟略高，实际是否更快要以板端 `hrt_model_exec perf`
+为准。
+
+## 6. 板端离线验证
+
+把 `.bin` 和一个校准输入传到板端，例如：
+
+```bash
+ssh root@192.168.150.167 'mkdir -p /root/go1_pro_deploy/bpu_quant_test'
+scp \
+  deploy_45dim_rl_gym/bpu_quantization/mapper_output_gemm/policy_robotlab_15000_int16_gemm.bin \
+  deploy_45dim_rl_gym/bpu_quantization/calibration_data/00000.bin \
+  root@192.168.150.167:/root/go1_pro_deploy/bpu_quant_test/
+```
+
+板端查看模型：
+
+```bash
+cd /root/go1_pro_deploy/bpu_quant_test
+hrt_model_exec model_info --model_file policy_robotlab_15000_int16_gemm.bin
+```
+
+板端测速：
+
+```bash
+hrt_model_exec perf \
+  --model_file policy_robotlab_15000_int16_gemm.bin \
+  --model_name policy_robotlab_15000_int16_gemm \
+  --input_file 00000.bin \
+  --frame_count 1000 \
+  --thread_num 1
+```
+
+原始版板端结果：
+
+- 平均延迟：`1.498257 ms`
+- 最大延迟：`2.942 ms`
+- FPS：`664.77`
+
+优化版板端结果：
+
+- 平均延迟：`0.872819 ms`
+- 最大延迟：`1.627 ms`
+- 最小延迟：`0.613 ms`
+- FPS：`1133.28`
+
+同一板端之前测 CPU ONNX 大约是 `1.63 ms`。原始 BPU hybrid `.bin` 只快了一点；
+Gemm 优化版移掉 MoE 中间 CPU fallback 后，板端离线平均延迟比 CPU ONNX 快约
+46%，比原始 BPU hybrid 快约 42%。
+
+原始版板端单样本输出与本机浮点 ONNX 对比：
+
+- `max_abs = 0.02545`
+- `mean_abs = 0.01040`
+- `l2 = 0.04584`
+
+优化版板端单样本输出与本机浮点 ONNX 对比：
+
+- `max_abs = 0.02528`
+- `mean_abs = 0.00962`
+- `l2 = 0.04187`
+
+这个误差对离线验证是可接受的，但还不足以直接上实机。
+
+## 7. 安全结论
+
+当前建议只做离线推理验证，不要把 `.bin` 接入真实机器人控制循环。原因：
+
+- 原始 BPU 端到端速度提升很小，不能解决目前右前腿掉线、力矩保护、上下坡不稳这些核心问题。
+- Gemm 优化版已经改善离线推理速度，但还没有做部署代码适配和悬空状态机测试。
+- 输出 shape 从 `[1, 12]` 变成 `[1, 12, 1, 1]`，部署代码需要单独适配。
+- 接入前至少要做更多 held-out 真实日志对比、悬空状态机测试，再进入地面低速测试。

deploy_45dim_rl_gym/bpu_quantization/calibration_data_metadata.json

@@ -0,0 +1,76 @@
+{
+  "format": "raw float32 feature-map",
+  "flat_shape": [
+    1,
+    450
+  ],
+  "mapper_shape": [
+    1,
+    1,
+    1,
+    450
+  ],
+  "history_len": 10,
+  "num_obs": 45,
+  "term_dims": [
+    3,
+    3,
+    3,
+    12,
+    12,
+    12
+  ],
+  "selected_samples": 512,
+  "candidate_rl_inputs": 62606,
+  "seed": 20260727,
+  "runs": [
+    {
+      "steps": "/Users/chenyouyuan/cyy_ws/deploy_go1_pro/logs/robotlab_go1_deploy_20260726_221258/steps.jsonl",
+      "samples": 6644
+    },
+    {
+      "steps": "/Users/chenyouyuan/cyy_ws/deploy_go1_pro/logs/robotlab_go1_deploy_20260726_222533/steps.jsonl",
+      "samples": 292
+    },
+    {
+      "steps": "/Users/chenyouyuan/cyy_ws/deploy_go1_pro/logs/robotlab_go1_deploy_20260726_230656/steps.jsonl",
+      "samples": 620
+    },
+    {
+      "steps": "/Users/chenyouyuan/cyy_ws/deploy_go1_pro/logs/robotlab_go1_deploy_20260726_230816/steps.jsonl",
+      "samples": 13705
+    },
+    {
+      "steps": "/Users/chenyouyuan/cyy_ws/deploy_go1_pro/logs/robotlab_go1_deploy_20260727_171014/steps.jsonl",
+      "samples": 275
+    },
+    {
+      "steps": "/Users/chenyouyuan/cyy_ws/deploy_go1_pro/logs/robotlab_go1_deploy_20260727_171147/steps.jsonl",
+      "samples": 8928
+    },
+    {
+      "steps": "/Users/chenyouyuan/cyy_ws/deploy_go1_pro/logs/robotlab_go1_deploy_20260727_171522/steps.jsonl",
+      "samples": 5737
+    },
+    {
+      "steps": "/Users/chenyouyuan/cyy_ws/deploy_go1_pro/logs/robotlab_go1_deploy_20260727_172753/steps.jsonl",
+      "samples": 1759
+    },
+    {
+      "steps": "/Users/chenyouyuan/cyy_ws/deploy_go1_pro/logs/robotlab_go1_deploy_20260727_173528/steps.jsonl",
+      "samples": 6049
+    },
+    {
+      "steps": "/Users/chenyouyuan/cyy_ws/deploy_go1_pro/logs/robotlab_go1_deploy_20260727_175350/steps.jsonl",
+      "samples": 2498
+    },
+    {
+      "steps": "/Users/chenyouyuan/cyy_ws/deploy_go1_pro/logs/robotlab_go1_deploy_20260727_180352/steps.jsonl",
+      "samples": 345
+    },
+    {
+      "steps": "/Users/chenyouyuan/cyy_ws/deploy_go1_pro/logs/robotlab_go1_deploy_20260727_180434/steps.jsonl",
+      "samples": 15754
+    }
+  ]
+}

deploy_45dim_rl_gym/bpu_quantization/compare_4d_onnx.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python3
+"""Compare original flat ONNX outputs with the 4D BPU-wrapper ONNX."""
+
+import argparse
+from pathlib import Path
+
+import numpy as np
+import onnxruntime as ort
+
+
+def load_samples(calibration_dir, limit, flat_dim):
+    paths = sorted(Path(calibration_dir).glob("*.bin"))[:limit]
+    if not paths:
+        raise FileNotFoundError(f"No calibration .bin files found in {calibration_dir}")
+    samples = []
+    for path in paths:
+        sample = np.fromfile(path, dtype=np.float32)
+        if sample.size != flat_dim:
+            raise ValueError(f"{path} has {sample.size} float32 values, expected {flat_dim}")
+        samples.append(sample.reshape(1, flat_dim))
+    return samples
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--flat-onnx", type=Path, required=True)
+    parser.add_argument("--bpu4d-onnx", type=Path, required=True)
+    parser.add_argument("--calibration-dir", type=Path, default=Path("calibration_data"))
+    parser.add_argument("--flat-dim", type=int, default=450)
+    parser.add_argument("--limit", type=int, default=64)
+    args = parser.parse_args()
+
+    flat = ort.InferenceSession(str(args.flat_onnx), providers=["CPUExecutionProvider"])
+    wrapped = ort.InferenceSession(str(args.bpu4d_onnx), providers=["CPUExecutionProvider"])
+    flat_input = flat.get_inputs()[0].name
+    wrapped_input = wrapped.get_inputs()[0].name
+
+    max_abs = 0.0
+    max_mean_abs = 0.0
+    for sample in load_samples(args.calibration_dir, args.limit, args.flat_dim):
+        out_flat = flat.run(None, {flat_input: sample})[0]
+        out_wrapped = wrapped.run(None, {wrapped_input: sample.reshape(1, 1, 1, args.flat_dim)})[0]
+        diff = np.abs(out_flat - out_wrapped)
+        max_abs = max(max_abs, float(diff.max()))
+        max_mean_abs = max(max_mean_abs, float(diff.mean()))
+
+    print(f"checked={min(args.limit, len(list(args.calibration_dir.glob('*.bin'))))}")
+    print(f"max_abs_diff={max_abs:.9g}")
+    print(f"max_mean_abs_diff={max_mean_abs:.9g}")
+
+
+if __name__ == "__main__":
+    main()

deploy_45dim_rl_gym/bpu_quantization/convert_opset.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python3
+"""Convert policy ONNX to an older opset accepted by X5 hb_mapper."""
+
+import argparse
+from pathlib import Path
+
+import onnx
+from onnx import version_converter
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--input", type=Path, required=True)
+    parser.add_argument("--output", type=Path, required=True)
+    parser.add_argument("--opset", type=int, default=11)
+    parser.add_argument("--ir-version", type=int, default=7)
+    args = parser.parse_args()
+
+    model = onnx.load(str(args.input))
+    converted = version_converter.convert_version(model, args.opset)
+    converted.ir_version = min(converted.ir_version, args.ir_version)
+    args.output.parent.mkdir(parents=True, exist_ok=True)
+    onnx.save(converted, str(args.output))
+    print(f"Wrote {args.output}")
+    print(f"ir={converted.ir_version}")
+    print(f"opsets={[(op.domain, op.version) for op in converted.opset_import]}")
+
+
+if __name__ == "__main__":
+    main()

deploy_45dim_rl_gym/bpu_quantization/downgrade_policy_to_opset11.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python3
+"""Downgrade the current RobotLab policy ONNX from opset18 to opset11.
+
+This is intentionally narrow: it only rewrites ops whose axes changed from an
+attribute in opset11 to an input tensor in newer opsets. The policy graph uses
+otherwise opset11-compatible operators.
+"""
+
+import argparse
+from pathlib import Path
+
+import numpy as np
+import onnx
+from onnx import helper, numpy_helper
+
+
+AXES_INPUT_OPS = {"Unsqueeze", "Squeeze", "ReduceL2", "ReduceSum"}
+REMOVE_ATTRS = {
+    "Reshape": {"allowzero"},
+}
+
+
+def constant_values(model):
+    values = {}
+    for initializer in model.graph.initializer:
+        values[initializer.name] = numpy_helper.to_array(initializer)
+
+    for node in model.graph.node:
+        if node.op_type != "Constant" or len(node.output) != 1:
+            continue
+        for attr in node.attribute:
+            if attr.name == "value":
+                values[node.output[0]] = numpy_helper.to_array(attr.t)
+                break
+    return values
+
+
+def axes_from_value(values, name):
+    if name not in values:
+        raise KeyError(f"Cannot find constant axes tensor {name!r}")
+    axes = np.asarray(values[name]).astype(np.int64).reshape(-1).tolist()
+    return [int(axis) for axis in axes]
+
+
+def set_opset(model, target):
+    for opset in model.opset_import:
+        if opset.domain in ("", "ai.onnx"):
+            opset.version = target
+            return
+    model.opset_import.append(helper.make_operatorsetid("", target))
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--input", type=Path, required=True)
+    parser.add_argument("--output", type=Path, required=True)
+    parser.add_argument("--target-opset", type=int, default=11)
+    parser.add_argument("--ir-version", type=int, default=7)
+    args = parser.parse_args()
+
+    model = onnx.load(str(args.input))
+    values = constant_values(model)
+    converted = 0
+
+    for node in model.graph.node:
+        if node.op_type in REMOVE_ATTRS:
+            remove_names = REMOVE_ATTRS[node.op_type]
+            kept_attrs = [attr for attr in node.attribute if attr.name not in remove_names]
+            del node.attribute[:]
+            node.attribute.extend(kept_attrs)
+
+        if node.op_type not in AXES_INPUT_OPS or len(node.input) < 2:
+            continue
+        axes = axes_from_value(values, node.input[1])
+        del node.input[1:]
+        node.attribute.extend([helper.make_attribute("axes", axes)])
+        converted += 1
+
+    set_opset(model, args.target_opset)
+    model.ir_version = min(model.ir_version, args.ir_version)
+
+    args.output.parent.mkdir(parents=True, exist_ok=True)
+    onnx.save(model, str(args.output))
+    print(f"Wrote {args.output}")
+    print(f"converted_axes_nodes={converted}")
+    print(f"ir={model.ir_version}")
+    print(f"opsets={[(op.domain, op.version) for op in model.opset_import]}")
+
+
+if __name__ == "__main__":
+    main()

deploy_45dim_rl_gym/bpu_quantization/keep_actions_output.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+"""Keep only the actions output in a policy ONNX graph."""
+
+import argparse
+from pathlib import Path
+
+import onnx
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--input", type=Path, required=True)
+    parser.add_argument("--output", type=Path, required=True)
+    parser.add_argument("--output-name", default="actions")
+    args = parser.parse_args()
+
+    model = onnx.load(str(args.input))
+    outputs = list(model.graph.output)
+    if not outputs:
+        raise ValueError("ONNX graph has no outputs")
+
+    selected = None
+    for output in outputs:
+        if output.name == args.output_name:
+            selected = output
+            break
+    if selected is None:
+        selected = outputs[0]
+        print(f"[WARN] output {args.output_name!r} not found; keeping first output {selected.name!r}")
+
+    del model.graph.output[:]
+    model.graph.output.append(selected)
+
+    args.output.parent.mkdir(parents=True, exist_ok=True)
+    onnx.save(model, str(args.output))
+    print(f"Wrote actions-only ONNX: {args.output}")
+
+
+if __name__ == "__main__":
+    main()

deploy_45dim_rl_gym/bpu_quantization/make_bpu_4d_onnx.py

@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+"""Wrap a flat policy ONNX input as a fixed 4D feature-map input for X5 hb_mapper."""
+
+import argparse
+from pathlib import Path
+
+import numpy as np
+import onnx
+from onnx import TensorProto, helper, numpy_helper
+
+
+def tensor_shape(value_info):
+    return [dim.dim_value for dim in value_info.type.tensor_type.shape.dim]
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--input", type=Path, required=True)
+    parser.add_argument("--output", type=Path, required=True)
+    parser.add_argument("--flat-dim", type=int, default=450)
+    parser.add_argument("--input-name", default="obs_4d")
+    args = parser.parse_args()
+
+    model = onnx.load(str(args.input))
+    if len(model.graph.input) != 1:
+        raise ValueError(f"Expected one input, got {len(model.graph.input)}")
+
+    old_input = model.graph.input[0]
+    old_name = old_input.name
+    old_shape = tensor_shape(old_input)
+    if old_shape != [1, args.flat_dim]:
+        raise ValueError(f"Expected input shape [1, {args.flat_dim}], got {old_shape}")
+
+    reshape_out_name = "__bpu_flat_input"
+    flatten_node = helper.make_node(
+        "Flatten",
+        inputs=[args.input_name],
+        outputs=[reshape_out_name],
+        name="Bpu4dInputFlatten",
+        axis=1,
+    )
+
+    for node in model.graph.node:
+        for index, name in enumerate(node.input):
+            if name == old_name:
+                node.input[index] = reshape_out_name
+
+    new_input = helper.make_tensor_value_info(
+        args.input_name, TensorProto.FLOAT, [1, 1, 1, args.flat_dim]
+    )
+    model.graph.input.remove(old_input)
+    model.graph.input.insert(0, new_input)
+    model.graph.node.insert(0, flatten_node)
+
+    args.output.parent.mkdir(parents=True, exist_ok=True)
+    onnx.save(model, str(args.output))
+    print(f"Wrote {args.output}")
+
+
+if __name__ == "__main__":
+    main()

deploy_45dim_rl_gym/bpu_quantization/make_calibration_data.py

@@ -0,0 +1,169 @@
+#!/usr/bin/env python3
+"""Build float32 BPU calibration inputs from recorded deployment logs."""
+
+import argparse
+import json
+import random
+from collections import deque
+from pathlib import Path
+
+import numpy as np
+
+
+NUM_OBS = 45
+TERM_DIMS = (3, 3, 3, 12, 12, 12)
+HERE = Path(__file__).resolve().parent
+DEFAULT_LOG_ROOT = HERE.parents[1] / "logs"
+
+
+def build_policy_input(history, history_len):
+    frames = list(history)
+    while len(frames) < history_len:
+        frames.insert(0, np.zeros(NUM_OBS, dtype=np.float32))
+
+    chunks = []
+    offset = 0
+    for dim in TERM_DIMS:
+        for frame in frames:
+            chunks.append(frame[offset:offset + dim])
+        offset += dim
+    return np.concatenate(chunks, dtype=np.float32)
+
+
+def reservoir_add(samples, value, seen, max_samples, rng):
+    if len(samples) < max_samples:
+        samples.append(value)
+        return
+    replace_index = rng.randrange(seen)
+    if replace_index < max_samples:
+        samples[replace_index] = value
+
+
+def collect_samples(log_paths, max_samples, seed, history_len):
+    rng = random.Random(seed)
+    samples = []
+    seen = 0
+    usable_runs = []
+
+    for steps_path in log_paths:
+        history = deque(maxlen=history_len)
+        run_seen = 0
+        was_rl = False
+
+        with steps_path.open("r", encoding="utf-8") as fp:
+            for line in fp:
+                try:
+                    record = json.loads(line)
+                except json.JSONDecodeError:
+                    continue
+
+                if record.get("mode") != "RL":
+                    if was_rl:
+                        history.clear()
+                    was_rl = False
+                    continue
+
+                was_rl = True
+                obs = np.asarray(record.get("obs_single"), dtype=np.float32)
+                if obs.shape != (NUM_OBS,) or not np.all(np.isfinite(obs)):
+                    continue
+
+                history.append(obs)
+                policy_input = build_policy_input(history, history_len)
+                if not np.all(np.isfinite(policy_input)):
+                    continue
+
+                seen += 1
+                run_seen += 1
+                reservoir_add(samples, policy_input, seen, max_samples, rng)
+
+        if run_seen:
+            usable_runs.append({"steps": str(steps_path), "samples": run_seen})
+
+    return samples, seen, usable_runs
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description="Create BPU float32 calibration .bin files from JSONL deployment logs."
+    )
+    parser.add_argument(
+        "--logs-root",
+        type=Path,
+        default=DEFAULT_LOG_ROOT,
+        help="Directory containing <log-prefix>_*/steps.jsonl.",
+    )
+    parser.add_argument("--log-prefix", default="robotlab_go1_deploy",
+                        help="Run directory prefix below --logs-root")
+    parser.add_argument("--history-len", type=int, default=10,
+                        help="Number of 45-dim observations to stack by term")
+    parser.add_argument(
+        "--output-dir",
+        type=Path,
+        default=HERE / "calibration_data",
+        help="Output directory for raw float32 feature-map .bin files.",
+    )
+    parser.add_argument("--max-samples", type=int, default=512)
+    parser.add_argument("--min-samples", type=int, default=32)
+    parser.add_argument("--seed", type=int, default=20260727)
+    parser.add_argument("--overwrite", action="store_true")
+    args = parser.parse_args()
+
+    if args.max_samples < 1:
+        raise ValueError("--max-samples must be positive")
+    if args.history_len < 1:
+        raise ValueError("--history-len must be positive")
+
+    log_paths = sorted(args.logs_root.glob(f"{args.log_prefix}_*/steps.jsonl"))
+    if not log_paths:
+        raise FileNotFoundError(
+            f"No step logs found for prefix {args.log_prefix!r} below {args.logs_root}"
+        )
+
+    samples, total_seen, usable_runs = collect_samples(
+        log_paths, args.max_samples, args.seed, args.history_len
+    )
+    if len(samples) < args.min_samples:
+        raise RuntimeError(
+            f"Only {len(samples)} valid RL inputs found; need at least {args.min_samples} calibration samples."
+        )
+
+    output_dir = args.output_dir.resolve()
+    existing = list(output_dir.glob("*.bin")) if output_dir.exists() else []
+    if existing and not args.overwrite:
+        raise FileExistsError(
+            f"{output_dir} already contains calibration files; pass --overwrite to replace them."
+        )
+    output_dir.mkdir(parents=True, exist_ok=True)
+    for path in existing:
+        path.unlink()
+
+    for index, sample in enumerate(samples):
+        sample.astype(np.float32, copy=False).tofile(output_dir / f"{index:05d}.bin")
+
+    policy_input_dim = NUM_OBS * args.history_len
+    metadata = {
+        "format": "raw float32 feature-map",
+        "flat_shape": [1, policy_input_dim],
+        "mapper_shape": [1, 1, 1, policy_input_dim],
+        "history_len": args.history_len,
+        "num_obs": NUM_OBS,
+        "term_dims": list(TERM_DIMS),
+        "log_prefix": args.log_prefix,
+        "selected_samples": len(samples),
+        "candidate_rl_inputs": total_seen,
+        "seed": args.seed,
+        "runs": usable_runs,
+    }
+    (output_dir.parent / f"{output_dir.name}_metadata.json").write_text(
+        json.dumps(metadata, indent=2, ensure_ascii=True) + "\n",
+        encoding="utf-8",
+    )
+    print(
+        f"Wrote {len(samples)} calibration inputs from {total_seen} RL observations "
+        f"to {output_dir}"
+    )
+
+
+if __name__ == "__main__":
+    main()

deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_int16.yaml

@@ -0,0 +1,27 @@
+model_parameters:
+  onnx_model: "./policy_robotlab_15000_bpu4d.onnx"
+  march: "bayes-e"
+  layer_out_dump: false
+  working_dir: "mapper_output"
+  output_model_file_prefix: "policy_robotlab_15000_int16"
+
+input_parameters:
+  input_name: "obs_4d"
+  input_shape: "1x1x1x450"
+  input_type_rt: "featuremap"
+  input_layout_rt: "NCHW"
+  input_type_train: "featuremap"
+  input_layout_train: "NCHW"
+  norm_type: "no_preprocess"
+
+calibration_parameters:
+  cal_data_dir: "./calibration_data"
+  cal_data_type: "float32"
+  calibration_type: "default"
+  optimization: "set_all_nodes_int16"
+  per_channel: true
+
+compiler_parameters:
+  compile_mode: "latency"
+  debug: false
+  optimize_level: "O3"

deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_int16_gemm.yaml

@@ -0,0 +1,27 @@
+model_parameters:
+  onnx_model: "./policy_robotlab_15000_bpu4d_gemm.onnx"
+  march: "bayes-e"
+  layer_out_dump: false
+  working_dir: "mapper_output_gemm"
+  output_model_file_prefix: "policy_robotlab_15000_int16_gemm"
+
+input_parameters:
+  input_name: "obs_4d"
+  input_shape: "1x1x1x450"
+  input_type_rt: "featuremap"
+  input_layout_rt: "NCHW"
+  input_type_train: "featuremap"
+  input_layout_train: "NCHW"
+  norm_type: "no_preprocess"
+
+calibration_parameters:
+  cal_data_dir: "./calibration_data"
+  cal_data_type: "float32"
+  calibration_type: "default"
+  optimization: "set_all_nodes_int16"
+  per_channel: true
+
+compiler_parameters:
+  compile_mode: "latency"
+  debug: false
+  optimize_level: "O3"

deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_26000_int16_gemm.yaml

@@ -0,0 +1,27 @@
+model_parameters:
+  onnx_model: "./policy_robotlab_26000_bpu4d_gemm.onnx"
+  march: "bayes-e"
+  layer_out_dump: false
+  working_dir: "mapper_output_26000_gemm"
+  output_model_file_prefix: "policy_robotlab_26000_int16_gemm"
+
+input_parameters:
+  input_name: "obs_4d"
+  input_shape: "1x1x1x450"
+  input_type_rt: "featuremap"
+  input_layout_rt: "NCHW"
+  input_type_train: "featuremap"
+  input_layout_train: "NCHW"
+  norm_type: "no_preprocess"
+
+calibration_parameters:
+  cal_data_dir: "./calibration_data_26000_robotlab_fast64"
+  cal_data_type: "float32"
+  calibration_type: "default"
+  optimization: "set_all_nodes_int16"
+  per_channel: true
+
+compiler_parameters:
+  compile_mode: "latency"
+  debug: false
+  optimize_level: "O3"

deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_26000_s100_int16_gemm.yaml

@@ -0,0 +1,33 @@
+model_parameters:
+  onnx_model: "./policy_robotlab_26000_bpu4d_gemm.onnx"
+  march: "nash-e"
+  layer_out_dump: false
+  working_dir: "mapper_output_26000_s100_gemm"
+  output_model_file_prefix: "policy_robotlab_26000_s100_int16_gemm"
+
+input_parameters:
+  input_name: "obs_4d"
+  input_shape: "1x1x1x450"
+  input_type_rt: "featuremap"
+  input_type_train: "featuremap"
+  input_layout_train: "NCHW"
+  norm_type: "no_preprocess"
+  separate_batch: false
+
+calibration_parameters:
+  cal_data_dir: "./calibration_data_26000_robotlab_fast64"
+  cal_data_type: "float32"
+  calibration_type: "max"
+  quant_config:
+    model_config:
+      all_node_type: int16
+      activation:
+        calibration_type: max
+  per_channel: true
+
+compiler_parameters:
+  compile_mode: "latency"
+  optimize_level: "O2"
+  core_num: 1
+  jobs: 8
+  cache_mode: "disable"

deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_6500_int16_gemm.yaml

@@ -0,0 +1,27 @@
+model_parameters:
+  onnx_model: "./policy_robotlab_6500_bpu4d_gemm.onnx"
+  march: "bayes-e"
+  layer_out_dump: false
+  working_dir: "mapper_output_6500_gemm"
+  output_model_file_prefix: "policy_robotlab_6500_int16_gemm"
+
+input_parameters:
+  input_name: "obs_4d"
+  input_shape: "1x1x1x450"
+  input_type_rt: "featuremap"
+  input_layout_rt: "NCHW"
+  input_type_train: "featuremap"
+  input_layout_train: "NCHW"
+  norm_type: "no_preprocess"
+
+calibration_parameters:
+  cal_data_dir: "./calibration_data_fast64"
+  cal_data_type: "float32"
+  calibration_type: "default"
+  optimization: "set_all_nodes_int16"
+  per_channel: true
+
+compiler_parameters:
+  compile_mode: "latency"
+  debug: false
+  optimize_level: "O3"

deploy_45dim_rl_gym/bpu_quantization/quantize_policy_s100.sh

@@ -0,0 +1,229 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
+
+POLICY="../policy_robotlab_26000.onnx"
+ROUND="26000"
+NAME=""
+HISTORY_LEN=10
+FLAT_DIM=""
+SAMPLES=64
+MIN_SAMPLES=32
+LOG_PREFIX="robotlab_go1_deploy"
+CAL_TAG="robotlab"
+DOCKER_IMAGE="registry.d-robotics.cc/deliver/ai_toolchain_ubuntu_22_s100_s600_cpu:v3.7.0"
+MARCH="nash-e"
+COMPARE_LIMIT=64
+RUN_CHECKER=1
+QUANT="int16"
+
+usage() {
+  cat <<'EOF'
+Usage:
+  ./quantize_policy_s100.sh [options]
+
+Default: quantize RobotLab policy_robotlab_26000.onnx as 10-frame/450-dim
+S100 int16 Gemm BPU model.
+
+Options:
+  --policy PATH          ONNX policy path, relative to this directory or absolute
+  --round NAME           round label used in output paths, e.g. 15k/25k/30k/35k
+  --name NAME            model basename; default is policy filename without .onnx
+  --history-len N        observation history length; Gym=5, RobotLab=10
+  --flat-dim N           flat input dim; default 45 * history-len
+  --samples N            calibration sample count; default 64 for faster mapping
+  --min-samples N        minimum valid samples required; default 32
+  --log-prefix PREFIX    log dir prefix below logs/, default rlgym_go1_deploy
+  --cal-tag TAG          calibration dir tag, default gym
+  --docker-image IMAGE   D-Robotics S100/S600 CPU toolchain image
+  --march MARCH          S100 march, default nash-e
+  --compare-limit N      float ONNX equivalence sample count, default 64
+  --quant int16|int8     int16 uses all_node_type int16; int8 uses default S100 PTQ
+  --skip-checker         accepted for parity with X5 script; hb_compile path ignores it
+EOF
+}
+
+while [[ $# -gt 0 ]]; do
+  case "$1" in
+    --policy) POLICY="$2"; shift 2 ;;
+    --round) ROUND="$2"; shift 2 ;;
+    --name) NAME="$2"; shift 2 ;;
+    --history-len) HISTORY_LEN="$2"; shift 2 ;;
+    --flat-dim) FLAT_DIM="$2"; shift 2 ;;
+    --samples) SAMPLES="$2"; shift 2 ;;
+    --min-samples) MIN_SAMPLES="$2"; shift 2 ;;
+    --log-prefix) LOG_PREFIX="$2"; shift 2 ;;
+    --cal-tag) CAL_TAG="$2"; shift 2 ;;
+    --docker-image) DOCKER_IMAGE="$2"; shift 2 ;;
+    --march) MARCH="$2"; shift 2 ;;
+    --compare-limit) COMPARE_LIMIT="$2"; shift 2 ;;
+    --quant) QUANT="$2"; shift 2 ;;
+    --skip-checker) RUN_CHECKER=0; shift ;;
+    -h|--help) usage; exit 0 ;;
+    *) echo "Unknown argument: $1" >&2; usage >&2; exit 2 ;;
+  esac
+done
+
+if [[ -z "${FLAT_DIM}" ]]; then
+  FLAT_DIM=$((45 * HISTORY_LEN))
+fi
+
+case "${QUANT}" in
+  int16|int8) ;;
+  *) echo "--quant must be int16 or int8, got: ${QUANT}" >&2; exit 2 ;;
+esac
+
+if [[ "${POLICY}" = /* ]]; then
+  POLICY_ABS="${POLICY}"
+else
+  POLICY_ABS="${SCRIPT_DIR}/${POLICY}"
+fi
+POLICY_ABS="$(cd "$(dirname "${POLICY_ABS}")" && pwd)/$(basename "${POLICY_ABS}")"
+
+if [[ ! -f "${POLICY_ABS}" ]]; then
+  echo "Policy not found: ${POLICY_ABS}" >&2
+  exit 2
+fi
+case "${POLICY_ABS}" in
+  "${REPO_ROOT}"/*) POLICY_REL="${POLICY_ABS#${REPO_ROOT}/}" ;;
+  *) echo "Policy must be inside repo root ${REPO_ROOT}: ${POLICY_ABS}" >&2; exit 2 ;;
+esac
+
+if [[ -z "${NAME}" ]]; then
+  NAME="$(basename "${POLICY_ABS}" .onnx)"
+fi
+
+CAL_DIR="calibration_data_${ROUND}_${CAL_TAG}_fast${SAMPLES}"
+if [[ "${QUANT}" = "int16" ]]; then
+  OUTPUT_DIR="mapper_output_${ROUND}_s100_gemm"
+  OUTPUT_PREFIX="${NAME}_s100_int16_gemm"
+else
+  OUTPUT_DIR="mapper_output_${ROUND}_s100_int8_gemm"
+  OUTPUT_PREFIX="${NAME}_s100_int8_gemm"
+fi
+YAML_FILE="${OUTPUT_PREFIX}.yaml"
+
+echo "[INFO] repo          : ${REPO_ROOT}"
+echo "[INFO] policy        : ${POLICY_REL}"
+echo "[INFO] name/round    : ${NAME} / ${ROUND}"
+echo "[INFO] history/shape : ${HISTORY_LEN} / 1x1x1x${FLAT_DIM}"
+echo "[INFO] calibration   : ${CAL_DIR} (${SAMPLES} samples, prefix ${LOG_PREFIX})"
+echo "[INFO] output        : ${OUTPUT_DIR}/${OUTPUT_PREFIX}.hbm"
+echo "[INFO] quant         : ${QUANT}"
+echo "[INFO] march         : ${MARCH}"
+echo "[INFO] docker image  : ${DOCKER_IMAGE}"
+
+docker run --rm --platform linux/amd64 \
+  -e POLICY_REL="${POLICY_REL}" \
+  -e NAME="${NAME}" \
+  -e HISTORY_LEN="${HISTORY_LEN}" \
+  -e FLAT_DIM="${FLAT_DIM}" \
+  -e SAMPLES="${SAMPLES}" \
+  -e MIN_SAMPLES="${MIN_SAMPLES}" \
+  -e LOG_PREFIX="${LOG_PREFIX}" \
+  -e CAL_DIR="${CAL_DIR}" \
+  -e OUTPUT_DIR="${OUTPUT_DIR}" \
+  -e OUTPUT_PREFIX="${OUTPUT_PREFIX}" \
+  -e YAML_FILE="${YAML_FILE}" \
+  -e COMPARE_LIMIT="${COMPARE_LIMIT}" \
+  -e RUN_CHECKER="${RUN_CHECKER}" \
+  -e QUANT="${QUANT}" \
+  -e MARCH="${MARCH}" \
+  -v "${REPO_ROOT}:/workspace/deploy_go1_pro" \
+  "${DOCKER_IMAGE}" \
+  bash -lc '
+    set -euo pipefail
+    cd /workspace/deploy_go1_pro/deploy_45dim_rl_gym/bpu_quantization
+
+    POLICY="/workspace/deploy_go1_pro/${POLICY_REL}"
+    ACTIONS_ONNX="${NAME}_actions.onnx"
+    OPSET_ONNX="${NAME}_opset11.onnx"
+    BPU4D_ONNX="${NAME}_bpu4d.onnx"
+    GEMM_ONNX="${NAME}_bpu4d_gemm.onnx"
+
+    python3 make_calibration_data.py \
+      --logs-root ../../logs \
+      --log-prefix "${LOG_PREFIX}" \
+      --history-len "${HISTORY_LEN}" \
+      --output-dir "${CAL_DIR}" \
+      --max-samples "${SAMPLES}" \
+      --min-samples "${MIN_SAMPLES}" \
+      --overwrite
+
+    python3 keep_actions_output.py \
+      --input "${POLICY}" \
+      --output "${ACTIONS_ONNX}"
+
+    python3 downgrade_policy_to_opset11.py \
+      --input "${ACTIONS_ONNX}" \
+      --output "${OPSET_ONNX}"
+
+    python3 make_bpu_4d_onnx.py \
+      --input "${OPSET_ONNX}" \
+      --output "${BPU4D_ONNX}" \
+      --flat-dim "${FLAT_DIM}"
+
+    python3 replace_group_conv_with_gemm.py \
+      --input "${BPU4D_ONNX}" \
+      --output "${GEMM_ONNX}"
+
+    python3 compare_4d_onnx.py \
+      --flat-onnx "${ACTIONS_ONNX}" \
+      --bpu4d-onnx "${GEMM_ONNX}" \
+      --calibration-dir "${CAL_DIR}" \
+      --flat-dim "${FLAT_DIM}" \
+      --limit "${COMPARE_LIMIT}"
+
+    if [[ "${QUANT}" = "int16" ]]; then
+      QUANT_CONFIG=$(cat <<EOF
+  quant_config:
+    model_config:
+      all_node_type: int16
+      activation:
+        calibration_type: max
+EOF
+)
+    else
+      QUANT_CONFIG=""
+    fi
+
+    cat > "${YAML_FILE}" <<YAML
+model_parameters:
+  onnx_model: "./${GEMM_ONNX}"
+  march: "${MARCH}"
+  layer_out_dump: false
+  working_dir: "${OUTPUT_DIR}"
+  output_model_file_prefix: "${OUTPUT_PREFIX}"
+
+input_parameters:
+  input_name: "obs_4d"
+  input_shape: "1x1x1x${FLAT_DIM}"
+  input_type_rt: "featuremap"
+  input_type_train: "featuremap"
+  input_layout_train: "NCHW"
+  norm_type: "no_preprocess"
+  separate_batch: false
+
+calibration_parameters:
+  cal_data_dir: "./${CAL_DIR}"
+  cal_data_type: "float32"
+  calibration_type: "max"
+${QUANT_CONFIG}
+  per_channel: true
+
+compiler_parameters:
+  compile_mode: "latency"
+  optimize_level: "O2"
+  core_num: 1
+  jobs: 8
+  cache_mode: "disable"
+YAML
+
+    hb_compile -c "${YAML_FILE}"
+
+    find "${OUTPUT_DIR}" -maxdepth 1 -type f \( -name "${OUTPUT_PREFIX}.hbm" -o -name "${OUTPUT_PREFIX}.bin" \) -print -exec ls -lh {} \;
+  '
+
+echo "[INFO] Done: deploy_45dim_rl_gym/bpu_quantization/${OUTPUT_DIR}/${OUTPUT_PREFIX}.hbm"

deploy_45dim_rl_gym/bpu_quantization/quantize_policy_x5.sh

@@ -0,0 +1,223 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
+
+POLICY="../policy_35k.onnx"
+ROUND="35k"
+NAME=""
+HISTORY_LEN=5
+FLAT_DIM=""
+SAMPLES=64
+MIN_SAMPLES=32
+LOG_PREFIX="rlgym_go1_deploy"
+CAL_TAG="gym"
+DOCKER_IMAGE="openexplorer/ai_toolchain_ubuntu_20_x5_cpu:v1.2.8"
+COMPARE_LIMIT=64
+RUN_CHECKER=1
+QUANT="int16"
+
+usage() {
+  cat <<'EOF'
+Usage:
+  ./quantize_policy_x5.sh [options]
+
+Default: quantize Gym policy_35k.onnx as 5-frame/225-dim int16 Gemm BPU model.
+
+Options:
+  --policy PATH          ONNX policy path, relative to this directory or absolute
+  --round NAME           round label used in output paths, e.g. 15k/25k/30k/35k
+  --name NAME            model basename; default is policy filename without .onnx
+  --history-len N        observation history length; Gym=5, RobotLab=10
+  --flat-dim N           flat input dim; default 45 * history-len
+  --samples N            calibration sample count; default 64 for faster mapping
+  --min-samples N        minimum valid samples required; default 32
+  --log-prefix PREFIX    log dir prefix below logs/, default rlgym_go1_deploy
+  --cal-tag TAG          calibration dir tag, default gym
+  --docker-image IMAGE   D-Robotics CPU toolchain image
+  --compare-limit N      float ONNX equivalence sample count, default 64
+  --quant int16|int8     int16 keeps current conservative path; int8 omits set_all_nodes_int16
+  --skip-checker         skip hb_mapper checker before makertbin
+EOF
+}
+
+while [[ $# -gt 0 ]]; do
+  case "$1" in
+    --policy) POLICY="$2"; shift 2 ;;
+    --round) ROUND="$2"; shift 2 ;;
+    --name) NAME="$2"; shift 2 ;;
+    --history-len) HISTORY_LEN="$2"; shift 2 ;;
+    --flat-dim) FLAT_DIM="$2"; shift 2 ;;
+    --samples) SAMPLES="$2"; shift 2 ;;
+    --min-samples) MIN_SAMPLES="$2"; shift 2 ;;
+    --log-prefix) LOG_PREFIX="$2"; shift 2 ;;
+    --cal-tag) CAL_TAG="$2"; shift 2 ;;
+    --docker-image) DOCKER_IMAGE="$2"; shift 2 ;;
+    --compare-limit) COMPARE_LIMIT="$2"; shift 2 ;;
+    --quant) QUANT="$2"; shift 2 ;;
+    --skip-checker) RUN_CHECKER=0; shift ;;
+    -h|--help) usage; exit 0 ;;
+    *) echo "Unknown argument: $1" >&2; usage >&2; exit 2 ;;
+  esac
+done
+
+if [[ -z "${FLAT_DIM}" ]]; then
+  FLAT_DIM=$((45 * HISTORY_LEN))
+fi
+
+case "${QUANT}" in
+  int16|int8) ;;
+  *) echo "--quant must be int16 or int8, got: ${QUANT}" >&2; exit 2 ;;
+esac
+
+if [[ "${POLICY}" = /* ]]; then
+  POLICY_ABS="${POLICY}"
+else
+  POLICY_ABS="${SCRIPT_DIR}/${POLICY}"
+fi
+POLICY_ABS="$(cd "$(dirname "${POLICY_ABS}")" && pwd)/$(basename "${POLICY_ABS}")"
+
+if [[ ! -f "${POLICY_ABS}" ]]; then
+  echo "Policy not found: ${POLICY_ABS}" >&2
+  exit 2
+fi
+case "${POLICY_ABS}" in
+  "${REPO_ROOT}"/*) POLICY_REL="${POLICY_ABS#${REPO_ROOT}/}" ;;
+  *) echo "Policy must be inside repo root ${REPO_ROOT}: ${POLICY_ABS}" >&2; exit 2 ;;
+esac
+
+if [[ -z "${NAME}" ]]; then
+  NAME="$(basename "${POLICY_ABS}" .onnx)"
+fi
+
+CAL_DIR="calibration_data_${ROUND}_${CAL_TAG}_fast${SAMPLES}"
+if [[ "${QUANT}" = "int16" ]]; then
+  OUTPUT_DIR="mapper_output_${ROUND}_gemm"
+  OUTPUT_PREFIX="${NAME}_int16_gemm"
+else
+  OUTPUT_DIR="mapper_output_${ROUND}_int8_gemm"
+  OUTPUT_PREFIX="${NAME}_int8_gemm"
+fi
+YAML_FILE="${OUTPUT_PREFIX}.yaml"
+
+echo "[INFO] repo          : ${REPO_ROOT}"
+echo "[INFO] policy        : ${POLICY_REL}"
+echo "[INFO] name/round    : ${NAME} / ${ROUND}"
+echo "[INFO] history/shape : ${HISTORY_LEN} / 1x1x1x${FLAT_DIM}"
+echo "[INFO] calibration   : ${CAL_DIR} (${SAMPLES} samples, prefix ${LOG_PREFIX})"
+echo "[INFO] output        : ${OUTPUT_DIR}/${OUTPUT_PREFIX}.bin"
+echo "[INFO] quant         : ${QUANT}"
+
+docker run --rm --platform linux/amd64 \
+  -e POLICY_REL="${POLICY_REL}" \
+  -e NAME="${NAME}" \
+  -e HISTORY_LEN="${HISTORY_LEN}" \
+  -e FLAT_DIM="${FLAT_DIM}" \
+  -e SAMPLES="${SAMPLES}" \
+  -e MIN_SAMPLES="${MIN_SAMPLES}" \
+  -e LOG_PREFIX="${LOG_PREFIX}" \
+  -e CAL_DIR="${CAL_DIR}" \
+  -e OUTPUT_DIR="${OUTPUT_DIR}" \
+  -e OUTPUT_PREFIX="${OUTPUT_PREFIX}" \
+  -e YAML_FILE="${YAML_FILE}" \
+  -e COMPARE_LIMIT="${COMPARE_LIMIT}" \
+  -e RUN_CHECKER="${RUN_CHECKER}" \
+  -e QUANT="${QUANT}" \
+  -v "${REPO_ROOT}:/workspace/deploy_go1_pro" \
+  "${DOCKER_IMAGE}" \
+  bash -lc '
+    set -euo pipefail
+    cd /workspace/deploy_go1_pro/deploy_45dim_rl_gym/bpu_quantization
+
+    POLICY="/workspace/deploy_go1_pro/${POLICY_REL}"
+    ACTIONS_ONNX="${NAME}_actions.onnx"
+    OPSET_ONNX="${NAME}_opset11.onnx"
+    BPU4D_ONNX="${NAME}_bpu4d.onnx"
+    GEMM_ONNX="${NAME}_bpu4d_gemm.onnx"
+
+    python3 make_calibration_data.py \
+      --logs-root ../../logs \
+      --log-prefix "${LOG_PREFIX}" \
+      --history-len "${HISTORY_LEN}" \
+      --output-dir "${CAL_DIR}" \
+      --max-samples "${SAMPLES}" \
+      --min-samples "${MIN_SAMPLES}" \
+      --overwrite
+
+    python3 keep_actions_output.py \
+      --input "${POLICY}" \
+      --output "${ACTIONS_ONNX}"
+
+    python3 downgrade_policy_to_opset11.py \
+      --input "${ACTIONS_ONNX}" \
+      --output "${OPSET_ONNX}"
+
+    python3 make_bpu_4d_onnx.py \
+      --input "${OPSET_ONNX}" \
+      --output "${BPU4D_ONNX}" \
+      --flat-dim "${FLAT_DIM}"
+
+    python3 replace_group_conv_with_gemm.py \
+      --input "${BPU4D_ONNX}" \
+      --output "${GEMM_ONNX}"
+
+    python3 compare_4d_onnx.py \
+      --flat-onnx "${ACTIONS_ONNX}" \
+      --bpu4d-onnx "${GEMM_ONNX}" \
+      --calibration-dir "${CAL_DIR}" \
+      --flat-dim "${FLAT_DIM}" \
+      --limit "${COMPARE_LIMIT}"
+
+    if [[ "${QUANT}" = "int16" ]]; then
+      OPTIMIZATION_LINE="  optimization: \"set_all_nodes_int16\""
+    else
+      OPTIMIZATION_LINE=""
+    fi
+
+    cat > "${YAML_FILE}" <<YAML
+model_parameters:
+  onnx_model: "./${GEMM_ONNX}"
+  march: "bayes-e"
+  layer_out_dump: false
+  working_dir: "${OUTPUT_DIR}"
+  output_model_file_prefix: "${OUTPUT_PREFIX}"
+
+input_parameters:
+  input_name: "obs_4d"
+  input_shape: "1x1x1x${FLAT_DIM}"
+  input_type_rt: "featuremap"
+  input_layout_rt: "NCHW"
+  input_type_train: "featuremap"
+  input_layout_train: "NCHW"
+  norm_type: "no_preprocess"
+
+calibration_parameters:
+  cal_data_dir: "./${CAL_DIR}"
+  cal_data_type: "float32"
+  calibration_type: "default"
+${OPTIMIZATION_LINE}
+  per_channel: true
+
+compiler_parameters:
+  compile_mode: "latency"
+  debug: false
+  optimize_level: "O3"
+YAML
+
+    if [[ "${RUN_CHECKER}" = "1" ]]; then
+      hb_mapper checker \
+        --model "${GEMM_ONNX}" \
+        --model-type onnx \
+        --march bayes-e \
+        --input-shape obs_4d "1x1x1x${FLAT_DIM}"
+    fi
+
+    hb_mapper makertbin \
+      --config "${YAML_FILE}" \
+      --model-type onnx
+
+    ls -lh "${OUTPUT_DIR}/${OUTPUT_PREFIX}.bin"
+  '
+
+echo "[INFO] Done: deploy_45dim_rl_gym/bpu_quantization/${OUTPUT_DIR}/${OUTPUT_PREFIX}.bin"

deploy_45dim_rl_gym/bpu_quantization/replace_group_conv_with_gemm.py

@@ -0,0 +1,159 @@
+#!/usr/bin/env python3
+"""Replace the policy's grouped 1x1 Conv expert layer with an equivalent Gemm.
+
+The RobotLab MoE expert layer is exported as:
+
+    [1, 2048] -> Unsqueeze -> grouped Conv1d(group=8, kernel=1)
+              -> Squeeze -> [1, 256]
+
+For this fixed shape, the grouped Conv is exactly a block-diagonal linear layer.
+Using Gemm lets hb_mapper keep the expert path on BPU instead of cutting through
+CPU float execution.
+"""
+
+import argparse
+from pathlib import Path
+
+import numpy as np
+import onnx
+from onnx import helper, numpy_helper
+
+
+CONV_NAME = "/student_moe_encoder/moe/experts/experts/Conv"
+
+
+def attr_value(node, name, default=None):
+    for attr in node.attribute:
+        if attr.name == name:
+            return helper.get_attribute_value(attr)
+    return default
+
+
+def initializer_map(model):
+    return {initializer.name: initializer for initializer in model.graph.initializer}
+
+
+def producer_map(model):
+    producers = {}
+    for node in model.graph.node:
+        for output in node.output:
+            producers[output] = node
+    return producers
+
+
+def replace_node(model):
+    initializers = initializer_map(model)
+    producers = producer_map(model)
+    nodes = list(model.graph.node)
+
+    conv = next((node for node in nodes if node.name == CONV_NAME), None)
+    if conv is None:
+        candidates = [
+            node for node in nodes
+            if node.op_type == "Conv"
+            and int(attr_value(node, "group", 1)) > 1
+            and attr_value(node, "kernel_shape") == [1]
+        ]
+        if len(candidates) == 1:
+            conv = candidates[0]
+            print(f"[WARN] {CONV_NAME!r} not found; using grouped Conv {conv.name!r}")
+    if conv is None:
+        raise ValueError(f"Cannot find unique grouped 1x1 Conv node; fixed name {CONV_NAME!r} not found")
+    if conv.op_type != "Conv":
+        raise ValueError(f"{CONV_NAME!r} is {conv.op_type}, expected Conv")
+
+    if attr_value(conv, "kernel_shape") != [1]:
+        raise ValueError("Only kernel_shape=[1] is supported")
+    if attr_value(conv, "strides") not in (None, [1]):
+        raise ValueError("Only stride=1 is supported")
+    if attr_value(conv, "pads") not in (None, [0, 0]):
+        raise ValueError("Only no padding is supported")
+
+    groups = int(attr_value(conv, "group", 1))
+    weight = numpy_helper.to_array(initializers[conv.input[1]])
+    bias = numpy_helper.to_array(initializers[conv.input[2]]) if len(conv.input) >= 3 else None
+    if weight.ndim != 3 or weight.shape[2] != 1:
+        raise ValueError(f"Expected Conv weight [out, in_per_group, 1], got {weight.shape}")
+
+    out_channels, in_per_group, _ = weight.shape
+    if out_channels % groups != 0:
+        raise ValueError(f"out_channels={out_channels} is not divisible by groups={groups}")
+    out_per_group = out_channels // groups
+    in_channels = in_per_group * groups
+
+    full_weight = np.zeros((out_channels, in_channels), dtype=np.float32)
+    flat_weight = weight[:, :, 0].astype(np.float32, copy=False)
+    for group in range(groups):
+        out_begin = group * out_per_group
+        out_end = out_begin + out_per_group
+        in_begin = group * in_per_group
+        in_end = in_begin + in_per_group
+        full_weight[out_begin:out_end, in_begin:in_end] = flat_weight[out_begin:out_end]
+
+    unsqueeze = producers.get(conv.input[0])
+    if unsqueeze is None or unsqueeze.op_type != "Unsqueeze":
+        raise ValueError("Expected Conv input to be produced by Unsqueeze")
+    squeeze_users = [node for node in nodes if conv.output[0] in node.input]
+    if len(squeeze_users) != 1 or squeeze_users[0].op_type != "Squeeze":
+        raise ValueError("Expected Conv output to feed exactly one Squeeze")
+    squeeze = squeeze_users[0]
+
+    gemm_weight_name = conv.input[1] + "_as_gemm"
+    model.graph.initializer.append(
+        numpy_helper.from_array(full_weight, name=gemm_weight_name)
+    )
+
+    gemm_inputs = [unsqueeze.input[0], gemm_weight_name]
+    if bias is not None:
+        gemm_inputs.append(conv.input[2])
+
+    gemm = helper.make_node(
+        "Gemm",
+        inputs=gemm_inputs,
+        outputs=[squeeze.output[0]],
+        name=CONV_NAME + "_as_Gemm",
+        alpha=1.0,
+        beta=1.0,
+        transB=1,
+    )
+
+    conv_index = nodes.index(conv)
+    remove_names = {unsqueeze.name, conv.name, squeeze.name}
+    new_nodes = []
+    inserted = False
+    for index, node in enumerate(nodes):
+        if node.name in remove_names:
+            if index >= conv_index and not inserted:
+                new_nodes.append(gemm)
+                inserted = True
+            continue
+        new_nodes.append(node)
+
+    del model.graph.node[:]
+    model.graph.node.extend(new_nodes)
+    return {
+        "groups": groups,
+        "conv_weight_shape": list(weight.shape),
+        "gemm_weight_shape": list(full_weight.shape),
+        "removed": sorted(remove_names),
+        "added": gemm.name,
+    }
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--input", type=Path, required=True)
+    parser.add_argument("--output", type=Path, required=True)
+    args = parser.parse_args()
+
+    model = onnx.load(str(args.input))
+    info = replace_node(model)
+    args.output.parent.mkdir(parents=True, exist_ok=True)
+    onnx.save(model, str(args.output))
+    print(f"Wrote {args.output}")
+    for key, value in info.items():
+        print(f"{key}={value}")
+
+
+if __name__ == "__main__":
+    main()

deploy_45dim_rl_gym/deploy_go1_onnx_mujoco_lab.py

@@ -0,0 +1,738 @@
+#!/usr/bin/env python3
+"""
+Go1 RobotLab ONNX Policy - MuJoCo Simulation Deployment.
+
+Loads the RobotLab ONNX policy exported from RoboGauge and runs it in MuJoCo
+with PD position control. The ONNX model uses a 10-frame history
+(450-dim stacked-by-terms input, stateless).
+
+Usage:
+    conda activate free_dog_sdk
+    MUJOCO_GL=glfw mjpython deploy_go1_onnx_mujoco_lab.py
+    MUJOCO_GL=glfw mjpython deploy_go1_onnx_mujoco_lab.py --terrain terrains/stairs/stairs_6.xml
+
+Controls (matching RoboGauge keyboard convention):
+    ↑ / ↓         forward / backward
+    ← / →         yaw left / right
+    , / .         strafe left / right
+    K             stop
+    R             reset robot
+    Esc           quit
+
+Architecture:
+    ONNX input:  obs [1, 450]  - 10 frames x 45 dims, stacked by TERMS
+    ONNX output: actions [1, 12]
+    Control:     target_q = default_q + 0.25 * action, PD with Kp=28, Kd=0.7
+"""
+
+import argparse
+import json
+import os
+import re
+import signal
+import sys
+import time
+from collections import deque
+from datetime import datetime
+from pathlib import Path
+
+import mujoco
+import numpy as np
+import onnxruntime as ort
+from mujoco import viewer
+
+# ── path setup ──
+SCRIPT_DIR = Path(__file__).resolve().parent
+DEFAULT_ONNX = str(SCRIPT_DIR / "policy_robotlab_26000.onnx")
+ROBOT_XML = str(SCRIPT_DIR / "go1.xml")
+TERRAINS_DIR = SCRIPT_DIR / "terrains"
+
+# ── policy constants (RoboGauge Go1Config) ──
+NUM_OBS = 45
+NUM_ACTIONS = 12
+HISTORY_LEN = 10         # RobotLab ONNX history frames
+ONNX_INPUT_DIM = 450     # 45 x 10, stacked by terms
+
+ACTION_SCALE = 0.25
+KP = 28.0
+KD = 0.7
+CLIP_ACTIONS = 100.0
+CLIP_OBS = 100.0
+
+ANG_VEL_SCALE = 0.25
+CMD_SCALE = np.array([1.0, 1.0, 1.0], dtype=np.float32)
+
+MAX_LIN_VEL_X = 1.0
+MAX_LIN_VEL_Y = 0.5
+MAX_ANG_VEL = 1.0
+
+DEFAULT_DOF_POS = np.array([
+    -0.1,  0.8, -1.5,   # FR_hip, FR_thigh, FR_calf
+     0.1,  0.8, -1.5,   # FL_hip, FL_thigh, FL_calf
+    -0.1,  1.0, -1.5,   # RR_hip, RR_thigh, RR_calf
+     0.1,  1.0, -1.5,   # RL_hip, RL_thigh, RL_calf
+], dtype=np.float32)
+
+# ── exit flag ──
+EXIT = False
+
+def _sig_handler(signum, frame):
+    global EXIT
+    EXIT = True
+
+signal.signal(signal.SIGINT, _sig_handler)
+signal.signal(signal.SIGTERM, _sig_handler)
+
+
+# ═══════════════════════════════════════════════════════════════
+# XML builder — merge terrain + robot into one MuJoCo scene
+# ═══════════════════════════════════════════════════════════════
+
+def _xml_inner(text, tag):
+    """Extract inner content of the first <tag>...</tag> in text."""
+    m = re.search(rf"<{tag}>(.*?)</{tag}>", text, re.DOTALL)
+    return m.group(1).strip() if m else ""
+
+
+def build_scene_xml(robot_xml_path, terrain_xml_path=None):
+    """Merge robot XML with optional terrain XML into a single scene model.
+
+    The robot XML (go1.xml) has base_link as the root body with no free joint.
+    We add a free joint and optionally merge terrain worldbody/asset elements.
+    """
+    robot = Path(robot_xml_path).read_text()
+
+    # 1) Add free joint to base_link
+    robot = robot.replace(
+        '<body name="base_link" pos="0 0 0.35">',
+        '<body name="base_link" pos="0 0 0.35">\n            <joint type="free"/>'
+    )
+
+    if terrain_xml_path is None:
+        # No terrain — add a simple flat floor
+        floor = (
+            '\n        <geom name="floor" size="0 0 0.05" type="plane" '
+            'rgba="0.5 0.9 0.9 0.1"/>\n'
+        )
+        robot = robot.replace(
+            '<body name="base_link"',
+            floor + '        <body name="base_link"'
+        )
+        return robot
+
+    terrain_text = Path(terrain_xml_path).read_text()
+
+    # 2) Merge terrain <asset> into robot <asset>
+    terrain_assets = _xml_inner(terrain_text, "asset")
+    if terrain_assets:
+        # Insert before closing </asset> of robot (or before <worldbody> if no asset)
+        robot = robot.replace('</asset>', '\n' + terrain_assets + '\n    </asset>', 1)
+
+    # 3) Merge terrain <visual> settings
+    terrain_visual = _xml_inner(terrain_text, "visual")
+    if terrain_visual:
+        robot = robot.replace('</visual>', '\n' + terrain_visual + '\n    </visual>', 1)
+
+    # 4) Merge terrain worldbody elements (lights, geoms, etc.) before base_link
+    terrain_wb = _xml_inner(terrain_text, "worldbody")
+    if terrain_wb:
+        # Remove <body> elements from terrain worldbody (we only want geoms/lights/cameras)
+        terrain_wb_no_bodies = re.sub(r'<body\b.*?</body>', '', terrain_wb, flags=re.DOTALL)
+        robot = robot.replace(
+            '<body name="base_link"',
+            terrain_wb_no_bodies.strip() + '\n        <body name="base_link"'
+        )
+
+    return robot
+
+
+# ═══════════════════════════════════════════════════════════════
+# Observation building
+# ═══════════════════════════════════════════════════════════════
+
+def get_projected_gravity(quat_wxyz):
+    qw, qx, qy, qz = quat_wxyz
+    g = np.zeros(3, dtype=np.float32)
+    g[0] = 2.0 * (-qz * qx + qw * qy)
+    g[1] = -2.0 * (qz * qy + qw * qx)
+    g[2] = 1.0 - 2.0 * (qw * qw + qz * qz)
+    return g
+
+
+def quat_rotate_inverse(q_wxyz, v):
+    """Rotate vector v from world frame into body frame using quaternion q."""
+    q_w = q_wxyz[0]
+    q_vec = np.array(q_wxyz[1:], dtype=np.float32)
+    v = np.array(v, dtype=np.float32)
+    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_to_rpy_deg(q_wxyz):
+    """Convert MuJoCo [w, x, y, z] quaternion to roll/pitch/yaw in degrees."""
+    qw, qx, qy, qz = [float(x) for x in q_wxyz]
+    sinr_cosp = 2.0 * (qw * qx + qy * qz)
+    cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy)
+    roll = np.arctan2(sinr_cosp, cosr_cosp)
+
+    sinp = 2.0 * (qw * qy - qz * qx)
+    pitch = np.arcsin(np.clip(sinp, -1.0, 1.0))
+
+    siny_cosp = 2.0 * (qw * qz + qx * qy)
+    cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz)
+    yaw = np.arctan2(siny_cosp, cosy_cosp)
+    return np.degrees(np.array([roll, pitch, yaw], dtype=np.float32))
+
+
+def read_sensor(model, data, name, expected_dim):
+    """Read a MuJoCo sensor by name, returning None if it is not present."""
+    sid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, name)
+    if sid < 0:
+        return None
+    adr = int(model.sensor_adr[sid])
+    dim = int(model.sensor_dim[sid])
+    if dim != expected_dim:
+        raise ValueError(f"sensor {name!r} has dim {dim}, expected {expected_dim}")
+    return np.asarray(data.sensordata[adr:adr + dim], dtype=np.float32).copy()
+
+
+class ObsBuilder:
+    """Build 45-dim single-frame obs and stack 10-frame history into 450-dim ONNX input.
+
+    ONNX expects observation terms stacked by groups (not by frames):
+        [ang_vel(t-9..t), gravity(t-9..t), cmd(t-9..t),
+         dof_pos(t-9..t), dof_vel(t-9..t), last_action(t-9..t)]
+    Within each group, frames go from oldest (t-9) to newest (t).
+    """
+
+    def __init__(self):
+        self.history: deque = deque(maxlen=HISTORY_LEN)
+
+    def reset(self):
+        self.history.clear()
+
+    def build_single_obs(self, base_ang_vel_body, base_quat_wxyz, cmd, q, dq, last_action):
+        """Build 45-dim single-frame observation (RoboGauge layout)."""
+        obs = np.zeros(NUM_OBS, dtype=np.float32)
+        obs[0:3] = np.asarray(base_ang_vel_body, dtype=np.float32) * ANG_VEL_SCALE
+        obs[3:6] = get_projected_gravity(np.asarray(base_quat_wxyz, dtype=np.float32))
+        obs[6:9] = np.asarray(cmd, dtype=np.float32) * CMD_SCALE
+        obs[9:21] = np.asarray(q, dtype=np.float32) - DEFAULT_DOF_POS
+        obs[21:33] = np.asarray(dq, dtype=np.float32) * 0.05
+        obs[33:45] = np.asarray(last_action, dtype=np.float32)
+        return np.clip(obs, -CLIP_OBS, CLIP_OBS)
+
+    def build_onnx_input(self, obs_single):
+        """Stack 10-frame history into 450-dim ONNX input (by-terms format)."""
+        self.history.append(obs_single.copy())
+
+        frames = list(self.history)
+        while len(frames) < HISTORY_LEN:
+            frames.insert(0, np.zeros(NUM_OBS, dtype=np.float32))
+
+        # term_dims: [ang_vel(3), gravity(3), cmd(3), dof_pos(12), dof_vel(12), last_action(12)]
+        term_dims = [3, 3, 3, 12, 12, 12]
+
+        stacked = []
+        offset = 0
+        for dim in term_dims:
+            for f_idx in range(HISTORY_LEN):
+                stacked.append(frames[f_idx][offset:offset + dim])
+            offset += dim
+
+        return np.concatenate(stacked, dtype=np.float32).reshape(1, -1)
+
+
+# ═══════════════════════════════════════════════════════════════
+# Keyboard input (pynput)
+# ═══════════════════════════════════════════════════════════════
+
+class KbReader:
+    """Non-blocking keyboard reader using a background pynput listener.
+
+    Key names match RoboGauge convention:
+        key.up / key.down  → forward/back
+        key.left / key.right → yaw
+        , / .              → strafe left/right
+        k                  → stop
+    """
+
+    def __init__(self):
+        import queue
+        import threading
+        self._q = queue.Queue()
+        self.held = set()
+        self._running = True
+        self._listener = None
+        self._thread = None
+
+    @staticmethod
+    def _name(key):
+        try:
+            if hasattr(key, 'char') and key.char is not None:
+                return key.char.lower()
+        except Exception:
+            pass
+        return str(key).lower()
+
+    def _worker(self):
+        while self._running:
+            try:
+                evt, key = self._q.get(timeout=0.05)
+            except Exception:
+                continue
+            name = self._name(key)
+            if evt == 'press':
+                self.held.add(name)
+            else:
+                self.held.discard(name)
+
+    def start(self):
+        from pynput import keyboard
+        self._listener = keyboard.Listener(
+            on_press=lambda k: self._q.put(('press', k)),
+            on_release=lambda k: self._q.put(('release', k)),
+        )
+        self._listener.start()
+        import threading
+        self._thread = threading.Thread(target=self._worker, daemon=True)
+        self._thread.start()
+        print("[INFO] keyboard started (↑↓←→ , . K R Esc)")
+
+    def is_held(self, key):
+        return self._name(key) in self.held
+
+    def snapshot(self):
+        return set(self.held)
+
+    def stop(self):
+        self._running = False
+        if self._listener:
+            self._listener.stop()
+
+
+def get_command(held):
+    """Extract velocity command from held key set.
+
+    Returns (vx, vy, yaw_rate).
+    """
+    if 'k' in held:
+        return 0.0, 0.0, 0.0
+
+    def axis(pos_key, neg_key, limit):
+        p = pos_key in held
+        n = neg_key in held
+        if p == n:
+            return 0.0
+        return limit if p else -limit
+
+    vx = axis('key.up', 'key.down', MAX_LIN_VEL_X)
+    vy = axis(',', '.', MAX_LIN_VEL_Y)
+    yaw = axis('key.left', 'key.right', MAX_ANG_VEL)
+    return vx, vy, yaw
+
+
+class SampleLogger:
+    def __init__(self, log_dir, terrain_path, cmd, args):
+        ts = datetime.now().strftime("%Y%m%d_%H%M%S")
+        self.run_dir = Path(log_dir).expanduser().resolve() / f"mujoco_stairs_sample_{ts}"
+        self.run_dir.mkdir(parents=True, exist_ok=True)
+        self.fp = open(self.run_dir / "steps.jsonl", "a", encoding="utf-8", buffering=1)
+        meta = {
+            "created_at": ts,
+            "terrain": str(terrain_path) if terrain_path else "flat",
+            "sample_seconds": args.sample_seconds,
+            "sample_cmd": list(cmd),
+            "spawn": [args.spawn_x, args.spawn_y, args.spawn_z],
+            "action_scale": ACTION_SCALE,
+            "clip_actions": args.clip_actions,
+            "max_target_step": args.max_target_step,
+            "kp": KP,
+            "kd": KD,
+            "control_dt": 0.02,
+            "sim_dt": 0.002,
+            "num_obs": NUM_OBS,
+            "history_len": HISTORY_LEN,
+            "onnx_input_dim": ONNX_INPUT_DIM,
+        }
+        (self.run_dir / "metadata.json").write_text(
+            json.dumps(meta, indent=2, ensure_ascii=False))
+        print(f"[INFO] sample log dir: {self.run_dir}")
+
+    def write(self, rec):
+        self.fp.write(json.dumps(rec, ensure_ascii=False) + "\n")
+
+    def close(self):
+        self.fp.flush()
+        self.fp.close()
+
+
+def summarize_samples(samples):
+    if not samples:
+        return {}
+
+    def max_abs_field(field):
+        return float(max(max(abs(x) for x in s[field]) for s in samples))
+
+    def p99_abs_field(field):
+        vals = sorted(abs(x) for s in samples for x in s[field])
+        if not vals:
+            return 0.0
+        idx = min(len(vals) - 1, int(0.99 * (len(vals) - 1)))
+        return float(vals[idx])
+
+    rpy = np.array([s["rpy_deg"] for s in samples], dtype=np.float32)
+    xyz = np.array([s["base_pos"] for s in samples], dtype=np.float32)
+    return {
+        "num_samples": len(samples),
+        "duration": float(samples[-1]["time_sim"] - samples[0]["time_sim"]),
+        "base_x_start": float(xyz[0, 0]),
+        "base_x_end": float(xyz[-1, 0]),
+        "base_x_progress": float(xyz[-1, 0] - xyz[0, 0]),
+        "base_z_min": float(np.min(xyz[:, 2])),
+        "base_z_max": float(np.max(xyz[:, 2])),
+        "rpy_min_deg": np.min(rpy, axis=0).astype(float).tolist(),
+        "rpy_max_deg": np.max(rpy, axis=0).astype(float).tolist(),
+        "action_raw_maxabs": max_abs_field("action_raw"),
+        "action_raw_p99abs": p99_abs_field("action_raw"),
+        "action_applied_maxabs": max_abs_field("action"),
+        "action_applied_p99abs": p99_abs_field("action"),
+        "target_offset_maxabs": max_abs_field("target_offset"),
+        "target_offset_p99abs": p99_abs_field("target_offset"),
+        "dof_vel_maxabs": max_abs_field("dof_vel"),
+        "torque_maxabs": max_abs_field("torques"),
+        "fallen": bool(any(s["fallen"] for s in samples)),
+    }
+
+
+# ═══════════════════════════════════════════════════════════════
+# Main
+# ═══════════════════════════════════════════════════════════════
+
+def main():
+    parser = argparse.ArgumentParser(description="Go1 RobotLab ONNX MuJoCo Deployment")
+    parser.add_argument("--onnx", type=str, default=DEFAULT_ONNX, help="ONNX model path")
+    parser.add_argument("--terrain", type=str, default=None,
+                        help="Terrain XML path (e.g. terrains/stairs/stairs_6.xml)")
+    parser.add_argument("--sample", action="store_true",
+                        help="Run a non-interactive fixed-command sampling rollout")
+    parser.add_argument("--sample-stairs-level", type=int, default=None,
+                        help="Shortcut for --sample --terrain terrains/stairs/stairs_LEVEL.xml")
+    parser.add_argument("--sample-seconds", type=float, default=20.0)
+    parser.add_argument("--sample-cmd-x", type=float, default=MAX_LIN_VEL_X)
+    parser.add_argument("--sample-cmd-y", type=float, default=0.0)
+    parser.add_argument("--sample-cmd-yaw", type=float, default=0.0)
+    parser.add_argument("--sample-log-dir", type=str, default=str(SCRIPT_DIR / "mujoco_logs"))
+    parser.add_argument("--spawn-x", type=float, default=None)
+    parser.add_argument("--spawn-y", type=float, default=None)
+    parser.add_argument("--spawn-z", type=float, default=None)
+    parser.add_argument("--clip-actions", type=float, default=CLIP_ACTIONS,
+                        help="Clip ONNX actions before applying action_scale")
+    parser.add_argument("--max-target-step", type=float, default=0.0,
+                        help="Optional per-control-step target q slew limit in rad")
+    args = parser.parse_args()
+
+    sample_mode = args.sample or args.sample_stairs_level is not None
+    if args.sample_stairs_level is not None:
+        args.sample = True
+        args.terrain = f"terrains/stairs/stairs_{args.sample_stairs_level}.xml"
+
+    if args.spawn_x is None:
+        args.spawn_x = -0.6 if args.sample_stairs_level is not None else 0.0
+    if args.spawn_y is None:
+        args.spawn_y = 0.0
+    if args.spawn_z is None:
+        args.spawn_z = 0.34
+    args.sample_log_dir = str(Path(args.sample_log_dir).expanduser().resolve())
+
+    # Resolve terrain path
+    terrain_path = None
+    if args.terrain:
+        terrain_path = Path(args.terrain)
+        if not terrain_path.is_absolute():
+            terrain_path = SCRIPT_DIR / terrain_path
+        if not terrain_path.exists():
+            print(f"[ERROR] terrain not found: {terrain_path}")
+            return 1
+
+    # ── Build scene XML ──
+    print(f"[INFO] robot: {ROBOT_XML}")
+    print(f"[INFO] terrain: {terrain_path or 'flat floor'}")
+    scene_xml = build_scene_xml(ROBOT_XML, str(terrain_path) if terrain_path else None)
+
+    # ── Load MuJoCo model ──
+    os.chdir(str(SCRIPT_DIR))  # meshdir="assets" is relative to go1.xml
+    model = mujoco.MjModel.from_xml_string(scene_xml)
+    data = mujoco.MjData(model)
+    model.opt.timestep = 0.002
+    print(f"[INFO] model: {model.nbody} bodies, qpos={model.nq}, actuators={model.nu}")
+    print(f"[INFO] timestep: {model.opt.timestep}s")
+    has_imu_gyro = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, "Body_Gyro") >= 0
+    has_imu_quat = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, "Body_Quat") >= 0
+    print(f"[INFO] IMU sensors: Body_Gyro={has_imu_gyro}, Body_Quat={has_imu_quat}")
+
+    # ── Load ONNX ──
+    session = ort.InferenceSession(args.onnx, providers=['CPUExecutionProvider'])
+    inp = session.get_inputs()[0]
+    print(f"[INFO] ONNX  input : {inp.name} {inp.shape}")
+    for o in session.get_outputs():
+        print(f"[INFO] ONNX  output: {o.name} {o.shape}")
+
+    # ── Init state ──
+    data.qpos[0:3] = np.array([args.spawn_x, args.spawn_y, args.spawn_z], dtype=np.float64)
+    data.qpos[3:7] = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64)
+    data.qpos[7:19] = DEFAULT_DOF_POS.astype(np.float64)
+    data.qvel[:] = 0.0
+    data.ctrl[:] = 0.0
+    mujoco.mj_forward(model, data)
+
+    # ── Control state ──
+    ctrl_dt = 0.02       # 50 Hz
+    sim_dt = model.opt.timestep
+    steps_per_inference = max(1, int(ctrl_dt / sim_dt))
+    print(f"[INFO] control: {ctrl_dt}s ({1/ctrl_dt:.0f}Hz), "
+          f"sim steps per inference: {steps_per_inference}")
+
+    last_action = np.zeros(NUM_ACTIONS, dtype=np.float32)
+    action_raw = np.zeros(NUM_ACTIONS, dtype=np.float32)
+    action = np.zeros(NUM_ACTIONS, dtype=np.float32)
+    target_pos = DEFAULT_DOF_POS.copy()
+    prev_target_pos = DEFAULT_DOF_POS.copy()
+    obs_builder = ObsBuilder()
+    step_count = 0
+    inference_step = 0
+
+    if sample_mode:
+        cmd = np.array([
+            args.sample_cmd_x,
+            args.sample_cmd_y,
+            args.sample_cmd_yaw,
+        ], dtype=np.float32)
+        cmd = np.clip(cmd, [-MAX_LIN_VEL_X, -MAX_LIN_VEL_Y, -MAX_ANG_VEL],
+                      [MAX_LIN_VEL_X, MAX_LIN_VEL_Y, MAX_ANG_VEL])
+        logger = SampleLogger(args.sample_log_dir, terrain_path, cmd, args)
+        samples = []
+        max_sim_steps = int(args.sample_seconds / sim_dt)
+        print(
+            f"[INFO] sampling: seconds={args.sample_seconds:.1f} "
+            f"cmd=({cmd[0]:.2f},{cmd[1]:.2f},{cmd[2]:.2f}) "
+            f"spawn=({args.spawn_x:.2f},{args.spawn_y:.2f},{args.spawn_z:.2f})"
+        )
+
+        while step_count < max_sim_steps and not EXIT:
+            if inference_step == 0:
+                imu_quat = read_sensor(model, data, "Body_Quat", 4)
+                imu_ang_vel = read_sensor(model, data, "Body_Gyro", 3)
+                quat_wxyz = imu_quat if imu_quat is not None else data.qpos[3:7].copy()
+                if imu_ang_vel is not None:
+                    base_ang_vel_body = imu_ang_vel
+                else:
+                    world_ang_vel = data.qvel[3:6].copy()
+                    base_ang_vel_body = quat_rotate_inverse(quat_wxyz, world_ang_vel)
+
+                q = data.qpos[7:19].copy()
+                dq = data.qvel[6:18].copy()
+                obs_single = obs_builder.build_single_obs(
+                    base_ang_vel_body, quat_wxyz, cmd, q, dq, last_action)
+                onnx_input = obs_builder.build_onnx_input(obs_single)
+                outputs = session.run(None, {'obs': onnx_input})
+                action_raw = outputs[0][0].astype(np.float32)
+                action = np.clip(action_raw, -args.clip_actions, args.clip_actions)
+                last_action = action.copy()
+
+                target_pos_raw = DEFAULT_DOF_POS + action * ACTION_SCALE
+                if args.max_target_step > 0.0:
+                    delta = np.clip(
+                        target_pos_raw - prev_target_pos,
+                        -args.max_target_step,
+                        args.max_target_step,
+                    )
+                    target_pos = prev_target_pos + delta
+                else:
+                    target_pos = target_pos_raw
+                prev_target_pos = target_pos.copy()
+
+            current_pos = data.qpos[7:19]
+            current_vel = data.qvel[6:18]
+            torques = KP * (target_pos - current_pos) - KD * current_vel
+            torques = np.clip(torques, -33.5, 33.5)
+            data.ctrl[:] = torques.astype(np.float64)
+
+            mujoco.mj_step(model, data)
+
+            if inference_step == 0:
+                quat_wxyz = data.qpos[3:7].copy()
+                rpy_deg = quat_to_rpy_deg(quat_wxyz)
+                fallen = bool(
+                    data.qpos[2] < 0.16
+                    or abs(rpy_deg[0]) > 60.0
+                    or abs(rpy_deg[1]) > 60.0
+                    or not np.all(np.isfinite(data.qpos))
+                )
+                rec = {
+                    "step": int(step_count),
+                    "control_step": int(step_count // steps_per_inference),
+                    "time_sim": float(data.time),
+                    "cmd": cmd.astype(float).tolist(),
+                    "base_pos": data.qpos[0:3].astype(float).tolist(),
+                    "base_quat": quat_wxyz.astype(float).tolist(),
+                    "rpy_deg": rpy_deg.astype(float).tolist(),
+                    "base_lin_vel": data.qvel[0:3].astype(float).tolist(),
+                    "base_ang_vel": data.qvel[3:6].astype(float).tolist(),
+                    "dof_pos": current_pos.astype(float).tolist(),
+                    "dof_vel": current_vel.astype(float).tolist(),
+                    "action_raw": action_raw.astype(float).tolist(),
+                    "action": action.astype(float).tolist(),
+                    "target_pos": target_pos.astype(float).tolist(),
+                    "target_offset": (target_pos - DEFAULT_DOF_POS).astype(float).tolist(),
+                    "torques": torques.astype(float).tolist(),
+                    "fallen": fallen,
+                }
+                logger.write(rec)
+                samples.append(rec)
+                if rec["control_step"] % 50 == 0:
+                    print(
+                        f"[sample {rec['control_step']:04d}] "
+                        f"t={rec['time_sim']:.2f} x={rec['base_pos'][0]:.2f} "
+                        f"z={rec['base_pos'][2]:.2f} rpy={np.round(rpy_deg, 1)} "
+                        f"act_max={np.max(np.abs(action)):.2f} "
+                        f"target_off={np.max(np.abs(target_pos - DEFAULT_DOF_POS)):.2f}"
+                    )
+                if fallen:
+                    print(f"[WARN] sample stopped: fallen at t={data.time:.2f}s")
+                    break
+
+            step_count += 1
+            inference_step = (inference_step + 1) % steps_per_inference
+
+        summary = summarize_samples(samples)
+        (logger.run_dir / "summary.json").write_text(
+            json.dumps(summary, indent=2, ensure_ascii=False))
+        logger.close()
+        print("[INFO] sample summary:")
+        print(json.dumps(summary, indent=2, ensure_ascii=False))
+        print(f"[INFO] sample saved: {logger.run_dir}")
+        return 0
+
+    # ── Keyboard ──
+    kb = KbReader()
+    kb.start()
+
+    # ── Viewer ──
+    view = viewer.launch_passive(model, data)
+    # Track the robot body
+    body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "base_link")
+    if body_id >= 0:
+        view.cam.type = mujoco.mjtCamera.mjCAMERA_TRACKING
+        view.cam.trackbodyid = body_id
+        view.cam.distance = 2.5
+        view.cam.elevation = -20
+        view.cam.azimuth = 60
+    print("[INFO] viewer launched")
+
+    loop_start = time.time()
+
+    while view.is_running() and not EXIT:
+        held = kb.snapshot()
+
+        # ── Quit ──
+        if 'key.esc' in held:
+            break
+
+        # ── Command from keyboard ──
+        vx, vy, yaw = get_command(held)
+        cmd = np.array([vx, vy, yaw], dtype=np.float32)
+
+        # ── Reset ──
+        if 'r' in held:
+            data.qpos[0:3] = np.array([0.0, 0.0, 0.34], dtype=np.float64)
+            data.qpos[3:7] = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64)
+            data.qpos[7:19] = DEFAULT_DOF_POS.astype(np.float64)
+            data.qvel[:] = 0.0
+            data.ctrl[:] = 0.0
+            last_action = np.zeros(NUM_ACTIONS, dtype=np.float32)
+            action_raw = np.zeros(NUM_ACTIONS, dtype=np.float32)
+            action = np.zeros(NUM_ACTIONS, dtype=np.float32)
+            target_pos = DEFAULT_DOF_POS.copy()
+            prev_target_pos = DEFAULT_DOF_POS.copy()
+            obs_builder.reset()
+            mujoco.mj_forward(model, data)
+            print("[RESET]")
+
+        # ── Inference ──
+        if inference_step == 0:
+            # Match RoboGauge: policy obs uses XML IMU gyro and framequat sensors.
+            # Fall back to qpos/qvel only for XMLs without those sensors.
+            imu_quat = read_sensor(model, data, "Body_Quat", 4)
+            imu_ang_vel = read_sensor(model, data, "Body_Gyro", 3)
+            quat_wxyz = imu_quat if imu_quat is not None else data.qpos[3:7].copy()
+            if imu_ang_vel is not None:
+                base_ang_vel_body = imu_ang_vel
+            else:
+                world_ang_vel = data.qvel[3:6].copy()
+                base_ang_vel_body = quat_rotate_inverse(quat_wxyz, world_ang_vel)
+
+            # Joint state (qpos[7:19] is FR,FL,RR,RL — matches policy order)
+            q = data.qpos[7:19].copy()
+            dq = data.qvel[6:18].copy()
+
+            # Build obs
+            obs_single = obs_builder.build_single_obs(
+                base_ang_vel_body, quat_wxyz, cmd, q, dq, last_action)
+            onnx_input = obs_builder.build_onnx_input(obs_single)
+
+            # Run ONNX
+            outputs = session.run(None, {'obs': onnx_input})
+            action_raw = outputs[0][0].astype(np.float32)  # [12]
+            action = np.clip(action_raw, -args.clip_actions, args.clip_actions)
+            last_action = action.copy()
+
+            target_pos_raw = DEFAULT_DOF_POS + action * ACTION_SCALE
+            if args.max_target_step > 0.0:
+                delta = np.clip(
+                    target_pos_raw - prev_target_pos,
+                    -args.max_target_step,
+                    args.max_target_step,
+                )
+                target_pos = prev_target_pos + delta
+            else:
+                target_pos = target_pos_raw
+            prev_target_pos = target_pos.copy()
+
+        # ── PD control ──
+        current_pos = data.qpos[7:19]
+        current_vel = data.qvel[6:18]
+        torques = KP * (target_pos - current_pos) - KD * current_vel
+        torques = np.clip(torques, -33.5, 33.5)
+        data.ctrl[:] = torques.astype(np.float64)
+
+        mujoco.mj_step(model, data)
+        view.sync()
+
+        # Real-time sync — use sim_dt because step_count increments every sim step
+        expected_time = step_count * sim_dt
+        elapsed = time.time() - loop_start
+        if 0 < expected_time - elapsed < ctrl_dt:
+            time.sleep(expected_time - elapsed)
+
+        step_count += 1
+        inference_step = (inference_step + 1) % steps_per_inference
+
+        # Periodic status
+        if step_count % 200 == 0:
+            z = data.qpos[2]
+            lin_vel_abs = np.linalg.norm(data.qvel[0:3])
+            print(f"[{step_count}] cmd=({vx:.1f},{vy:.1f},{yaw:.1f}) "
+                  f"z={z:.3f} |v|={lin_vel_abs:.2f} "
+                  f"act[0:4]={np.round(action[:4], 3)}")
+
+    kb.stop()
+    view.close()
+    print("[INFO] done.")
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main())

deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py

@@ -31,8 +31,10 @@ low-level checks, override the default with --power-factor 1.
 
 import argparse
 import json
+import os
 import signal
 import subprocess
+import sys
 import time
 from collections import deque
 from datetime import datetime
@@ -42,13 +44,32 @@ from pathlib import Path
 import numpy as np
 import onnxruntime as ort
 
+HERE = Path(__file__).parent.resolve()
+
+
+def _add_sdk_root_to_path():
+    candidates = []
+    env_root = os.environ.get("GO1_PRO_SDK_ROOT")
+    if env_root:
+        candidates.append(Path(env_root).expanduser())
+    for parent in (HERE, *HERE.parents):
+        candidates.append(parent / "go1_pro_sdk")
+
+    for sdk_root in candidates:
+        if (sdk_root / "go1_pro_sdk").exists():
+            sys.path.insert(0, str(sdk_root))
+            return sdk_root
+    return None
+
+
+SDK_ROOT = _add_sdk_root_to_path()
+
 from go1_pro_sdk import (
     MCUClient, LowCmd, MotorCmd, MotorMode,
     apply_safety, PowerProtectViolation, JOINT_NAMES,
 )
 
 
-HERE = Path(__file__).parent.resolve()
 DEFAULT_ONNX = HERE / "policy_30k.onnx"
 SPORT_KILL_CMD = (
     'ssh pi@192.168.123.161 "sudo pkill -9 -f keep_sport_alive; '
@@ -136,6 +157,49 @@ def motor_tau(state):
     return np.array([state.motorState[i].tauEst for i in range(NUM_ACTIONS)], dtype=np.float32)
 
 
+def motor_mode(state):
+    return np.array([state.motorState[i].mode for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_temperature(state):
+    return np.array([state.motorState[i].temperature for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_reserve(state):
+    return np.array([state.motorState[i].reserve for i in range(NUM_ACTIONS)], dtype=np.int32)
+
+
+def motor_servo_fault(state):
+    modes = motor_mode(state)
+    bad = [
+        f"{JOINT_NAMES[i]}={int(modes[i])}"
+        for i in range(NUM_ACTIONS)
+        if int(modes[i]) != int(MotorMode.Servo)
+    ]
+    if bad:
+        return "motor feedback not servo: " + ", ".join(bad)
+    return None
+
+
+class MotorServoGuard:
+    def __init__(self, fault_frames):
+        self.fault_frames = max(1, int(fault_frames))
+        self.consecutive_bad = 0
+
+    def reset(self):
+        self.consecutive_bad = 0
+
+    def update(self, state):
+        reason = motor_servo_fault(state)
+        if reason is None:
+            self.reset()
+            return None
+        self.consecutive_bad += 1
+        if self.consecutive_bad >= self.fault_frames:
+            return f"{reason} ({self.consecutive_bad} consecutive frames)"
+        return None
+
+
 def validate_joint_order():
     sdk_names = list(JOINT_NAMES)
     if sdk_names != EXPECTED_SDK_JOINT_NAMES:
@@ -316,7 +380,10 @@ class JsonlLogger:
         rec = {"step": int(step), "time_wall": time.time()}
         for k, v in kw.items():
             if isinstance(v, np.ndarray):
-                rec[k] = np.asarray(v, dtype=np.float32).reshape(-1).tolist()
+                if np.issubdtype(v.dtype, np.integer):
+                    rec[k] = np.asarray(v, dtype=np.int32).reshape(-1).tolist()
+                else:
+                    rec[k] = np.asarray(v, dtype=np.float32).reshape(-1).tolist()
             elif isinstance(v, (np.float32, np.float64)):
                 rec[k] = float(v)
             elif isinstance(v, (np.int32, np.int64)):
@@ -390,7 +457,7 @@ def send_position_cmd(client, state, targets, args):
     client.send(cmd)
 
 
-def ramp_to_default(client, args, state):
+def ramp_to_default(client, args, state, logger=None, step_base=0):
     print("[INFO] Ramping to default pose...")
     current = motor_pos(state)
     error = current - DEFAULT_DOF_POS
@@ -402,6 +469,8 @@ def ramp_to_default(client, args, state):
 
     ramp_steps = max(1, int(args.ramp_time * args.ramp_hz))
     dt = 1.0 / args.ramp_hz
+    log_every = max(1, int(args.ramp_hz / 5.0))
+    next_t = time.perf_counter()
 
     for i in range(ramp_steps):
         if EXIT:
@@ -430,6 +499,8 @@ def ramp_to_default(client, args, state):
             position_protect_limit=None,
         )
         client.send(cmd)
+        if logger is not None and (i % log_every == 0 or i == ramp_steps - 1):
+            log_state(logger, step_base * 100000 + i, "RAMP", state, target=target)
         if i % max(1, ramp_steps // 4) == 0:
             actual = motor_pos(state)
             print(
@@ -437,16 +508,29 @@ def ramp_to_default(client, args, state):
                 f"target_err={np.max(np.abs(target - DEFAULT_DOF_POS)):.3f} "
                 f"actual_err={np.max(np.abs(actual - DEFAULT_DOF_POS)):.3f}"
             )
-        time.sleep(dt)
+        next_t += dt
+        sleep = next_t - time.perf_counter()
+        if sleep > 0:
+            time.sleep(sleep)
+        else:
+            next_t = time.perf_counter()
 
-    for _ in range(max(1, int(0.5 * args.ramp_hz))):
+    hold_steps = max(1, int(0.5 * args.ramp_hz))
+    for i in range(hold_steps):
         if EXIT:
             return state
         new_state = client.recv_latest()
         if new_state is not None:
             state = new_state
         send_hold_cmd(client, state, args)
-        time.sleep(dt)
+        if logger is not None and (i % log_every == 0 or i == hold_steps - 1):
+            log_state(logger, step_base * 100000 + ramp_steps + i, "RAMP_HOLD", state, target=DEFAULT_DOF_POS)
+        next_t += dt
+        sleep = next_t - time.perf_counter()
+        if sleep > 0:
+            time.sleep(sleep)
+        else:
+            next_t = time.perf_counter()
 
     print("[INFO] Default pose reached.")
     return state
@@ -481,8 +565,14 @@ def action_ok(action_raw, args):
     if not np.all(np.isfinite(action_raw)):
         return False, "action is non-finite"
     max_abs = float(np.max(np.abs(action_raw)))
+    hard_limit = float(args.action_hard_trip_limit)
+    if hard_limit > 0 and max_abs > hard_limit:
+        return False, f"action abs {max_abs:.2f} > hard trip {hard_limit:.2f}"
     if max_abs > args.action_trip_limit:
-        return False, f"action abs {max_abs:.2f} > trip {args.action_trip_limit:.2f}"
+        return True, (
+            f"action abs {max_abs:.2f} > soft trip {args.action_trip_limit:.2f}; "
+            f"clipped to {args.action_clip:.2f}"
+        )
     return True, "ok"
 
 
@@ -580,6 +670,9 @@ def log_state(logger, step, mode, state, cmd=None, cmd_raw=None, obs_single=None
         dof_pos=motor_pos(state),
         dof_vel=motor_vel(state),
         tau_est=motor_tau(state),
+        motor_mode=motor_mode(state),
+        motor_temperature=motor_temperature(state),
+        motor_reserve=motor_reserve(state),
         commands_raw=np.zeros(3, dtype=np.float32) if cmd_raw is None else cmd_raw,
         commands=np.zeros(3, dtype=np.float32) if cmd is None else cmd,
         obs_single=np.zeros(NUM_OBS, dtype=np.float32) if obs_single is None else obs_single,
@@ -799,6 +892,7 @@ def run_deploy(args):
         edge.update(state)
         obs_builder = ObsHistoryBuilder()
         cmd_filter = CommandFilter(args)
+        servo_guard = MotorServoGuard(args.motor_mode_fault_frames)
 
         sm_state = State.IDLE
         last_action = np.zeros(NUM_ACTIONS, dtype=np.float32)
@@ -833,8 +927,15 @@ def run_deploy(args):
                 prev_action[:] = 0.0
                 prev_target = DEFAULT_DOF_POS.copy()
                 rl_step = 0
+                servo_guard.reset()
                 send_damping(client)
 
+            if sm_state in (State.HOLD, State.OBS_TEST, State.INFER_TEST, State.RL):
+                servo_fault = servo_guard.update(state)
+            else:
+                servo_guard.reset()
+                servo_fault = None
+
             cmd_raw = get_command(state, args)
             if sm_state in (State.OBS_TEST, State.INFER_TEST, State.RL):
                 cmd = cmd_filter.update(cmd_raw)
@@ -853,7 +954,7 @@ def run_deploy(args):
                 if r2_rose:
                     print("\n[R2] IDLE -> CALIBRATE")
                     sm_state = State.CALIBRATE
-                    state = ramp_to_default(client, args, state)
+                    state = ramp_to_default(client, args, state, logger=logger, step_base=step)
                     obs_builder.reset()
                     cmd_filter.reset()
                     last_action[:] = 0.0
@@ -865,21 +966,31 @@ def run_deploy(args):
             elif sm_state == State.HOLD:
                 send_hold_cmd(client, state, args)
                 if r2_rose:
-                    print("\n[R2] HOLD -> OBS_TEST")
-                    obs_builder.reset()
-                    cmd_filter.reset()
-                    last_action[:] = 0.0
-                    prev_action[:] = 0.0
-                    sm_state = State.OBS_TEST
+                    if servo_fault:
+                        reason = servo_fault
+                        print(f"\n[FAULT] HOLD -> OBS_TEST blocked: {reason}")
+                        sm_state = State.FAULT
+                        send_damping(client)
+                    else:
+                        print("\n[R2] HOLD -> OBS_TEST")
+                        obs_builder.reset()
+                        cmd_filter.reset()
+                        last_action[:] = 0.0
+                        prev_action[:] = 0.0
+                        sm_state = State.OBS_TEST
 
             elif sm_state == State.OBS_TEST:
                 send_hold_cmd(client, state, args)
                 obs_single = obs_builder.build_single(state, cmd, last_action)
                 obs_builder.build_onnx_input(obs_single)
-                ok, reason = state_ok(state, args)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if not ok:
                     print(f"\n[FAULT] OBS_TEST state check failed: {reason}")
                     sm_state = State.FAULT
+                    send_damping(client)
                 elif r2_rose:
                     print("\n[R2] OBS_TEST -> INFER_TEST")
                     obs_builder.reset()
@@ -892,7 +1003,10 @@ def run_deploy(args):
                 send_hold_cmd(client, state, args)
                 obs_single = obs_builder.build_single(state, cmd, last_action)
                 onnx_input = obs_builder.build_onnx_input(obs_single)
-                ok, reason = state_ok(state, args)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
                     ok, reason = action_ok(action_raw, args)
@@ -925,14 +1039,17 @@ def run_deploy(args):
                     cmd_filter.reset()
                     last_action[:] = 0.0
                     prev_action[:] = 0.0
-                    state = ramp_to_default(client, args, state)
+                    state = ramp_to_default(client, args, state, logger=logger, step_base=step)
                     prev_target = DEFAULT_DOF_POS.copy()
                     rl_step = 0
                     continue
 
                 obs_single = obs_builder.build_single(state, cmd, last_action)
                 onnx_input = obs_builder.build_onnx_input(obs_single)
-                ok, reason = state_ok(state, args)
+                if servo_fault:
+                    ok, reason = False, servo_fault
+                else:
+                    ok, reason = state_ok(state, args)
                 if ok:
                     action_raw = policy(onnx_input)
                     ok, reason = action_ok(action_raw, args)
@@ -965,7 +1082,7 @@ def run_deploy(args):
                 if r2_rose:
                     print("\n[R2] FAULT -> CALIBRATE")
                     sm_state = State.CALIBRATE
-                    state = ramp_to_default(client, args, state)
+                    state = ramp_to_default(client, args, state, logger=logger, step_base=step)
                     prev_target = DEFAULT_DOF_POS.copy()
                     sm_state = State.HOLD
                     print("[STATE] HOLD")
@@ -992,6 +1109,10 @@ def run_deploy(args):
                 )
                 print(f"  RC: {fmt_rc(state)}")
                 print(f"  cmd={np.round(cmd, 3)} q={np.round(motor_pos(state), 2)}")
+                print(
+                    f"  motor_mode[FR]={motor_mode(state)[:3].tolist()} "
+                    f"temp[FR]={motor_temperature(state)[:3].tolist()}"
+                )
                 if sm_state in (State.INFER_TEST, State.RL, State.FAULT):
                     print(
                         f"  action_raw_max={np.max(np.abs(action_raw)):.3f} "
@@ -1076,8 +1197,8 @@ def build_arg_parser():
     parser.add_argument("--cmd-yaw", type=float, default=0.0)
 
     parser.add_argument("--rate-hz", type=float, default=50.0)
-    parser.add_argument("--ramp-time", type=float, default=2.5)
-    parser.add_argument("--ramp-hz", type=float, default=100.0)
+    parser.add_argument("--ramp-time", type=float, default=5.0)
+    parser.add_argument("--ramp-hz", type=float, default=50.0)
     parser.add_argument("--warmup-steps", type=int, default=50)
     parser.add_argument("--max-steps", type=int, default=0)
     parser.add_argument("--print-every", type=int, default=50)
@@ -1086,7 +1207,10 @@ def build_arg_parser():
     parser.add_argument("--max-pitch-deg", type=float, default=35.0)
     parser.add_argument("--max-dof-vel", type=float, default=30.0)
     parser.add_argument("--max-gyro", type=float, default=15.0)
-    parser.add_argument("--action-trip-limit", type=float, default=12.0)
+    parser.add_argument("--action-trip-limit", type=float, default=12.0,
+                        help="Soft raw-action warning threshold; output is still clipped to --action-clip")
+    parser.add_argument("--action-hard-trip-limit", type=float, default=16.0,
+                        help="Hard raw-action fault threshold; <=0 disables the hard trip")
     parser.add_argument("--action-clip", type=float, default=4.0)
     parser.add_argument("--action-ema-alpha", type=float, default=0.0)
     parser.add_argument("--max-target-step", type=float, default=0.08)
@@ -1094,6 +1218,8 @@ def build_arg_parser():
 
     parser.add_argument("--log-dir", default=str(HERE / "logs"), help="JSONL log directory; empty disables logging")
     parser.add_argument("--log-flush-every", type=int, default=50)
+    parser.add_argument("--motor-mode-fault-frames", type=int, default=3,
+                        help="Consecutive non-servo feedback frames required before a motor-mode fault")
     return parser