diff --git a/.gitignore b/.gitignore index 5292519..127215e 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,19 @@ -logs/ \ No newline at end of file +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 diff --git a/deploy_45dim_rl_gym/bpu_deploy_x5/README.md b/deploy_45dim_rl_gym/bpu_deploy_x5/README.md new file mode 100644 index 0000000..64ba1e3 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_deploy_x5/README.md @@ -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 +对照。 diff --git a/deploy_45dim_rl_gym/bpu_deploy_x5/bpu_policy.py b/deploy_45dim_rl_gym/bpu_deploy_x5/bpu_policy.py new file mode 100644 index 0000000..ceea632 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_deploy_x5/bpu_policy.py @@ -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 diff --git a/deploy_45dim_rl_gym/bpu_deploy_x5/cpp/bpu_dnn_bench.cpp b/deploy_45dim_rl_gym/bpu_deploy_x5/cpp/bpu_dnn_bench.cpp new file mode 100644 index 0000000..4c5daa3 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_deploy_x5/cpp/bpu_dnn_bench.cpp @@ -0,0 +1,103 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 *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(sizeof(float)) != 0) { + return false; + } + data->resize(static_cast(size) / sizeof(float)); + ifs.read(reinterpret_cast(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 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(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; +} diff --git a/deploy_45dim_rl_gym/bpu_deploy_x5/cpp/bpu_dnn_policy.cpp b/deploy_45dim_rl_gym/bpu_deploy_x5/cpp/bpu_dnn_policy.cpp new file mode 100644 index 0000000..70bfaf3 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_deploy_x5/cpp/bpu_dnn_policy.cpp @@ -0,0 +1,227 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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(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(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 &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 input_tensors_; + std::vector 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(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(handle); +} + +const char *rlgym_bpu_version() { + return "cpp_dnn_api_x5"; +} + +} diff --git a/deploy_45dim_rl_gym/bpu_deploy_x5/cpp/build_board.sh b/deploy_45dim_rl_gym/bpu_deploy_x5/cpp/build_board.sh new file mode 100644 index 0000000..27d060b --- /dev/null +++ b/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 diff --git a/deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_robotlab_bpu_x5_fastcpp.py b/deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_robotlab_bpu_x5_fastcpp.py new file mode 100644 index 0000000..57fd858 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_robotlab_bpu_x5_fastcpp.py @@ -0,0 +1,1241 @@ +#!/usr/bin/env python3 +""" +Deploy the RoboGauge Go1 45-dim RobotLab BPU policy on Unitree Go1 PRO. + +This uses go1_pro_sdk direct MCU control, not LCM or the official Unitree SDK. + +Policy: + - policy_robotlab_6500_int16_gemm.bin + - single-frame obs: 45 dims + - BPU input: 10-frame history, 1x1x1x450 featuremap, stacked by observation terms + - command scale: [1.0, 1.0, 1.0] + - joint order: FR, FL, RR, RL, matching go1_pro_sdk motor order + +Safety-first workflow: + 1. MONITOR: --monitor, no motor command + 2. OBS-CHECK: --obs-check, no motor command + 3. INFER-CHECK: --infer-check, BPU only, no motor command + 4. STATE MACHINE: + IDLE -> CALIBRATE -> HOLD -> OBS_TEST -> INFER_TEST -> RL + R2 advances one layer, L2 emergency-stops to IDLE. + RL output is only enabled when --enable-rl is passed. + +Before low-level control, kill sport processes on the Pi: + ssh pi@192.168.123.161 + sudo pkill -9 -f keep_sport_alive + sudo pkill -9 -f Legged_sport + sudo pkill -9 -f appTransit + +Initial tests should be done with the robot suspended. For extra-conservative +low-level checks, override the default with --power-factor 1. +""" + +import argparse +import json +import signal +import subprocess +import sys +import time +from collections import deque +from datetime import datetime +from enum import Enum +from pathlib import Path + +import numpy as np + +from go1_pro_sdk import ( + MCUClient, MotorMode, PowerProtectViolation, JOINT_NAMES, +) + +from bpu_policy import BpuInferLibPolicy + + +HERE = Path(__file__).parent.resolve() +DEPLOY_ROOT = HERE.parents[1] +WORKSPACE_ROOT = HERE.parents[2] +SDK_FAST_LOW_CMD = WORKSPACE_ROOT / "go1_pro_sdk" / "fast_lowcmd_cpp" +if SDK_FAST_LOW_CMD.exists(): + sys.path.insert(0, str(SDK_FAST_LOW_CMD)) +try: + from fast_lowcmd import FastLowCmdBuilder +except ImportError as exc: + raise RuntimeError( + "C++ fast LowCmd backend is required for this entrypoint. " + "Build it first: cd /root/go1_pro_sdk/fast_lowcmd_cpp && " + "PYTHONPATH=/root/go1_pro_sdk python3 setup.py build_ext --inplace" + ) from exc + +DEFAULT_BPU_MODEL = ( + HERE.parent / "bpu_quantization" / "mapper_output_6500_gemm" / + "policy_robotlab_6500_int16_gemm.bin" +) +LOWCMD_BACKEND = "cpp_checked_servo12" +SPORT_KILL_CMD = ( + 'ssh pi@192.168.123.161 "sudo pkill -9 -f keep_sport_alive; ' + 'sudo pkill -9 -f Legged_sport; sudo pkill -9 -f appTransit"' +) + +NUM_OBS = 45 +NUM_ACTIONS = 12 +HISTORY_LEN = 10 +POLICY_INPUT_DIM = NUM_OBS * HISTORY_LEN +BPU_INPUT_SHAPE = [1, 1, 1, POLICY_INPUT_DIM] +BPU_OUTPUT_SHAPE = [1, NUM_ACTIONS, 1, 1] + +ACTION_SCALE = 0.25 +CLIP_OBS = 100.0 +ANG_VEL_SCALE = 0.25 +DOF_VEL_SCALE = 0.05 +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_YAW = 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) +EXPECTED_SDK_JOINT_NAMES = [ + "FR_0", "FR_1", "FR_2", + "FL_0", "FL_1", "FL_2", + "RR_0", "RR_1", "RR_2", + "RL_0", "RL_1", "RL_2", +] +_FAST_LOW_CMD_BUILDER = None +POLICY_JOINT_NAMES = [ + "FR_hip", "FR_thigh", "FR_calf", + "FL_hip", "FL_thigh", "FL_calf", + "RR_hip", "RR_thigh", "RR_calf", + "RL_hip", "RL_thigh", "RL_calf", +] + +EXIT = False + + +def _sig_handler(signum, frame): + global EXIT + EXIT = True + + +signal.signal(signal.SIGINT, _sig_handler) +signal.signal(signal.SIGTERM, _sig_handler) + + +class State(Enum): + IDLE = "IDLE" + CALIBRATE = "CALIBRATE" + HOLD = "HOLD" + OBS_TEST = "OBS_TEST" + INFER_TEST = "INFER_TEST" + RL = "RL" + FAULT = "FAULT" + + +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 as_np(values, dtype=np.float32): + return np.asarray(values, dtype=dtype) + + +def motor_pos(state): + return np.array([state.motorState[i].q for i in range(NUM_ACTIONS)], dtype=np.float32) + + +def motor_vel(state): + return np.array([state.motorState[i].dq for i in range(NUM_ACTIONS)], dtype=np.float32) + + +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: + raise RuntimeError( + "go1_pro_sdk JOINT_NAMES order mismatch.\n" + f" expected: {EXPECTED_SDK_JOINT_NAMES}\n" + f" actual : {sdk_names}" + ) + print("[INFO] Joint order check passed: SDK and policy both use FR, FL, RR, RL.") + for i, (sdk_name, policy_name, q0) in enumerate( + zip(sdk_names, POLICY_JOINT_NAMES, DEFAULT_DOF_POS)): + print(f" [{i:02d}] {sdk_name:4s} -> {policy_name:8s} default={q0:+.3f}") + + +def apply_deadzone(value, deadzone): + if deadzone <= 0.0: + return float(value) + mag = abs(float(value)) + if mag <= deadzone: + return 0.0 + return float(np.sign(value) * (mag - deadzone) / max(1e-6, 1.0 - deadzone)) + + +def get_command(state, args): + if args.no_rc: + cmd = np.array([args.cmd_x, args.cmd_y, args.cmd_yaw], dtype=np.float32) + else: + r = state.remote + ly = apply_deadzone(r.ly, args.rc_deadzone) + lx = apply_deadzone(r.lx, args.rc_deadzone) + rx = apply_deadzone(r.rx, args.rc_deadzone) + if args.swap_vy_yaw: + cmd = np.array([ + ly * args.rc_vx_scale, + -rx * args.rc_vy_scale, + -lx * args.rc_wz_scale, + ], dtype=np.float32) + else: + cmd = np.array([ + ly * args.rc_vx_scale, + -lx * args.rc_vy_scale, + -rx * args.rc_wz_scale, + ], dtype=np.float32) + + limits = np.array([MAX_LIN_VEL_X, MAX_LIN_VEL_Y, MAX_ANG_VEL_YAW], dtype=np.float32) + return np.clip(cmd, -limits, limits) + + +class CommandFilter: + def __init__(self, args): + self.alpha = float(args.cmd_ema_alpha) + self.max_step = np.array([ + args.max_cmd_step_x, + args.max_cmd_step_y, + args.max_cmd_step_yaw, + ], dtype=np.float32) + self.prev = np.zeros(3, dtype=np.float32) + + def reset(self): + self.prev[:] = 0.0 + + def update(self, raw_cmd): + cmd = np.asarray(raw_cmd, dtype=np.float32) + if 0.0 < self.alpha < 1.0: + cmd = self.alpha * cmd + (1.0 - self.alpha) * self.prev + if np.any(self.max_step > 0.0): + limit = np.where(self.max_step > 0.0, self.max_step, np.inf) + cmd = self.prev + np.clip(cmd - self.prev, -limit, limit) + self.prev = cmd.astype(np.float32) + return self.prev.copy() + + +class ObsHistoryBuilder: + """Build RoboGauge 45-dim obs and 450-dim RobotLab term-stacked policy history.""" + + def __init__(self): + self.history = deque(maxlen=HISTORY_LEN) + + def reset(self): + self.history.clear() + + def build_single(self, state, cmd, last_action): + quat = as_np(state.imu.quaternion) + gyro = as_np(state.imu.gyroscope) + q = motor_pos(state) + dq = motor_vel(state) + + obs = np.zeros(NUM_OBS, dtype=np.float32) + obs[0:3] = gyro * ANG_VEL_SCALE + obs[3:6] = get_projected_gravity(quat) + obs[6:9] = cmd * CMD_SCALE + obs[9:21] = q - DEFAULT_DOF_POS + obs[21:33] = dq * DOF_VEL_SCALE + obs[33:45] = last_action + obs = np.clip(obs, -CLIP_OBS, CLIP_OBS) + return np.nan_to_num(obs, nan=0.0, posinf=0.0, neginf=0.0) + + def build_policy_input(self, obs_single): + 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 = [3, 3, 3, 12, 12, 12] + chunks = [] + offset = 0 + for dim in term_dims: + for frame in frames: + chunks.append(frame[offset:offset + dim]) + offset += dim + obs = np.concatenate(chunks, dtype=np.float32).reshape(1, POLICY_INPUT_DIM) + return np.nan_to_num(obs, nan=0.0, posinf=0.0, neginf=0.0) + + +class RCEdgeDetector: + def __init__(self): + self._prev = set() + + def update(self, state): + current = set(state.remote.pressed) + rising = current - self._prev + falling = self._prev - current + self._prev = current + return rising, falling + + +class JsonlLogger: + def __init__(self, log_dir, args): + self.enabled = bool(log_dir) + self.fp = None + self.run_dir = None + self.flush_every = max(1, int(args.log_flush_every)) + if not self.enabled: + return + + ts = datetime.now().strftime("%Y%m%d_%H%M%S") + self.run_dir = Path(log_dir).expanduser().resolve() / f"robotlab_go1_deploy_{ts}" + self.run_dir.mkdir(parents=True, exist_ok=True) + meta = { + "created_at": ts, + "num_obs": NUM_OBS, + "history_len": HISTORY_LEN, + "policy_backend": BpuInferLibPolicy.backend_name, + "bpu_model": str(Path(args.bpu_model).expanduser()), + "policy_input_dim": POLICY_INPUT_DIM, + "bpu_input_shape": BPU_INPUT_SHAPE, + "bpu_output_shape": BPU_OUTPUT_SHAPE, + "action_scale": ACTION_SCALE, + "default_dof_pos": DEFAULT_DOF_POS.tolist(), + "joint_names_sdk": list(JOINT_NAMES), + "joint_names_sdk_expected": EXPECTED_SDK_JOINT_NAMES, + "joint_names_policy": POLICY_JOINT_NAMES, + "joint_order_policy": ["FR", "FL", "RR", "RL"], + "lowcmd_backend": LOWCMD_BACKEND, + } + for k, v in vars(args).items(): + if isinstance(v, (str, int, float, bool, type(None))): + meta[k] = v + (self.run_dir / "metadata.json").write_text(json.dumps(meta, indent=2, ensure_ascii=False)) + self.fp = open(self.run_dir / "steps.jsonl", "a", encoding="utf-8", buffering=1) + print(f"[INFO] Log dir: {self.run_dir}") + + def log(self, step, **kw): + if not self.enabled: + return + rec = {"step": int(step), "time_wall": time.time()} + for k, v in kw.items(): + if isinstance(v, np.ndarray): + 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)): + rec[k] = int(v) + else: + rec[k] = v + self.fp.write(json.dumps(rec, ensure_ascii=False) + "\n") + if step % self.flush_every == 0: + self.fp.flush() + + def close(self): + if self.fp: + self.fp.flush() + self.fp.close() + print(f"[INFO] Log saved: {self.run_dir}") + + +def fmt_rc(state): + r = state.remote + btns = ",".join(r.pressed) if r.pressed else "none" + return ( + f"lx={r.lx:+.2f} ly={r.ly:+.2f} rx={r.rx:+.2f} ry={r.ry:+.2f} " + f"L2={r.L2:.2f} btns={btns}" + ) + + +def get_fast_lowcmd_builder(): + global _FAST_LOW_CMD_BUILDER + if _FAST_LOW_CMD_BUILDER is None: + _FAST_LOW_CMD_BUILDER = FastLowCmdBuilder() + return _FAST_LOW_CMD_BUILDER + + +def send_damping(client): + raw = get_fast_lowcmd_builder().build_encrypted_damping() + client.send_raw(raw) + + +def build_servo12_raw(targets, kp, kd, state=None, position_protect_limit=None): + actual_q = None if state is None else motor_pos(state) + actual_tau = None if state is None else motor_tau(state) + pp_limit = 0.0 if position_protect_limit is None else float(position_protect_limit) + return get_fast_lowcmd_builder().build_encrypted_servo12_checked( + np.asarray(targets, dtype=np.float32), + float(kp), + float(kd), + actual_q=actual_q, + actual_tau=actual_tau, + position_protect_limit=pp_limit, + ) + + +def send_servo12_fast(client, targets, kp, kd, state=None, position_protect_limit=None): + raw = build_servo12_raw(targets, kp, kd, state, position_protect_limit) + client.send_raw(raw) + + +def send_hold_cmd(client, state, args): + send_servo12_fast(client, DEFAULT_DOF_POS, args.kp, args.kd, state=state) + + +def send_position_cmd(client, state, targets, args): + pp_limit = args.position_protect_limit if args.position_protect_limit > 0 else None + send_servo12_fast(client, targets, args.kp, args.kd, state=state, position_protect_limit=pp_limit) + + +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 + max_error = float(np.max(np.abs(error))) + print(f"[INFO] Current max default-pose error: {max_error:.3f} rad") + if max_error < 0.05: + print("[INFO] Already near default pose.") + return 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: + return state + new_state = client.recv_latest() + if new_state is not None: + state = new_state + + ratio = float(i + 1) / float(ramp_steps) + target = current + ratio * (DEFAULT_DOF_POS - current) + send_servo12_fast(client, target, args.kp_cal, args.kd_cal, state=state) + 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( + f" ramp {i:4d}/{ramp_steps} " + f"target_err={np.max(np.abs(target - DEFAULT_DOF_POS)):.3f} " + f"actual_err={np.max(np.abs(actual - DEFAULT_DOF_POS)):.3f}" + ) + next_t += dt + sleep = next_t - time.perf_counter() + if sleep > 0: + time.sleep(sleep) + else: + next_t = time.perf_counter() + + 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) + 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 + + +def state_ok(state, args): + q = motor_pos(state) + dq = motor_vel(state) + gyro = as_np(state.imu.gyroscope) + quat = as_np(state.imu.quaternion) + grav = get_projected_gravity(quat) + rpy_deg = np.degrees(as_np(state.imu.rpy)) + + checks = [ + (np.all(np.isfinite(q)), "joint position is non-finite"), + (np.all(np.isfinite(dq)), "joint velocity is non-finite"), + (np.all(np.isfinite(gyro)), "gyro is non-finite"), + (np.all(np.isfinite(quat)), "quaternion is non-finite"), + (0.5 <= np.linalg.norm(grav) <= 1.5, f"gravity norm={np.linalg.norm(grav):.3f}"), + (np.max(np.abs(dq)) <= args.max_dof_vel, f"max dof vel={np.max(np.abs(dq)):.2f}"), + (np.max(np.abs(gyro)) <= args.max_gyro, f"max gyro={np.max(np.abs(gyro)):.2f}"), + (abs(rpy_deg[0]) <= args.max_roll_deg, f"roll={rpy_deg[0]:.1f} deg"), + (abs(rpy_deg[1]) <= args.max_pitch_deg, f"pitch={rpy_deg[1]:.1f} deg"), + ] + for ok, reason in checks: + if not ok: + return False, reason + return True, "ok" + + +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 True, ( + f"action abs {max_abs:.2f} > soft trip {args.action_trip_limit:.2f}; " + f"clipped to {args.action_clip:.2f}" + ) + return True, "ok" + + +def clamp_action(action_raw, args): + return np.clip(action_raw, -args.action_clip, args.action_clip).astype(np.float32) + + +def smooth_action(action, prev_action, args): + alpha = float(args.action_ema_alpha) + if 0.0 < alpha < 1.0: + return (alpha * action + (1.0 - alpha) * prev_action).astype(np.float32) + return action.astype(np.float32) + + +def limit_target_step(target, prev_target, args): + limit = float(args.max_target_step) + if limit <= 0: + return target.astype(np.float32) + delta = np.clip(target - prev_target, -limit, limit) + return (prev_target + delta).astype(np.float32) + + +def kill_sport_processes(host, user): + cmds = [ + "sudo pkill -9 -f keep_sport_alive", + "sudo pkill -9 -f Legged_sport", + "sudo pkill -9 -f appTransit", + ] + ssh_target = f"{user}@{host}" if user else host + print(f"[INFO] Equivalent manual command: {SPORT_KILL_CMD}") + print(f"[INFO] Killing sport processes on {ssh_target}...") + try: + result = subprocess.run( + ["ssh", ssh_target, " && ".join(cmds)], + capture_output=True, + text=True, + timeout=15, + ) + if result.returncode == 0: + print("[INFO] Sport processes killed.") + return True + stderr = result.stderr.strip() + if "no process" in stderr.lower() or not stderr: + print("[INFO] No sport processes found.") + return True + print(f"[WARN] SSH returned {result.returncode}: {stderr}") + except FileNotFoundError: + print("[WARN] ssh command not found; kill sport processes manually.") + except subprocess.TimeoutExpired: + print("[WARN] SSH timed out; check Pi network.") + except Exception as exc: + print(f"[WARN] Failed to kill sport processes: {exc}") + return False + + +def connect_client(args): + validate_joint_order() + + if args.kill_sport: + kill_sport_processes(args.pi_host, args.pi_user) + + print(f"[INFO] LowCmd backend: {LOWCMD_BACKEND}") + print("[INFO] Connecting to MCU...") + client = MCUClient() + print("[INFO] Waking MCU...") + client.wake_mcu(n_frames=50, dt=0.01) + state = client.recv_state(timeout=2.0) + if state is None: + client.close() + raise RuntimeError("No LowState received. Check robot network and sport processes.") + + print(f"[INFO] Connected. Battery={state.bms.SOC}%") + print(f"[INFO] RPY deg: {np.round(np.degrees(as_np(state.imu.rpy)), 1)}") + print("[INFO] Initial joint positions (rad):") + q = motor_pos(state) + for i, name in enumerate(JOINT_NAMES): + print(f" [{i:02d}] {name:4s}: q={q[i]:+7.3f}, default={DEFAULT_DOF_POS[i]:+7.3f}") + return client, state + + +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): + timing = {} if timing is None else timing + logger.log( + step, + mode=mode, + battery_soc=state.bms.SOC, + rc_lx=state.remote.lx, + rc_ly=state.remote.ly, + rc_rx=state.remote.rx, + rc_ry=state.remote.ry, + rc_buttons=state.remote.pressed, + imu_rpy_deg=np.degrees(as_np(state.imu.rpy)), + imu_quat=as_np(state.imu.quaternion), + base_ang_vel=as_np(state.imu.gyroscope), + projected_gravity=get_projected_gravity(as_np(state.imu.quaternion)), + 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, + action_raw=np.zeros(NUM_ACTIONS, dtype=np.float32) if action_raw is None else action_raw, + action_safe=np.zeros(NUM_ACTIONS, dtype=np.float32) if action_safe is None else action_safe, + joint_targets=np.zeros(NUM_ACTIONS, dtype=np.float32) if target is None else target, + state_reason=state_reason, + loop_dt_ms=float(timing.get("loop_dt_ms", 0.0)), + recv_ms=float(timing.get("recv_ms", 0.0)), + policy_ms=float(timing.get("policy_ms", 0.0)), + work_ms=float(timing.get("work_ms", 0.0)), + ) + + +def run_monitor(args): + print(MONITOR_BANNER) + input("Press Enter to start monitor...") + logger = JsonlLogger(args.log_dir, args) + client = None + try: + client, state = connect_client(args) + edge = RCEdgeDetector() + edge.update(state) + step = 0 + dt = 1.0 / args.rate_hz + next_t = time.perf_counter() + while not EXIT: + new_state = client.recv_latest() + if new_state is not None: + state = new_state + rising, falling = edge.update(state) + if step % args.print_every == 0: + rpy = np.degrees(as_np(state.imu.rpy)) + print(f"\n[MONITOR {step}] bat={state.bms.SOC}% rpy={np.round(rpy, 1)}") + print(f" RC: {fmt_rc(state)}") + if rising: + print(f" rising: {sorted(rising)}") + if falling: + print(f" falling: {sorted(falling)}") + print(f" q: {np.round(motor_pos(state), 3)}") + print(f" dq: {np.round(motor_vel(state), 3)}") + log_state(logger, step, "MONITOR", state) + step += 1 + if args.max_steps > 0 and step >= args.max_steps: + break + next_t += dt + sleep = next_t - time.perf_counter() + if sleep > 0: + time.sleep(sleep) + else: + next_t = time.perf_counter() + finally: + logger.close() + if client is not None: + client.close() + + +def run_obs_check(args): + print(OBS_BANNER) + input("Press Enter to start obs-check...") + logger = JsonlLogger(args.log_dir, args) + client = None + try: + client, state = connect_client(args) + obs_builder = ObsHistoryBuilder() + cmd_filter = CommandFilter(args) + last_action = np.zeros(NUM_ACTIONS, dtype=np.float32) + step = 0 + dt = 1.0 / args.rate_hz + next_t = time.perf_counter() + while not EXIT: + new_state = client.recv_latest() + if new_state is not None: + state = new_state + cmd_raw = get_command(state, args) + cmd = cmd_filter.update(cmd_raw) + obs_single = obs_builder.build_single(state, cmd, last_action) + policy_input = obs_builder.build_policy_input(obs_single) + if step % args.print_every == 0: + print(f"\n[OBS {step}] bat={state.bms.SOC}% {fmt_rc(state)}") + print(f" obs[0:3] gyro {np.round(obs_single[0:3], 4)}") + print(f" obs[3:6] gravity {np.round(obs_single[3:6], 4)}") + print(f" obs[6:9] cmd {np.round(obs_single[6:9], 4)} raw={np.round(cmd, 3)}") + print(f" obs[9:21] q-qd max={np.max(np.abs(obs_single[9:21])):.3f}") + print(f" obs[21:33] dq max={np.max(np.abs(obs_single[21:33])):.3f}") + print(f" policy_input shape={policy_input.shape} min={policy_input.min():.3f} max={policy_input.max():.3f}") + log_state(logger, step, "OBS_CHECK", state, cmd=cmd, cmd_raw=cmd_raw, obs_single=obs_single) + step += 1 + if args.max_steps > 0 and step >= args.max_steps: + break + next_t += dt + sleep = next_t - time.perf_counter() + if sleep > 0: + time.sleep(sleep) + else: + next_t = time.perf_counter() + finally: + logger.close() + if client is not None: + client.close() + + +def run_infer_check(args): + print(INFER_BANNER) + input("Press Enter to start infer-check...") + logger = JsonlLogger(args.log_dir, args) + client = None + try: + policy = BpuInferLibPolicy(args.bpu_model, priority=args.bpu_priority, bpu_cores=args.bpu_cores) + client, state = connect_client(args) + obs_builder = ObsHistoryBuilder() + cmd_filter = CommandFilter(args) + last_action = np.zeros(NUM_ACTIONS, dtype=np.float32) + prev_action = np.zeros(NUM_ACTIONS, dtype=np.float32) + step = 0 + dt = 1.0 / args.rate_hz + next_t = time.perf_counter() + while not EXIT: + new_state = client.recv_latest() + if new_state is not None: + state = new_state + cmd_raw = get_command(state, args) + cmd = cmd_filter.update(cmd_raw) + obs_single = obs_builder.build_single(state, cmd, last_action) + policy_input = obs_builder.build_policy_input(obs_single) + action_raw = policy(policy_input) + ok, reason = action_ok(action_raw, args) + action_safe = smooth_action(clamp_action(action_raw, args), prev_action, args) + prev_action = action_safe.copy() + last_action = action_safe.copy() + if step % args.print_every == 0: + print(f"\n[INFER {step}] ok={ok} reason={reason}") + print(f" cmd={np.round(cmd, 3)} action_raw={np.round(action_raw[:4], 3)} max={np.max(np.abs(action_raw)):.3f}") + print(f" action_safe max={np.max(np.abs(action_safe)):.3f}") + log_state( + logger, step, "INFER_CHECK", state, cmd=cmd, obs_single=obs_single, + cmd_raw=cmd_raw, + action_raw=action_raw, action_safe=action_safe, state_reason=reason, + ) + if not ok and args.trip_on_infer_check: + print(f"[FAULT] {reason}") + break + step += 1 + if args.max_steps > 0 and step >= args.max_steps: + break + next_t += dt + sleep = next_t - time.perf_counter() + if sleep > 0: + time.sleep(sleep) + else: + next_t = time.perf_counter() + finally: + logger.close() + if client is not None: + client.close() + + +STARTUP_BANNER = """ +============================================================ + RoboGauge Go1 RobotLab BPU deployment + + State machine: + IDLE --R2--> CALIBRATE --> HOLD --R2--> OBS_TEST + OBS_TEST --R2--> INFER_TEST --R2--> RL + Any active state --L2--> IDLE damping + + RL motor commands require --enable-rl. Without it, R2 at INFER_TEST + will stay in INFER_TEST. + + Initial tests should be done with the robot suspended. + Use --kill-sport to run the Pi sport-process kill step before MCU control. + Manual equivalent: + ssh pi@192.168.123.161 "sudo pkill -9 -f keep_sport_alive; sudo pkill -9 -f Legged_sport; sudo pkill -9 -f appTransit" + After sport processes are killed, keep battery removal available as + the final stop method; the original sport-mode remote combo is not active. +============================================================ +""" + +MONITOR_BANNER = """ +============================================================ + MONITOR: read RC, IMU, and joint state only. No motor command. +============================================================ +""" + +OBS_BANNER = """ +============================================================ + OBS-CHECK: build 45-dim obs and 450-dim RobotLab history only. + No motor command. +============================================================ +""" + +INFER_BANNER = """ +============================================================ + INFER-CHECK: build obs and run BPU only. + No motor command. +============================================================ +""" + + +def run_deploy(args): + print(STARTUP_BANNER) + input("Press Enter when ready...") + + policy = BpuInferLibPolicy(args.bpu_model, priority=args.bpu_priority, bpu_cores=args.bpu_cores) + logger = JsonlLogger(args.log_dir, args) + client = None + state = None + sm_state = State.IDLE + step = 0 + cmd_raw = np.zeros(3, dtype=np.float32) + cmd = np.zeros(3, dtype=np.float32) + obs_single = None + action_raw = np.zeros(NUM_ACTIONS, dtype=np.float32) + action_safe = np.zeros(NUM_ACTIONS, dtype=np.float32) + target = DEFAULT_DOF_POS.copy() + reason = "ok" + + try: + client, state = connect_client(args) + edge = RCEdgeDetector() + 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) + prev_action = np.zeros(NUM_ACTIONS, dtype=np.float32) + prev_target = DEFAULT_DOF_POS.copy() + step = 0 + rl_step = 0 + dt = 1.0 / args.rate_hz + next_t = time.perf_counter() + prev_loop_t = None + + print("[INFO] R2 advances layers. L2 emergency-stops to IDLE.") + print("[INFO] Ctrl+C exits with safe_stop.") + + while not EXIT: + loop_t0 = time.perf_counter() + timing = None + if args.log_timing: + timing = { + "loop_dt_ms": 0.0 if prev_loop_t is None else (loop_t0 - prev_loop_t) * 1000.0, + "recv_ms": 0.0, + "policy_ms": 0.0, + "work_ms": 0.0, + } + prev_loop_t = loop_t0 + + recv_t0 = time.perf_counter() + new_state = client.recv_latest() + if timing is not None: + timing["recv_ms"] = (time.perf_counter() - recv_t0) * 1000.0 + if new_state is not None: + state = new_state + if state is None: + time.sleep(0.001) + continue + + rising, _ = edge.update(state) + r2_rose = "R2" in rising + l2_rose = "L2" in rising + + if l2_rose and sm_state != State.IDLE: + print(f"\n[L2] {sm_state.value} -> IDLE damping") + sm_state = State.IDLE + obs_builder.reset() + cmd_filter.reset() + last_action[:] = 0.0 + 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) + else: + cmd_filter.reset() + cmd = cmd_raw + obs_single = None + action_raw = np.zeros(NUM_ACTIONS, dtype=np.float32) + action_safe = np.zeros(NUM_ACTIONS, dtype=np.float32) + target = DEFAULT_DOF_POS.copy() + reason = "ok" + + if sm_state == State.IDLE: + if step % 10 == 0: + send_damping(client) + if r2_rose: + print("\n[R2] IDLE -> CALIBRATE") + sm_state = State.CALIBRATE + state = ramp_to_default(client, args, state, logger=logger, step_base=step) + obs_builder.reset() + cmd_filter.reset() + last_action[:] = 0.0 + prev_action[:] = 0.0 + prev_target = DEFAULT_DOF_POS.copy() + sm_state = State.HOLD + print("[STATE] HOLD") + + elif sm_state == State.HOLD: + send_hold_cmd(client, state, args) + if r2_rose: + 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_policy_input(obs_single) + 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() + cmd_filter.reset() + last_action[:] = 0.0 + prev_action[:] = 0.0 + sm_state = State.INFER_TEST + + elif sm_state == State.INFER_TEST: + send_hold_cmd(client, state, args) + obs_single = obs_builder.build_single(state, cmd, last_action) + policy_input = obs_builder.build_policy_input(obs_single) + if servo_fault: + ok, reason = False, servo_fault + else: + ok, reason = state_ok(state, args) + if ok: + policy_t0 = time.perf_counter() + action_raw = policy(policy_input) + if timing is not None: + timing["policy_ms"] += (time.perf_counter() - policy_t0) * 1000.0 + ok, reason = action_ok(action_raw, args) + if ok: + action_safe = smooth_action(clamp_action(action_raw, args), prev_action, args) + prev_action = action_safe.copy() + last_action = action_safe.copy() + else: + print(f"\n[FAULT] INFER_TEST failed: {reason}") + sm_state = State.FAULT + + if r2_rose and sm_state == State.INFER_TEST: + if not args.enable_rl: + print("\n[GUARD] RL blocked. Re-run with --enable-rl after OBS/INFER logs look safe.") + else: + print("\n[R2] INFER_TEST -> RL") + obs_builder.reset() + cmd_filter.reset() + last_action[:] = 0.0 + prev_action[:] = 0.0 + prev_target = DEFAULT_DOF_POS.copy() + rl_step = 0 + sm_state = State.RL + + elif sm_state == State.RL: + if r2_rose: + print("\n[R2] RL -> HOLD") + sm_state = State.HOLD + obs_builder.reset() + cmd_filter.reset() + last_action[:] = 0.0 + prev_action[:] = 0.0 + 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) + policy_input = obs_builder.build_policy_input(obs_single) + if servo_fault: + ok, reason = False, servo_fault + else: + ok, reason = state_ok(state, args) + if ok: + policy_t0 = time.perf_counter() + action_raw = policy(policy_input) + if timing is not None: + timing["policy_ms"] += (time.perf_counter() - policy_t0) * 1000.0 + ok, reason = action_ok(action_raw, args) + if not ok: + print(f"\n[FAULT] RL failed: {reason}") + sm_state = State.FAULT + send_damping(client) + else: + action_clipped = clamp_action(action_raw, args) + action_safe = smooth_action(action_clipped, prev_action, args) + target_raw = DEFAULT_DOF_POS + action_safe * ACTION_SCALE + target = limit_target_step(target_raw, prev_target, args) + prev_action = action_safe.copy() + last_action = action_safe.copy() + if rl_step >= args.warmup_steps: + prev_target = target.copy() + send_position_cmd(client, state, target, args) + else: + target = DEFAULT_DOF_POS.copy() + prev_target = DEFAULT_DOF_POS.copy() + send_hold_cmd(client, state, args) + rl_step += 1 + + elif sm_state == State.FAULT: + send_damping(client) + obs_builder.reset() + cmd_filter.reset() + last_action[:] = 0.0 + prev_action[:] = 0.0 + if r2_rose: + print("\n[R2] FAULT -> CALIBRATE") + sm_state = State.CALIBRATE + 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") + + if timing is not None: + timing["work_ms"] = (time.perf_counter() - loop_t0) * 1000.0 + + log_state( + logger, + step, + sm_state.value, + state, + cmd=cmd, + cmd_raw=cmd_raw, + obs_single=obs_single, + action_raw=action_raw, + action_safe=action_safe, + target=target, + state_reason=reason, + timing=timing, + ) + + if step % args.print_every == 0: + rpy = np.degrees(as_np(state.imu.rpy)) + print( + f"\n[STEP {step}] state={sm_state.value} bat={state.bms.SOC}% " + f"rpy={np.round(rpy, 1)}" + ) + 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} " + f"action_safe_max={np.max(np.abs(action_safe)):.3f} reason={reason}" + ) + if sm_state == State.RL: + print(f" target={np.round(target, 2)} rl_step={rl_step}") + + step += 1 + if args.max_steps > 0 and step >= args.max_steps: + print("[INFO] max_steps reached.") + break + + next_t += dt + sleep = next_t - time.perf_counter() + if sleep > 0: + time.sleep(sleep) + else: + next_t = time.perf_counter() + + except PowerProtectViolation as exc: + reason = f"power protect: {exc}" + print(f"\n[FAULT] {reason}") + if client is not None: + send_damping(client) + if state is not None: + log_state( + logger, + step, + State.FAULT.value, + state, + cmd=cmd, + cmd_raw=cmd_raw, + obs_single=obs_single, + action_raw=action_raw, + action_safe=action_safe, + target=target, + state_reason=reason, + ) + + finally: + logger.close() + if client is not None: + print("[INFO] Safe stopping...") + client.safe_stop(n_frames=50, dt=0.002) + client.close() + print("[INFO] Done.") + + +def build_arg_parser(): + parser = argparse.ArgumentParser(description="Deploy RoboGauge Go1 RobotLab BPU on Go1 PRO") + parser.add_argument("--bpu-model", default=str(DEFAULT_BPU_MODEL), + help="Path to policy_robotlab_6500_int16_gemm.bin") + parser.add_argument("--bpu-priority", type=int, default=0) + parser.add_argument("--bpu-cores", type=int, nargs="+", default=[0], + help="Reserved BPU core ids for future runtime scheduling") + + parser.add_argument("--kill-sport", action="store_true", help="Kill Pi sport processes via SSH") + parser.add_argument("--pi-host", default="192.168.123.161") + parser.add_argument("--pi-user", default="pi") + + parser.add_argument("--monitor", action="store_true", help="Read RC/IMU/joints only; no motor command") + parser.add_argument("--obs-check", action="store_true", help="Build obs/history only; no motor command") + parser.add_argument("--infer-check", action="store_true", help="Run BPU inference only; no motor command") + parser.add_argument("--enable-rl", action="store_true", help="Allow state machine to enter RL motor-control state") + parser.add_argument("--no-rc", action="store_true", help="Use fixed --cmd-* instead of RC sticks") + parser.add_argument("--swap-vy-yaw", action="store_true", help="Map left stick x to yaw and right stick x to vy") + + parser.add_argument("--kp", type=float, default=28.0) + parser.add_argument("--kd", type=float, default=0.7) + parser.add_argument("--kp-cal", type=float, default=20.0) + parser.add_argument("--kd-cal", type=float, default=1.0) + parser.add_argument("--power-factor", type=int, default=7) + parser.add_argument("--position-protect-limit", type=float, default=0.0) + + parser.add_argument("--rc-vx-scale", type=float, default=MAX_LIN_VEL_X) + parser.add_argument("--rc-vy-scale", type=float, default=MAX_LIN_VEL_Y) + parser.add_argument("--rc-wz-scale", type=float, default=MAX_ANG_VEL_YAW) + parser.add_argument("--rc-deadzone", type=float, default=0.05) + parser.add_argument("--cmd-ema-alpha", type=float, default=1.0) + parser.add_argument("--max-cmd-step-x", type=float, default=0.0) + parser.add_argument("--max-cmd-step-y", type=float, default=0.0) + parser.add_argument("--max-cmd-step-yaw", type=float, default=0.0) + parser.add_argument("--cmd-x", type=float, default=0.0) + parser.add_argument("--cmd-y", type=float, default=0.0) + 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=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) + + parser.add_argument("--max-roll-deg", type=float, default=35.0) + 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, + 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) + parser.add_argument("--trip-on-infer-check", action="store_true") + + parser.add_argument("--log-dir", default=str(DEPLOY_ROOT / "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") + parser.add_argument("--log-timing", action="store_true", + help="Log per-loop timing fields for deploy-mode latency diagnosis") + return parser + + +def main(): + args = build_arg_parser().parse_args() + if args.monitor: + return run_monitor(args) + if args.obs_check: + return run_obs_check(args) + if args.infer_check: + return run_infer_check(args) + return run_deploy(args) + + +if __name__ == "__main__": + main() diff --git a/deploy_45dim_rl_gym/bpu_deploy_x5/test_bpu_policy.py b/deploy_45dim_rl_gym/bpu_deploy_x5/test_bpu_policy.py new file mode 100644 index 0000000..3adfd92 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_deploy_x5/test_bpu_policy.py @@ -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() diff --git a/deploy_45dim_rl_gym/bpu_quantization/README.md b/deploy_45dim_rl_gym/bpu_quantization/README.md new file mode 100644 index 0000000..21df170 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_quantization/README.md @@ -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 真实日志对比、悬空状态机测试，再进入地面低速测试。 diff --git a/deploy_45dim_rl_gym/bpu_quantization/calibration_data_metadata.json b/deploy_45dim_rl_gym/bpu_quantization/calibration_data_metadata.json new file mode 100644 index 0000000..400097b --- /dev/null +++ b/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 + } + ] +} diff --git a/deploy_45dim_rl_gym/bpu_quantization/compare_4d_onnx.py b/deploy_45dim_rl_gym/bpu_quantization/compare_4d_onnx.py new file mode 100644 index 0000000..906c6ad --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_quantization/compare_4d_onnx.py @@ -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() diff --git a/deploy_45dim_rl_gym/bpu_quantization/convert_opset.py b/deploy_45dim_rl_gym/bpu_quantization/convert_opset.py new file mode 100644 index 0000000..6172b32 --- /dev/null +++ b/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() diff --git a/deploy_45dim_rl_gym/bpu_quantization/downgrade_policy_to_opset11.py b/deploy_45dim_rl_gym/bpu_quantization/downgrade_policy_to_opset11.py new file mode 100644 index 0000000..711af1b --- /dev/null +++ b/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() diff --git a/deploy_45dim_rl_gym/bpu_quantization/make_bpu_4d_onnx.py b/deploy_45dim_rl_gym/bpu_quantization/make_bpu_4d_onnx.py new file mode 100644 index 0000000..9bbe004 --- /dev/null +++ b/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() diff --git a/deploy_45dim_rl_gym/bpu_quantization/make_calibration_data.py b/deploy_45dim_rl_gym/bpu_quantization/make_calibration_data.py new file mode 100644 index 0000000..cc0cc04 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_quantization/make_calibration_data.py @@ -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() diff --git a/deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_int16.yaml b/deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_int16.yaml new file mode 100644 index 0000000..a50f115 --- /dev/null +++ b/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" diff --git a/deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_int16_gemm.yaml b/deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_int16_gemm.yaml new file mode 100644 index 0000000..b8b0e77 --- /dev/null +++ b/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" diff --git a/deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_6500_int16_gemm.yaml b/deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_6500_int16_gemm.yaml new file mode 100644 index 0000000..f4aa2d6 --- /dev/null +++ b/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" diff --git a/deploy_45dim_rl_gym/bpu_quantization/replace_group_conv_with_gemm.py b/deploy_45dim_rl_gym/bpu_quantization/replace_group_conv_with_gemm.py new file mode 100644 index 0000000..03970b3 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_quantization/replace_group_conv_with_gemm.py @@ -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() diff --git a/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab_fastcpp.py b/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab_fastcpp.py index 93239ab..2e96cbf 100644 --- a/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab_fastcpp.py +++ b/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk_lab_fastcpp.py @@ -62,7 +62,7 @@ except ImportError as exc: "PYTHONPATH=/root/go1_pro_sdk python3 setup.py build_ext --inplace" ) from exc -DEFAULT_ONNX = HERE / "policy_robotlab_15000.onnx" +DEFAULT_ONNX = HERE / "policy_robotlab_6500.onnx" LOWCMD_BACKEND = "cpp_checked_servo12" SPORT_KILL_CMD = ( 'ssh pi@192.168.123.161 "sudo pkill -9 -f keep_sport_alive; ' @@ -271,9 +271,15 @@ class ObsHistoryBuilder: def __init__(self): 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): 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): quat = as_np(state.imu.quaternion) @@ -291,9 +297,18 @@ class ObsHistoryBuilder: obs = np.clip(obs, -CLIP_OBS, CLIP_OBS) 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_time.append(float(now)) 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: 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) 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: def __init__(self, onnx_path): @@ -361,6 +384,7 @@ class JsonlLogger: "joint_names_policy": POLICY_JOINT_NAMES, "joint_order_policy": ["FR", "FL", "RR", "RL"], "lowcmd_backend": LOWCMD_BACKEND, + "history_expected_span_ms": (HISTORY_LEN - 1) * 20.0, } for k, v in vars(args).items(): 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, - 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 + history = {} if history is None else history logger.log( step, 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)), policy_ms=float(timing.get("policy_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 next_t = time.perf_counter() prev_loop_t = None + last_state_t = time.perf_counter() print("[INFO] R2 advances layers. L2 emergency-stops to IDLE.") print("[INFO] Ctrl+C exits with safe_stop.") @@ -893,6 +925,8 @@ def run_deploy(args): "recv_ms": 0.0, "policy_ms": 0.0, "work_ms": 0.0, + "state_fresh": False, + "state_age_ms": 0.0, } prev_loop_t = loop_t0 @@ -902,9 +936,14 @@ def run_deploy(args): timing["recv_ms"] = (time.perf_counter() - recv_t0) * 1000.0 if new_state is not None: state = new_state + last_state_t = loop_t0 + if timing is not None: + timing["state_fresh"] = True if state is None: time.sleep(0.001) continue + if timing is not None: + timing["state_age_ms"] = (loop_t0 - last_state_t) * 1000.0 rising, _ = edge.update(state) r2_rose = "R2" in rising @@ -974,7 +1013,7 @@ def run_deploy(args): 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) + obs_builder.build_onnx_input(obs_single, now=loop_t0) if servo_fault: ok, reason = False, servo_fault else: @@ -994,7 +1033,7 @@ def run_deploy(args): elif sm_state == State.INFER_TEST: 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) + onnx_input = obs_builder.build_onnx_input(obs_single, now=loop_t0) if servo_fault: ok, reason = False, servo_fault else: @@ -1040,7 +1079,7 @@ def run_deploy(args): continue 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: ok, reason = False, servo_fault else: @@ -1101,6 +1140,7 @@ def run_deploy(args): target=target, state_reason=reason, timing=timing, + history=obs_builder.stats(), ) 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_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: print(f" target={np.round(target, 2)} rl_step={rl_step}")