cyy/go1_pro_deploy
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

bpu

Browse Source
This commit is contained in:
cyy_mac
2026-07-27 23:17:55 +08:00
parent ebbe9d5432
commit e017bce6e2
20 changed files with 3062 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
.gitignore vendored
View File

@@ -1 +1,19 @@
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/mapper_output/
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_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

188
deploy_45dim_rl_gym/bpu_deploy_x5/README.md Normal file
View File

@@ -0,0 +1,188 @@
# 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 版本。
## 重要限制
不要用 `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_6500_gemm/policy_robotlab_6500_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_6500_gemm/policy_robotlab_6500_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_6500_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 \
--check-reference-00000 \
--repeat 1000
```
通过时应看到：
```text
reference_max_abs_diff 0.0
```
当前板端 `root@192.168.150.167` 已验证：
```text
reference_max_abs_diff 4.76837158203125e-07
reference_mean_abs_diff 3.071812386679085e-07
repeat=1000 avg_ms=1.019592 # Python ctypes -> C++ DNN API
duplicate_errors=0
```
纯 C++ benchmark：
```bash
cd /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_deploy_x5/cpp
./bpu_dnn_bench
```
当前板端结果：
```text
reference_max_abs_diff 0
repeat=1000 cpp_avg_ms=0.711932
duplicate_errors=0
```
## 离线推理检查
先不要接电机控制，只运行 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 读取状态，但不会发送电机指令。
## 悬空状态机测试
只有 `--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
对照。

207
deploy_45dim_rl_gym/bpu_deploy_x5/bpu_policy.py Normal file
View File

@@ -0,0 +1,207 @@
#!/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"
input_shape = (1, 1, 1, 450)
output_shape = (1, 12, 1, 1)
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_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._output = np.empty(12, 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 != 450:
raise ValueError(f"BPU policy input has {arr.size} values, expected 450")
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)
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 != 450:
raise ValueError(f"BPU policy input has {arr.size} values, expected 450")
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

103
deploy_45dim_rl_gym/bpu_deploy_x5/cpp/bpu_dnn_bench.cpp Normal file
View File

@@ -0,0 +1,103 @@
#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 *input450, float *output12, char *err,
int err_len);
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]);
std::vector<float> obs;
if (!read_f32_file(input, &obs) || obs.size() != 450) {
std::cerr << "failed to read 450 float32 input: " << input << "\n";
return 2;
}
char err[1024] = {};
void *handle = rlgym_bpu_create(model, 1, 0, err, sizeof(err));
if (handle == nullptr) {
std::cerr << err << "\n";
return 3;
}
float out[12] = {};
if (rlgym_bpu_infer(handle, obs.data(), out, err, sizeof(err)) != 0) {
std::cerr << err << "\n";
rlgym_bpu_destroy(handle);
return 4;
}
float max_diff = 0.0f;
for (int i = 0; i < 12; ++i) {
max_diff = std::max(max_diff, std::fabs(out[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, 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 << "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, out + 12, [](float a, float b) {
return std::fabs(a) < std::fabs(b);
})
<< "\n";
rlgym_bpu_destroy(handle);
return 0;
}

227
deploy_45dim_rl_gym/bpu_deploy_x5/cpp/bpu_dnn_policy.cpp Normal file
View File

@@ -0,0 +1,227 @@
#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 kInputFloats = 450;
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");
validate_tensor(input_tensors_[0].properties, kInputFloats, HB_DNN_TENSOR_TYPE_F32,
"input");
validate_tensor(output_tensors_[0].properties, kOutputFloats, 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) * kInputFloats);
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) * kOutputFloats);
}
private:
static void validate_tensor(const hbDNNTensorProperties &props, int expected_count,
int expected_type, const char *name) {
const int count = element_count(props.validShape);
if (count != expected_count) {
std::ostringstream oss;
oss << name << " valid element count " << count << " != " << expected_count;
throw std::runtime_error(oss.str());
}
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 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 *input450, float *output12, 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(input450, output12);
return 0;
} catch (const std::exception &e) {
set_error(err, err_len, e.what());
return -1;
}
}
void rlgym_bpu_destroy(void *handle) {
delete static_cast<BpuDnnPolicy *>(handle);
}
const char *rlgym_bpu_version() {
return "cpp_dnn_api_x5";
}
}

20
deploy_45dim_rl_gym/bpu_deploy_x5/cpp/build_board.sh Normal file
View File

@@ -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

1241
deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_robotlab_bpu_x5_fastcpp.py Normal file
View File

File diff suppressed because it is too large Load Diff

74
deploy_45dim_rl_gym/bpu_deploy_x5/test_bpu_policy.py Normal file
View File

@@ -0,0 +1,74 @@
#!/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 bpu_infer_lib repeatedly, and optionally checks the known
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)
if data.size != 450:
raise ValueError(f"{input_path} has {data.size} float32 values, expected 450")
policy = BpuInferLibPolicy(args.bpu_model)
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:
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()

237
deploy_45dim_rl_gym/bpu_quantization/README.md Normal file
View File

@@ -0,0 +1,237 @@
# RDK X5 BPU 量化流程
这个目录用于把 `../policy_robotlab_15000.onnx` 转成 RDK X5 可运行的
Horizon runtime `.bin`。量化在 Mac 上用 CPU Docker 完成，板端只做离线
`hrt_model_exec` 验证，暂时不要直接接入实机控制。
参考资料：
- 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。因此这里不能直接拿原始 RobotLab ONNX 编译，
需要先降级 opset，再把 `[1, 450]` 输入包成固定 4D `NCHW`：
`[1, 1, 1, 450]`。
## 当前状态
已经完成 `policy_robotlab_15000.onnx` 和 `policy_robotlab_6500.onnx` 的 int16
量化。当前 BPU 部署默认使用 6500 版本：
- 原始模型：`../policy_robotlab_6500.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_6500_gemm/policy_robotlab_6500_int16_gemm.bin`
`mapper_output*/`、`.hb_check/`、校准数据、中间 ONNX、`hb_mapper` 日志都被
`.gitignore` 忽略；需要时按下面步骤重新生成。仓库里只保留脚本和 YAML 配置。
6500 量化使用 `calibration_data_fast64/` 的 64 个真实样本。原因是
`hb_mapper` 会先尝试 calibration batch 8，但当前 4D featuremap 包装会被工具链
内部改成固定 batch 的 reshape，batch 8 失败后会退回 batch 1；用 64 样本可以把
校准时间从 512 次 batch1 显著降下来。
## 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 真实日志对比、悬空状态机测试，再进入地面低速测试。

76
deploy_45dim_rl_gym/bpu_quantization/calibration_data_metadata.json Normal file
View File

@@ -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
}
]
}

46
deploy_45dim_rl_gym/bpu_quantization/compare_4d_onnx.py Normal file
View File

@@ -0,0 +1,46 @@
#!/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):
paths = sorted(Path(calibration_dir).glob("*.bin"))[:limit]
if not paths:
raise FileNotFoundError(f"No calibration .bin files found in {calibration_dir}")
return [np.fromfile(path, dtype=np.float32).reshape(1, 450) for path in paths]
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("--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):
out_flat = flat.run(None, {flat_input: sample})[0]
out_wrapped = wrapped.run(None, {wrapped_input: sample.reshape(1, 1, 1, 450)})[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()

30
deploy_45dim_rl_gym/bpu_quantization/convert_opset.py Normal file
View File

@@ -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()

91
deploy_45dim_rl_gym/bpu_quantization/downgrade_policy_to_opset11.py Normal file
View File

@@ -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()

61
deploy_45dim_rl_gym/bpu_quantization/make_bpu_4d_onnx.py Normal file
View File

@@ -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()

160
deploy_45dim_rl_gym/bpu_quantization/make_calibration_data.py Normal file
View File

@@ -0,0 +1,160 @@
#!/usr/bin/env python3
"""Build float32 BPU calibration inputs from recorded RobotLab deployment logs."""
import argparse
import json
import random
from collections import deque
from pathlib import Path
import numpy as np
NUM_OBS = 45
HISTORY_LEN = 10
ONNX_INPUT_DIM = NUM_OBS * HISTORY_LEN
TERM_DIMS = (3, 3, 3, 12, 12, 12)
HERE = Path(__file__).resolve().parent
DEFAULT_LOG_ROOT = HERE.parents[1] / "logs"
def build_onnx_input(history):
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):
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)
onnx_input = build_onnx_input(history)
if not np.all(np.isfinite(onnx_input)):
continue
seen += 1
run_seen += 1
reservoir_add(samples, onnx_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 RobotLab JSONL logs."
)
parser.add_argument(
"--logs-root",
type=Path,
default=DEFAULT_LOG_ROOT,
help="Directory containing robotlab_go1_deploy_*/steps.jsonl.",
)
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("--seed", type=int, default=20260727)
parser.add_argument("--overwrite", action="store_true")
args = parser.parse_args()
if args.max_samples < 32:
raise ValueError("--max-samples must be at least 32")
log_paths = sorted(args.logs_root.glob("robotlab_go1_deploy_*/steps.jsonl"))
if not log_paths:
raise FileNotFoundError(f"No RobotLab step logs found below {args.logs_root}")
samples, total_seen, usable_runs = collect_samples(
log_paths, args.max_samples, args.seed
)
if len(samples) < 32:
raise RuntimeError(
f"Only {len(samples)} valid RL inputs found; need at least 32 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")
metadata = {
"format": "raw float32 feature-map",
"flat_shape": [1, ONNX_INPUT_DIM],
"mapper_shape": [1, 1, 1, ONNX_INPUT_DIM],
"history_len": HISTORY_LEN,
"num_obs": NUM_OBS,
"term_dims": list(TERM_DIMS),
"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()

27
deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_int16.yaml Normal file
View File

@@ -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"

27
deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_int16_gemm.yaml Normal file
View File

@@ -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"

27
deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_6500_int16_gemm.yaml Normal file
View File

@@ -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"

149
deploy_45dim_rl_gym/bpu_quantization/replace_group_conv_with_gemm.py Normal file
View File

@@ -0,0 +1,149 @@
#!/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:
raise ValueError(f"Cannot find node {CONV_NAME!r}")
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()

58
deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab_fastcpp.py
View File

@@ -62,7 +62,7 @@ except ImportError as exc:
"PYTHONPATH=/root/go1_pro_sdk python3 setup.py build_ext --inplace" "PYTHONPATH=/root/go1_pro_sdk python3 setup.py build_ext --inplace"
) from exc ) from exc
DEFAULT_ONNX = HERE / "policy_robotlab_15000.onnx" DEFAULT_ONNX = HERE / "policy_robotlab_6500.onnx"
LOWCMD_BACKEND = "cpp_checked_servo12" LOWCMD_BACKEND = "cpp_checked_servo12"
SPORT_KILL_CMD = ( SPORT_KILL_CMD = (
'ssh pi@192.168.123.161 "sudo pkill -9 -f keep_sport_alive; ' 'ssh pi@192.168.123.161 "sudo pkill -9 -f keep_sport_alive; '
@@ -271,9 +271,15 @@ class ObsHistoryBuilder:
def __init__(self): def __init__(self):
self.history = deque(maxlen=HISTORY_LEN) self.history = deque(maxlen=HISTORY_LEN)
self.history_time = deque(maxlen=HISTORY_LEN)
self.last_zero_pad = HISTORY_LEN
self.last_span_ms = 0.0
def reset(self): def reset(self):
self.history.clear() self.history.clear()
self.history_time.clear()
self.last_zero_pad = HISTORY_LEN
self.last_span_ms = 0.0
def build_single(self, state, cmd, last_action): def build_single(self, state, cmd, last_action):
quat = as_np(state.imu.quaternion) quat = as_np(state.imu.quaternion)
@@ -291,9 +297,18 @@ class ObsHistoryBuilder:
obs = np.clip(obs, -CLIP_OBS, CLIP_OBS) obs = np.clip(obs, -CLIP_OBS, CLIP_OBS)
return np.nan_to_num(obs, nan=0.0, posinf=0.0, neginf=0.0) return np.nan_to_num(obs, nan=0.0, posinf=0.0, neginf=0.0)
def build_onnx_input(self, obs_single): def build_onnx_input(self, obs_single, now=None):
if now is None:
now = time.perf_counter()
self.history.append(obs_single.copy()) self.history.append(obs_single.copy())
self.history_time.append(float(now))
frames = list(self.history) frames = list(self.history)
times = list(self.history_time)
self.last_zero_pad = max(0, HISTORY_LEN - len(frames))
if len(times) >= 2:
self.last_span_ms = float((times[-1] - times[0]) * 1000.0)
else:
self.last_span_ms = 0.0
while len(frames) < HISTORY_LEN: while len(frames) < HISTORY_LEN:
frames.insert(0, np.zeros(NUM_OBS, dtype=np.float32)) frames.insert(0, np.zeros(NUM_OBS, dtype=np.float32))
@@ -307,6 +322,14 @@ class ObsHistoryBuilder:
obs = np.concatenate(chunks, dtype=np.float32).reshape(1, ONNX_INPUT_DIM) obs = np.concatenate(chunks, dtype=np.float32).reshape(1, ONNX_INPUT_DIM)
return np.nan_to_num(obs, nan=0.0, posinf=0.0, neginf=0.0) return np.nan_to_num(obs, nan=0.0, posinf=0.0, neginf=0.0)
def stats(self):
return {
"history_fill": len(self.history),
"history_zero_pad": self.last_zero_pad,
"history_span_ms": self.last_span_ms,
"history_expected_span_ms": (HISTORY_LEN - 1) * 20.0,
}
class OnnxPolicy: class OnnxPolicy:
def __init__(self, onnx_path): def __init__(self, onnx_path):
@@ -361,6 +384,7 @@ class JsonlLogger:
"joint_names_policy": POLICY_JOINT_NAMES, "joint_names_policy": POLICY_JOINT_NAMES,
"joint_order_policy": ["FR", "FL", "RR", "RL"], "joint_order_policy": ["FR", "FL", "RR", "RL"],
"lowcmd_backend": LOWCMD_BACKEND, "lowcmd_backend": LOWCMD_BACKEND,
"history_expected_span_ms": (HISTORY_LEN - 1) * 20.0,
} }
for k, v in vars(args).items(): for k, v in vars(args).items():
if isinstance(v, (str, int, float, bool, type(None))): if isinstance(v, (str, int, float, bool, type(None))):
@@ -625,8 +649,9 @@ def connect_client(args):
def log_state(logger, step, mode, state, cmd=None, cmd_raw=None, obs_single=None, action_raw=None, def log_state(logger, step, mode, state, cmd=None, cmd_raw=None, obs_single=None, action_raw=None,
action_safe=None, target=None, state_reason="ok", timing=None): action_safe=None, target=None, state_reason="ok", timing=None, history=None):
timing = {} if timing is None else timing timing = {} if timing is None else timing
history = {} if history is None else history
logger.log( logger.log(
step, step,
mode=mode, mode=mode,
@@ -657,6 +682,12 @@ def log_state(logger, step, mode, state, cmd=None, cmd_raw=None, obs_single=None
recv_ms=float(timing.get("recv_ms", 0.0)), recv_ms=float(timing.get("recv_ms", 0.0)),
policy_ms=float(timing.get("policy_ms", 0.0)), policy_ms=float(timing.get("policy_ms", 0.0)),
work_ms=float(timing.get("work_ms", 0.0)), work_ms=float(timing.get("work_ms", 0.0)),
state_fresh=bool(timing.get("state_fresh", False)),
state_age_ms=float(timing.get("state_age_ms", 0.0)),
history_fill=int(history.get("history_fill", 0)),
history_zero_pad=int(history.get("history_zero_pad", HISTORY_LEN)),
history_span_ms=float(history.get("history_span_ms", 0.0)),
history_expected_span_ms=float(history.get("history_expected_span_ms", (HISTORY_LEN - 1) * 20.0)),
) )
@@ -880,6 +911,7 @@ def run_deploy(args):
dt = 1.0 / args.rate_hz dt = 1.0 / args.rate_hz
next_t = time.perf_counter() next_t = time.perf_counter()
prev_loop_t = None prev_loop_t = None
last_state_t = time.perf_counter()
print("[INFO] R2 advances layers. L2 emergency-stops to IDLE.") print("[INFO] R2 advances layers. L2 emergency-stops to IDLE.")
print("[INFO] Ctrl+C exits with safe_stop.") print("[INFO] Ctrl+C exits with safe_stop.")
@@ -893,6 +925,8 @@ def run_deploy(args):
"recv_ms": 0.0, "recv_ms": 0.0,
"policy_ms": 0.0, "policy_ms": 0.0,
"work_ms": 0.0, "work_ms": 0.0,
"state_fresh": False,
"state_age_ms": 0.0,
} }
prev_loop_t = loop_t0 prev_loop_t = loop_t0
@@ -902,9 +936,14 @@ def run_deploy(args):
timing["recv_ms"] = (time.perf_counter() - recv_t0) * 1000.0 timing["recv_ms"] = (time.perf_counter() - recv_t0) * 1000.0
if new_state is not None: if new_state is not None:
state = new_state state = new_state
last_state_t = loop_t0
if timing is not None:
timing["state_fresh"] = True
if state is None: if state is None:
time.sleep(0.001) time.sleep(0.001)
continue continue
if timing is not None:
timing["state_age_ms"] = (loop_t0 - last_state_t) * 1000.0
rising, _ = edge.update(state) rising, _ = edge.update(state)
r2_rose = "R2" in rising r2_rose = "R2" in rising
@@ -974,7 +1013,7 @@ def run_deploy(args):
elif sm_state == State.OBS_TEST: elif sm_state == State.OBS_TEST:
send_hold_cmd(client, state, args) send_hold_cmd(client, state, args)
obs_single = obs_builder.build_single(state, cmd, last_action) obs_single = obs_builder.build_single(state, cmd, last_action)
obs_builder.build_onnx_input(obs_single) obs_builder.build_onnx_input(obs_single, now=loop_t0)
if servo_fault: if servo_fault:
ok, reason = False, servo_fault ok, reason = False, servo_fault
else: else:
@@ -994,7 +1033,7 @@ def run_deploy(args):
elif sm_state == State.INFER_TEST: elif sm_state == State.INFER_TEST:
send_hold_cmd(client, state, args) send_hold_cmd(client, state, args)
obs_single = obs_builder.build_single(state, cmd, last_action) obs_single = obs_builder.build_single(state, cmd, last_action)
onnx_input = obs_builder.build_onnx_input(obs_single) onnx_input = obs_builder.build_onnx_input(obs_single, now=loop_t0)
if servo_fault: if servo_fault:
ok, reason = False, servo_fault ok, reason = False, servo_fault
else: else:
@@ -1040,7 +1079,7 @@ def run_deploy(args):
continue continue
obs_single = obs_builder.build_single(state, cmd, last_action) obs_single = obs_builder.build_single(state, cmd, last_action)
onnx_input = obs_builder.build_onnx_input(obs_single) onnx_input = obs_builder.build_onnx_input(obs_single, now=loop_t0)
if servo_fault: if servo_fault:
ok, reason = False, servo_fault ok, reason = False, servo_fault
else: else:
@@ -1101,6 +1140,7 @@ def run_deploy(args):
target=target, target=target,
state_reason=reason, state_reason=reason,
timing=timing, timing=timing,
history=obs_builder.stats(),
) )
if step % args.print_every == 0: if step % args.print_every == 0:
@@ -1120,6 +1160,12 @@ def run_deploy(args):
f" action_raw_max={np.max(np.abs(action_raw)):.3f} " f" action_raw_max={np.max(np.abs(action_raw)):.3f} "
f"action_safe_max={np.max(np.abs(action_safe)):.3f} reason={reason}" f"action_safe_max={np.max(np.abs(action_safe)):.3f} reason={reason}"
) )
print(
f" hist={obs_builder.last_span_ms:.1f}/180.0ms "
f"fill={len(obs_builder.history)}/{HISTORY_LEN} "
f"fresh={bool(timing.get('state_fresh', False)) if timing else False} "
f"age={timing.get('state_age_ms', 0.0) if timing else 0.0:.1f}ms"
)
if sm_state == State.RL: if sm_state == State.RL:
print(f" target={np.round(target, 2)} rl_step={rl_step}") print(f" target={np.round(target, 2)} rl_step={rl_step}")