diff --git a/.gitignore b/.gitignore index 127215e..6e37921 100644 --- a/.gitignore +++ b/.gitignore @@ -5,10 +5,18 @@ __pycache__/ deploy_45dim_rl_gym/bpu_quantization/.hb_check/ deploy_45dim_rl_gym/bpu_quantization/calibration_data/ deploy_45dim_rl_gym/bpu_quantization/calibration_data_fast*/ +deploy_45dim_rl_gym/bpu_quantization/calibration_data_*_gym_fast*/ +deploy_45dim_rl_gym/bpu_quantization/calibration_data_*_gym_fast*_metadata.json deploy_45dim_rl_gym/bpu_quantization/mapper_output/ deploy_45dim_rl_gym/bpu_quantization/mapper_output_gemm/ +deploy_45dim_rl_gym/bpu_quantization/mapper_output_*_gemm/ deploy_45dim_rl_gym/bpu_quantization/mapper_output_6500_gemm/ deploy_45dim_rl_gym/bpu_quantization/hb_mapper_*.log* +deploy_45dim_rl_gym/bpu_quantization/policy_[0-9]*_actions.onnx +deploy_45dim_rl_gym/bpu_quantization/policy_[0-9]*_bpu4d.onnx +deploy_45dim_rl_gym/bpu_quantization/policy_[0-9]*_bpu4d_gemm.onnx +deploy_45dim_rl_gym/bpu_quantization/policy_[0-9]*_int16_gemm.yaml +deploy_45dim_rl_gym/bpu_quantization/policy_[0-9]*_opset11.onnx deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_bpu4d.onnx deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_bpu4d_gemm.onnx deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_15000_opset11.onnx diff --git a/deploy_45dim_rl_gym/README.md b/deploy_45dim_rl_gym/README.md index 55acc77..c4e9e68 100644 --- a/deploy_45dim_rl_gym/README.md +++ b/deploy_45dim_rl_gym/README.md @@ -8,6 +8,7 @@ - `deploy_go1_rlgym_pro_sdk_fastcpp.py`：真机 Go1 PRO 低层部署脚本，独立使用 C++ LowCmd 构包/加密后端 - `deploy_go1_rlgym_pro_sdk_lab.py`：RobotLab 策略真机部署脚本 - `deploy_go1_rlgym_pro_sdk_lab_fastcpp.py`：RobotLab 策略真机部署脚本，独立使用 C++ LowCmd 构包/加密后端 +- `deploy_go1_rlgym_bpu_x5_fastcpp.py`：Gym 5 帧 BPU 真机部署脚本，使用 C++ LowCmd/LowState 热路径 该 ONNX 策略使用 45 维单帧观测，以及 5 帧、共 225 维的历史输入，按观测项分组堆叠： @@ -52,6 +53,14 @@ cd /root/go1_pro_sdk/fast_lowcmd_cpp PYTHONPATH=/root/go1_pro_sdk python3 setup.py build_ext --inplace ``` +Gym 5 帧 BPU 部署入口默认走 `policy_35k` 对应的 `mapper_output_35k_gemm/policy_35k_int16_gemm.bin`， +也可以显式切换： + +```bash +PYTHONPATH=/root/go1_pro_deploy python3 deploy_45dim_rl_gym/deploy_go1_rlgym_bpu_x5_fastcpp.py --bpu-round 35k +PYTHONPATH=/root/go1_pro_deploy python3 deploy_45dim_rl_gym/deploy_go1_rlgym_bpu_x5_fastcpp.py --bpu-round 30k +``` + ## 安全说明 直接低层控电机是危险操作。 diff --git a/deploy_45dim_rl_gym/bpu_deploy_x5/bpu_policy.py b/deploy_45dim_rl_gym/bpu_deploy_x5/bpu_policy.py index ceea632..00d1ffd 100644 --- a/deploy_45dim_rl_gym/bpu_deploy_x5/bpu_policy.py +++ b/deploy_45dim_rl_gym/bpu_deploy_x5/bpu_policy.py @@ -59,8 +59,6 @@ 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() @@ -94,6 +92,10 @@ class BpuInferLibPolicy: self._lib.rlgym_bpu_infer.restype = ctypes.c_int self._lib.rlgym_bpu_destroy.argtypes = [ctypes.c_void_p] self._lib.rlgym_bpu_destroy.restype = None + self._lib.rlgym_bpu_input_floats.argtypes = [ctypes.c_void_p] + self._lib.rlgym_bpu_input_floats.restype = ctypes.c_int + self._lib.rlgym_bpu_output_floats.argtypes = [ctypes.c_void_p] + self._lib.rlgym_bpu_output_floats.restype = ctypes.c_int self._lib.rlgym_bpu_version.argtypes = [] self._lib.rlgym_bpu_version.restype = ctypes.c_char_p @@ -107,7 +109,13 @@ class BpuInferLibPolicy: ) if not self._handle: raise RuntimeError(err.value.decode("utf-8", errors="replace")) - self._output = np.empty(12, dtype=np.float32) + self.input_size = int(self._lib.rlgym_bpu_input_floats(self._handle)) + self.output_size = int(self._lib.rlgym_bpu_output_floats(self._handle)) + if self.input_size <= 0 or self.output_size <= 0: + raise RuntimeError("invalid BPU tensor sizes reported by runtime") + self.input_shape = (1, 1, 1, self.input_size) + self.output_shape = (1, self.output_size, 1, 1) + self._output = np.empty(self.output_size, dtype=np.float32) self._output_ptr = self._output.ctypes.data_as(ctypes.POINTER(ctypes.c_float)) self._err = ctypes.create_string_buffer(1024) @@ -131,8 +139,8 @@ class BpuInferLibPolicy: 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") + if arr.size != self.input_size: + raise ValueError(f"BPU policy input has {arr.size} values, expected {self.input_size}") input_flat = np.ascontiguousarray(arr.reshape(-1), dtype=np.float32) rc = self._lib.rlgym_bpu_infer( self._handle, @@ -172,6 +180,9 @@ class BpuInferLibPythonPolicy: self.priority = int(priority) self.bpu_cores = tuple(int(core) for core in bpu_cores) self.suppress_runtime_output = bool(suppress_runtime_output) + self.input_size = 450 + self.input_shape = (1, 1, 1, self.input_size) + self.output_shape = (1, 12, 1, 1) with suppress_c_output(self.suppress_runtime_output): self.infer = Infer(False) loaded = self.infer.load_model(str(self.model_path)) @@ -185,8 +196,8 @@ class BpuInferLibPythonPolicy: 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") + if arr.size != self.input_size: + raise ValueError(f"BPU policy input has {arr.size} values, expected {self.input_size}") input_4d = np.ascontiguousarray(arr.reshape(self.input_shape), dtype=np.float32) with suppress_c_output(self.suppress_runtime_output): copied = self.infer.read_numpy_arr_float32(input_4d, 0) 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 index 4c5daa3..86ba76c 100644 --- 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 @@ -12,8 +12,10 @@ 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 rlgym_bpu_infer(void *handle, const float *input, float *output, char *err, int err_len); +int rlgym_bpu_input_floats(void *handle); +int rlgym_bpu_output_floats(void *handle); void rlgym_bpu_destroy(void *handle); } @@ -54,12 +56,6 @@ int main(int argc, char **argv) { 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) { @@ -67,20 +63,37 @@ int main(int argc, char **argv) { return 3; } - float out[12] = {}; - if (rlgym_bpu_infer(handle, obs.data(), out, err, sizeof(err)) != 0) { + const int input_floats = rlgym_bpu_input_floats(handle); + const int output_floats = rlgym_bpu_output_floats(handle); + if (input_floats <= 0 || output_floats <= 0) { + std::cerr << "invalid tensor sizes from BPU runtime\n"; + rlgym_bpu_destroy(handle); + return 2; + } + + std::vector obs; + if (!read_f32_file(input, &obs) || static_cast(obs.size()) != input_floats) { + std::cerr << "failed to read " << input_floats << " float32 input: " << input << "\n"; + rlgym_bpu_destroy(handle); + return 2; + } + + std::vector out(static_cast(output_floats), 0.0f); + if (rlgym_bpu_infer(handle, obs.data(), out.data(), err, sizeof(err)) != 0) { std::cerr << err << "\n"; rlgym_bpu_destroy(handle); return 4; } float max_diff = 0.0f; - for (int i = 0; i < 12; ++i) { - max_diff = std::max(max_diff, std::fabs(out[i] - kReference[i])); + if (input_floats == 450 && output_floats == 12) { + for (int i = 0; i < 12; ++i) { + max_diff = std::max(max_diff, std::fabs(out[static_cast(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) { + if (rlgym_bpu_infer(handle, obs.data(), out.data(), err, sizeof(err)) != 0) { std::cerr << err << "\n"; rlgym_bpu_destroy(handle); return 5; @@ -90,10 +103,13 @@ int main(int argc, char **argv) { const double elapsed_ms = std::chrono::duration(t1 - t0).count(); - std::cout << "reference_max_abs_diff " << max_diff << "\n"; + std::cout << "input_floats=" << input_floats << " output_floats=" << output_floats << "\n"; + if (input_floats == 450 && output_floats == 12) { + std::cout << "reference_max_abs_diff " << max_diff << "\n"; + } std::cout << "repeat=" << repeat << " cpp_avg_ms=" << (elapsed_ms / repeat) << "\n"; std::cout << "action0=" << out[0] << " action_max_abs=" - << *std::max_element(out, out + 12, [](float a, float b) { + << *std::max_element(out.begin(), out.end(), [](float a, float b) { return std::fabs(a) < std::fabs(b); }) << "\n"; 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 index 70bfaf3..e565f9f 100644 --- 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 @@ -13,7 +13,6 @@ namespace { -constexpr int kInputFloats = 450; constexpr int kOutputFloats = 12; void set_error(char *err, int err_len, const std::string &msg) { @@ -88,10 +87,10 @@ class BpuDnnPolicy { 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"); + input_floats_ = element_count(input_tensors_[0].properties.validShape); + output_floats_ = element_count(output_tensors_[0].properties.validShape); + validate_tensor(input_tensors_[0].properties, HB_DNN_TENSOR_TYPE_F32, "input"); + validate_tensor(output_tensors_[0].properties, HB_DNN_TENSOR_TYPE_F32, "output"); alloc_tensor_mem(input_tensors_[0]); alloc_tensor_mem(output_tensors_[0]); @@ -112,7 +111,7 @@ class BpuDnnPolicy { } auto &input_mem = input_tensors_[0].sysMem[0]; - std::memcpy(input_mem.virAddr, input, sizeof(float) * kInputFloats); + std::memcpy(input_mem.virAddr, input, sizeof(float) * input_floats_); int code = hbSysFlushMem(&input_mem, HB_SYS_MEM_CACHE_CLEAN); if (code != 0) { throw std::runtime_error(dnn_error("hbSysFlushMem(input)", code)); @@ -137,18 +136,15 @@ class BpuDnnPolicy { if (code != 0) { throw std::runtime_error(dnn_error("hbSysFlushMem(output)", code)); } - std::memcpy(output, output_mem.virAddr, sizeof(float) * kOutputFloats); + std::memcpy(output, output_mem.virAddr, sizeof(float) * output_floats_); } + int input_floats() const { return input_floats_; } + int output_floats() const { return output_floats_; } + private: - static void validate_tensor(const hbDNNTensorProperties &props, int expected_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()); - } + static void validate_tensor(const hbDNNTensorProperties &props, int expected_type, + const char *name) { if (props.tensorType != expected_type) { std::ostringstream oss; oss << name << " tensor type " << props.tensorType << " != " << expected_type; @@ -182,6 +178,8 @@ class BpuDnnPolicy { std::string model_name_; std::vector input_tensors_; std::vector output_tensors_; + int input_floats_{0}; + int output_floats_{0}; int bpu_core_{HB_BPU_CORE_ANY}; int priority_{HB_DNN_PRIORITY_LOWEST}; }; @@ -201,14 +199,14 @@ void *rlgym_bpu_create(const char *model_path, int bpu_core, int priority, } } -int rlgym_bpu_infer(void *handle, const float *input450, float *output12, char *err, +int rlgym_bpu_infer(void *handle, const float *input, float *output, char *err, int err_len) { try { set_error(err, err_len, ""); if (handle == nullptr) { throw std::runtime_error("null policy handle"); } - static_cast(handle)->infer(input450, output12); + static_cast(handle)->infer(input, output); return 0; } catch (const std::exception &e) { set_error(err, err_len, e.what()); @@ -216,6 +214,20 @@ int rlgym_bpu_infer(void *handle, const float *input450, float *output12, char * } } +int rlgym_bpu_input_floats(void *handle) { + if (handle == nullptr) { + return 0; + } + return static_cast(handle)->input_floats(); +} + +int rlgym_bpu_output_floats(void *handle) { + if (handle == nullptr) { + return 0; + } + return static_cast(handle)->output_floats(); +} + void rlgym_bpu_destroy(void *handle) { delete static_cast(handle); } diff --git a/deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_gym_bpu_x5_fastcpp.py b/deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_gym_bpu_x5_fastcpp.py new file mode 100644 index 0000000..cefd913 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_deploy_x5/deploy_go1_gym_bpu_x5_fastcpp.py @@ -0,0 +1,10 @@ +#!/usr/bin/env python3 +"""Compatibility wrapper for the Gym 5-frame BPU deployment entrypoint.""" + +import runpy +from pathlib import Path + + +if __name__ == "__main__": + entry = Path(__file__).resolve().parents[1] / "deploy_go1_rlgym_bpu_x5_fastcpp.py" + runpy.run_path(str(entry), run_name="__main__") 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 index 3adfd92..97b4bed 100644 --- a/deploy_45dim_rl_gym/bpu_deploy_x5/test_bpu_policy.py +++ b/deploy_45dim_rl_gym/bpu_deploy_x5/test_bpu_policy.py @@ -2,8 +2,8 @@ """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. +input file, runs the C++ DNN API backend repeatedly, and optionally checks the +known RobotLab 00000.bin reference output captured from hrt_model_exec. """ import argparse @@ -47,15 +47,19 @@ def main(): 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) + if data.size != policy.input_size: + raise ValueError( + f"{input_path} has {data.size} float32 values, " + f"but model expects {policy.input_size}" + ) action = policy(data) print("action", np.array2string(action, precision=6)) print("action_max_abs", float(np.max(np.abs(action)))) if args.check_reference_00000: + if policy.input_size != 450: + raise ValueError("--check-reference-00000 is only valid for the 450-dim RobotLab reference input") diff = np.abs(action - REFERENCE_00000) print("reference_max_abs_diff", float(diff.max())) print("reference_mean_abs_diff", float(diff.mean())) diff --git a/deploy_45dim_rl_gym/bpu_quantization/README.md b/deploy_45dim_rl_gym/bpu_quantization/README.md index 21df170..21c6fb8 100644 --- a/deploy_45dim_rl_gym/bpu_quantization/README.md +++ b/deploy_45dim_rl_gym/bpu_quantization/README.md @@ -1,8 +1,8 @@ # RDK X5 BPU 量化流程 -这个目录用于把 `../policy_robotlab_15000.onnx` 转成 RDK X5 可运行的 -Horizon runtime `.bin`。量化在 Mac 上用 CPU Docker 完成，板端只做离线 -`hrt_model_exec` 验证，暂时不要直接接入实机控制。 +这个目录用于把 Gym/RobotLab 的 ONNX 策略转成 RDK X5 可运行的 Horizon +runtime `.bin`。量化在 Mac 上用 CPU Docker 完成，板端可以做离线测速和实机 +部署测试。 参考资料： @@ -10,14 +10,103 @@ Horizon runtime `.bin`。量化在 Mac 上用 CPU Docker 完成，板端只做 - 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]`。 +4 维输入，且 N 维只能为 1。因此这里不能直接拿原始 ONNX 编译，需要先裁剪 +actions-only 输出、降级 opset，再把 `[1, D]` 输入包成固定 4D `NCHW`： +Gym 5 帧是 `[1, 1, 1, 225]`，RobotLab 10 帧是 `[1, 1, 1, 450]`。 ## 当前状态 +新增 Gym 5 帧策略的一键量化入口，默认目标是： + +- 原始模型：`../policy_35k.onnx` +- 原始输入：`obs [1, 225]` +- BPU 编译输入：`obs_4d [1, 1, 1, 225]` +- BPU 输出：`actions [1, 12, 1, 1]` +- 默认输出：`mapper_output_35k_gemm/policy_35k_int16_gemm.bin` +- 默认校准数据：`calibration_data_35k_gym_fast64/` + +本机 Docker 已完成一次默认量化： + +- 浮点 4D/Gemm 图等价对比：64 个真实样本上 `max_abs_diff = 7.15e-7` +- `hb_mapper makertbin` 输出：`actions` cosine `0.998524`，L1 `0.014313`，L2 `0.005103`，Chebyshev `0.040004` +- 编译估计延迟：`463.9 us` +- 产物大小：`2.0M` +- 产物路径：`deploy_45dim_rl_gym/bpu_quantization/mapper_output_35k_gemm/policy_35k_int16_gemm.bin` + +一键量化命令： + +```bash +cd /Users/chenyouyuan/cyy_ws/deploy_go1_pro/deploy_45dim_rl_gym/bpu_quantization +./quantize_policy_x5.sh +``` + +切换其他 Gym 轮次时直接指定模型和 round： + +```bash +./quantize_policy_x5.sh --policy ../policy_30k.onnx --round 30k +./quantize_policy_x5.sh --policy ../policy_25k.onnx --round 25k +./quantize_policy_x5.sh --policy ../policy_15k.onnx --round 15k +``` + +脚本默认只抽 64 个真实 RL 样本做校准和最多 64 个样本做浮点等价对比，避免 +之前 512 样本和 batch 回退导致的量化流程过慢。需要更稳的校准时再手动加大： + +```bash +./quantize_policy_x5.sh --samples 128 --compare-limit 128 +``` + +Gym BPU 部署入口支持快速切换轮次： + +```bash +cd /root/go1_pro_deploy +PYTHONPATH=/root/go1_pro_deploy python3 deploy_45dim_rl_gym/deploy_go1_rlgym_bpu_x5_fastcpp.py \ + --bpu-round 35k \ + --kill-sport \ + --enable-rl \ + --log-dir logs \ + --kp 32 --kd 1.0 \ + --kp-cal 20 --kd-cal 1.0 \ + --power-factor 9 \ + --position-protect-limit 0.0 \ + --action-clip 6.5 \ + --action-trip-limit 8.0 \ + --action-hard-trip-limit 16.0 \ + --max-target-step 0.0 \ + --max-roll-deg 50 \ + --max-pitch-deg 50 \ + --swap-vy-yaw \ + --rc-vx-scale 0.5 \ + --rc-vy-scale 0.5 \ + --rc-wz-scale 1.0 \ + --log-timing +``` + +也可以直接指定 bin： + +```bash +PYTHONPATH=/root/go1_pro_deploy python3 deploy_45dim_rl_gym/deploy_go1_rlgym_bpu_x5_fastcpp.py \ + --bpu-model deploy_45dim_rl_gym/bpu_quantization/mapper_output_35k_gemm/policy_35k_int16_gemm.bin \ + --infer-check --max-steps 1000 --log-timing +``` + +板端离线测速： + +```bash +cd /root/go1_pro_deploy +PYTHONPATH=/root/go1_pro_deploy python3 deploy_45dim_rl_gym/bpu_deploy_x5/test_bpu_policy.py \ + --bpu-model deploy_45dim_rl_gym/bpu_quantization/mapper_output_35k_gemm/policy_35k_int16_gemm.bin \ + --input-bin deploy_45dim_rl_gym/bpu_quantization/calibration_data_35k_gym_fast64/00000.bin \ + --repeat 1000 + +cd /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_deploy_x5/cpp +./bpu_dnn_bench \ + /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/mapper_output_35k_gemm/policy_35k_int16_gemm.bin \ + /root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/calibration_data_35k_gym_fast64/00000.bin \ + 1000 +``` + 已经完成 `policy_robotlab_15000.onnx` 和 `policy_robotlab_6500.onnx` 的 int16 -量化。当前 BPU 部署默认使用 6500 版本： +量化。RobotLab BPU 部署默认仍使用 6500 版本： - 原始模型：`../policy_robotlab_6500.onnx` - 原始输入：`obs [1, 450]` diff --git a/deploy_45dim_rl_gym/bpu_quantization/compare_4d_onnx.py b/deploy_45dim_rl_gym/bpu_quantization/compare_4d_onnx.py index 906c6ad..8c2e936 100644 --- a/deploy_45dim_rl_gym/bpu_quantization/compare_4d_onnx.py +++ b/deploy_45dim_rl_gym/bpu_quantization/compare_4d_onnx.py @@ -8,11 +8,17 @@ import numpy as np import onnxruntime as ort -def load_samples(calibration_dir, limit): +def load_samples(calibration_dir, limit, flat_dim): paths = sorted(Path(calibration_dir).glob("*.bin"))[:limit] if not paths: raise FileNotFoundError(f"No calibration .bin files found in {calibration_dir}") - return [np.fromfile(path, dtype=np.float32).reshape(1, 450) for path in paths] + samples = [] + for path in paths: + sample = np.fromfile(path, dtype=np.float32) + if sample.size != flat_dim: + raise ValueError(f"{path} has {sample.size} float32 values, expected {flat_dim}") + samples.append(sample.reshape(1, flat_dim)) + return samples def main(): @@ -20,6 +26,7 @@ def main(): parser.add_argument("--flat-onnx", type=Path, required=True) parser.add_argument("--bpu4d-onnx", type=Path, required=True) parser.add_argument("--calibration-dir", type=Path, default=Path("calibration_data")) + parser.add_argument("--flat-dim", type=int, default=450) parser.add_argument("--limit", type=int, default=64) args = parser.parse_args() @@ -30,9 +37,9 @@ def main(): max_abs = 0.0 max_mean_abs = 0.0 - for sample in load_samples(args.calibration_dir, args.limit): + for sample in load_samples(args.calibration_dir, args.limit, args.flat_dim): out_flat = flat.run(None, {flat_input: sample})[0] - out_wrapped = wrapped.run(None, {wrapped_input: sample.reshape(1, 1, 1, 450)})[0] + out_wrapped = wrapped.run(None, {wrapped_input: sample.reshape(1, 1, 1, args.flat_dim)})[0] diff = np.abs(out_flat - out_wrapped) max_abs = max(max_abs, float(diff.max())) max_mean_abs = max(max_mean_abs, float(diff.mean())) diff --git a/deploy_45dim_rl_gym/bpu_quantization/keep_actions_output.py b/deploy_45dim_rl_gym/bpu_quantization/keep_actions_output.py new file mode 100644 index 0000000..5dd93f1 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_quantization/keep_actions_output.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +"""Keep only the actions output in a policy ONNX graph.""" + +import argparse +from pathlib import Path + +import onnx + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--input", type=Path, required=True) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--output-name", default="actions") + args = parser.parse_args() + + model = onnx.load(str(args.input)) + outputs = list(model.graph.output) + if not outputs: + raise ValueError("ONNX graph has no outputs") + + selected = None + for output in outputs: + if output.name == args.output_name: + selected = output + break + if selected is None: + selected = outputs[0] + print(f"[WARN] output {args.output_name!r} not found; keeping first output {selected.name!r}") + + del model.graph.output[:] + model.graph.output.append(selected) + + args.output.parent.mkdir(parents=True, exist_ok=True) + onnx.save(model, str(args.output)) + print(f"Wrote actions-only ONNX: {args.output}") + + +if __name__ == "__main__": + main() diff --git a/deploy_45dim_rl_gym/bpu_quantization/make_calibration_data.py b/deploy_45dim_rl_gym/bpu_quantization/make_calibration_data.py index cc0cc04..712356d 100644 --- a/deploy_45dim_rl_gym/bpu_quantization/make_calibration_data.py +++ b/deploy_45dim_rl_gym/bpu_quantization/make_calibration_data.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Build float32 BPU calibration inputs from recorded RobotLab deployment logs.""" +"""Build float32 BPU calibration inputs from recorded deployment logs.""" import argparse import json @@ -11,16 +11,14 @@ 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): +def build_policy_input(history, history_len): frames = list(history) - while len(frames) < HISTORY_LEN: + while len(frames) < history_len: frames.insert(0, np.zeros(NUM_OBS, dtype=np.float32)) chunks = [] @@ -41,14 +39,14 @@ def reservoir_add(samples, value, seen, max_samples, rng): samples[replace_index] = value -def collect_samples(log_paths, max_samples, seed): +def collect_samples(log_paths, max_samples, seed, history_len): rng = random.Random(seed) samples = [] seen = 0 usable_runs = [] for steps_path in log_paths: - history = deque(maxlen=HISTORY_LEN) + history = deque(maxlen=history_len) run_seen = 0 was_rl = False @@ -71,13 +69,13 @@ def collect_samples(log_paths, max_samples, seed): continue history.append(obs) - onnx_input = build_onnx_input(history) - if not np.all(np.isfinite(onnx_input)): + policy_input = build_policy_input(history, history_len) + if not np.all(np.isfinite(policy_input)): continue seen += 1 run_seen += 1 - reservoir_add(samples, onnx_input, seen, max_samples, rng) + reservoir_add(samples, policy_input, seen, max_samples, rng) if run_seen: usable_runs.append({"steps": str(steps_path), "samples": run_seen}) @@ -87,14 +85,18 @@ def collect_samples(log_paths, max_samples, seed): def main(): parser = argparse.ArgumentParser( - description="Create BPU float32 calibration .bin files from RobotLab JSONL logs." + description="Create BPU float32 calibration .bin files from JSONL deployment logs." ) parser.add_argument( "--logs-root", type=Path, default=DEFAULT_LOG_ROOT, - help="Directory containing robotlab_go1_deploy_*/steps.jsonl.", + help="Directory containing _*/steps.jsonl.", ) + parser.add_argument("--log-prefix", default="robotlab_go1_deploy", + help="Run directory prefix below --logs-root") + parser.add_argument("--history-len", type=int, default=10, + help="Number of 45-dim observations to stack by term") parser.add_argument( "--output-dir", type=Path, @@ -102,23 +104,28 @@ def main(): help="Output directory for raw float32 feature-map .bin files.", ) parser.add_argument("--max-samples", type=int, default=512) + parser.add_argument("--min-samples", type=int, default=32) parser.add_argument("--seed", type=int, default=20260727) parser.add_argument("--overwrite", action="store_true") args = parser.parse_args() - if args.max_samples < 32: - raise ValueError("--max-samples must be at least 32") + if args.max_samples < 1: + raise ValueError("--max-samples must be positive") + if args.history_len < 1: + raise ValueError("--history-len must be positive") - log_paths = sorted(args.logs_root.glob("robotlab_go1_deploy_*/steps.jsonl")) + log_paths = sorted(args.logs_root.glob(f"{args.log_prefix}_*/steps.jsonl")) if not log_paths: - raise FileNotFoundError(f"No RobotLab step logs found below {args.logs_root}") + raise FileNotFoundError( + f"No step logs found for prefix {args.log_prefix!r} below {args.logs_root}" + ) samples, total_seen, usable_runs = collect_samples( - log_paths, args.max_samples, args.seed + log_paths, args.max_samples, args.seed, args.history_len ) - if len(samples) < 32: + if len(samples) < args.min_samples: raise RuntimeError( - f"Only {len(samples)} valid RL inputs found; need at least 32 calibration samples." + f"Only {len(samples)} valid RL inputs found; need at least {args.min_samples} calibration samples." ) output_dir = args.output_dir.resolve() @@ -134,13 +141,15 @@ def main(): for index, sample in enumerate(samples): sample.astype(np.float32, copy=False).tofile(output_dir / f"{index:05d}.bin") + policy_input_dim = NUM_OBS * args.history_len metadata = { "format": "raw float32 feature-map", - "flat_shape": [1, ONNX_INPUT_DIM], - "mapper_shape": [1, 1, 1, ONNX_INPUT_DIM], - "history_len": HISTORY_LEN, + "flat_shape": [1, policy_input_dim], + "mapper_shape": [1, 1, 1, policy_input_dim], + "history_len": args.history_len, "num_obs": NUM_OBS, "term_dims": list(TERM_DIMS), + "log_prefix": args.log_prefix, "selected_samples": len(samples), "candidate_rl_inputs": total_seen, "seed": args.seed, diff --git a/deploy_45dim_rl_gym/bpu_quantization/quantize_policy_x5.sh b/deploy_45dim_rl_gym/bpu_quantization/quantize_policy_x5.sh new file mode 100755 index 0000000..b2ffc29 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_quantization/quantize_policy_x5.sh @@ -0,0 +1,199 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)" + +POLICY="../policy_35k.onnx" +ROUND="35k" +NAME="" +HISTORY_LEN=5 +FLAT_DIM="" +SAMPLES=64 +MIN_SAMPLES=32 +LOG_PREFIX="rlgym_go1_deploy" +DOCKER_IMAGE="openexplorer/ai_toolchain_ubuntu_20_x5_cpu:v1.2.8" +COMPARE_LIMIT=64 +RUN_CHECKER=1 + +usage() { + cat <<'EOF' +Usage: + ./quantize_policy_x5.sh [options] + +Default: quantize Gym policy_35k.onnx as 5-frame/225-dim int16 Gemm BPU model. + +Options: + --policy PATH ONNX policy path, relative to this directory or absolute + --round NAME round label used in output paths, e.g. 15k/25k/30k/35k + --name NAME model basename; default is policy filename without .onnx + --history-len N observation history length; Gym=5, RobotLab=10 + --flat-dim N flat input dim; default 45 * history-len + --samples N calibration sample count; default 64 for faster mapping + --min-samples N minimum valid samples required; default 32 + --log-prefix PREFIX log dir prefix below logs/, default rlgym_go1_deploy + --docker-image IMAGE D-Robotics CPU toolchain image + --compare-limit N float ONNX equivalence sample count, default 64 + --skip-checker skip hb_mapper checker before makertbin +EOF +} + +while [[ $# -gt 0 ]]; do + case "$1" in + --policy) POLICY="$2"; shift 2 ;; + --round) ROUND="$2"; shift 2 ;; + --name) NAME="$2"; shift 2 ;; + --history-len) HISTORY_LEN="$2"; shift 2 ;; + --flat-dim) FLAT_DIM="$2"; shift 2 ;; + --samples) SAMPLES="$2"; shift 2 ;; + --min-samples) MIN_SAMPLES="$2"; shift 2 ;; + --log-prefix) LOG_PREFIX="$2"; shift 2 ;; + --docker-image) DOCKER_IMAGE="$2"; shift 2 ;; + --compare-limit) COMPARE_LIMIT="$2"; shift 2 ;; + --skip-checker) RUN_CHECKER=0; shift ;; + -h|--help) usage; exit 0 ;; + *) echo "Unknown argument: $1" >&2; usage >&2; exit 2 ;; + esac +done + +if [[ -z "${FLAT_DIM}" ]]; then + FLAT_DIM=$((45 * HISTORY_LEN)) +fi + +if [[ "${POLICY}" = /* ]]; then + POLICY_ABS="${POLICY}" +else + POLICY_ABS="${SCRIPT_DIR}/${POLICY}" +fi +POLICY_ABS="$(cd "$(dirname "${POLICY_ABS}")" && pwd)/$(basename "${POLICY_ABS}")" + +if [[ ! -f "${POLICY_ABS}" ]]; then + echo "Policy not found: ${POLICY_ABS}" >&2 + exit 2 +fi +case "${POLICY_ABS}" in + "${REPO_ROOT}"/*) POLICY_REL="${POLICY_ABS#${REPO_ROOT}/}" ;; + *) echo "Policy must be inside repo root ${REPO_ROOT}: ${POLICY_ABS}" >&2; exit 2 ;; +esac + +if [[ -z "${NAME}" ]]; then + NAME="$(basename "${POLICY_ABS}" .onnx)" +fi + +CAL_DIR="calibration_data_${ROUND}_gym_fast${SAMPLES}" +OUTPUT_DIR="mapper_output_${ROUND}_gemm" +OUTPUT_PREFIX="${NAME}_int16_gemm" +YAML_FILE="${OUTPUT_PREFIX}.yaml" + +echo "[INFO] repo : ${REPO_ROOT}" +echo "[INFO] policy : ${POLICY_REL}" +echo "[INFO] name/round : ${NAME} / ${ROUND}" +echo "[INFO] history/shape : ${HISTORY_LEN} / 1x1x1x${FLAT_DIM}" +echo "[INFO] calibration : ${CAL_DIR} (${SAMPLES} samples, prefix ${LOG_PREFIX})" +echo "[INFO] output : ${OUTPUT_DIR}/${OUTPUT_PREFIX}.bin" + +docker run --rm --platform linux/amd64 \ + -e POLICY_REL="${POLICY_REL}" \ + -e NAME="${NAME}" \ + -e HISTORY_LEN="${HISTORY_LEN}" \ + -e FLAT_DIM="${FLAT_DIM}" \ + -e SAMPLES="${SAMPLES}" \ + -e MIN_SAMPLES="${MIN_SAMPLES}" \ + -e LOG_PREFIX="${LOG_PREFIX}" \ + -e CAL_DIR="${CAL_DIR}" \ + -e OUTPUT_DIR="${OUTPUT_DIR}" \ + -e OUTPUT_PREFIX="${OUTPUT_PREFIX}" \ + -e YAML_FILE="${YAML_FILE}" \ + -e COMPARE_LIMIT="${COMPARE_LIMIT}" \ + -e RUN_CHECKER="${RUN_CHECKER}" \ + -v "${REPO_ROOT}:/workspace/deploy_go1_pro" \ + "${DOCKER_IMAGE}" \ + bash -lc ' + set -euo pipefail + cd /workspace/deploy_go1_pro/deploy_45dim_rl_gym/bpu_quantization + + POLICY="/workspace/deploy_go1_pro/${POLICY_REL}" + ACTIONS_ONNX="${NAME}_actions.onnx" + OPSET_ONNX="${NAME}_opset11.onnx" + BPU4D_ONNX="${NAME}_bpu4d.onnx" + GEMM_ONNX="${NAME}_bpu4d_gemm.onnx" + + python3 make_calibration_data.py \ + --logs-root ../../logs \ + --log-prefix "${LOG_PREFIX}" \ + --history-len "${HISTORY_LEN}" \ + --output-dir "${CAL_DIR}" \ + --max-samples "${SAMPLES}" \ + --min-samples "${MIN_SAMPLES}" \ + --overwrite + + python3 keep_actions_output.py \ + --input "${POLICY}" \ + --output "${ACTIONS_ONNX}" + + python3 downgrade_policy_to_opset11.py \ + --input "${ACTIONS_ONNX}" \ + --output "${OPSET_ONNX}" + + python3 make_bpu_4d_onnx.py \ + --input "${OPSET_ONNX}" \ + --output "${BPU4D_ONNX}" \ + --flat-dim "${FLAT_DIM}" + + python3 replace_group_conv_with_gemm.py \ + --input "${BPU4D_ONNX}" \ + --output "${GEMM_ONNX}" + + python3 compare_4d_onnx.py \ + --flat-onnx "${ACTIONS_ONNX}" \ + --bpu4d-onnx "${GEMM_ONNX}" \ + --calibration-dir "${CAL_DIR}" \ + --flat-dim "${FLAT_DIM}" \ + --limit "${COMPARE_LIMIT}" + + cat > "${YAML_FILE}" < 1 + and attr_value(node, "kernel_shape") == [1] + ] + if len(candidates) == 1: + conv = candidates[0] + print(f"[WARN] {CONV_NAME!r} not found; using grouped Conv {conv.name!r}") + if conv is None: + raise ValueError(f"Cannot find unique grouped 1x1 Conv node; fixed name {CONV_NAME!r} not found") if conv.op_type != "Conv": raise ValueError(f"{CONV_NAME!r} is {conv.op_type}, expected Conv") diff --git a/deploy_45dim_rl_gym/deploy_go1_rlgym_bpu_x5_fastcpp.py b/deploy_45dim_rl_gym/deploy_go1_rlgym_bpu_x5_fastcpp.py new file mode 100644 index 0000000..ff68bd6 --- /dev/null +++ b/deploy_45dim_rl_gym/deploy_go1_rlgym_bpu_x5_fastcpp.py @@ -0,0 +1,1281 @@ +#!/usr/bin/env python3 +""" +Deploy the RoboGauge Go1 45-dim RL-Gym BPU policy on Unitree Go1 PRO. + +This uses go1_pro_sdk direct MCU control, not LCM or the official Unitree SDK. + +Policy: + - policy_35k_int16_gemm.bin by default + - single-frame obs: 45 dims + - BPU input: 5-frame history, 1x1x1x225 featuremap, stacked by observation terms + - 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 ( + MotorMode, PowerProtectViolation, JOINT_NAMES, +) + +HERE = Path(__file__).parent.resolve() +DEPLOY_ROOT = HERE.parent +WORKSPACE_ROOT = HERE.parents[1] +sys.path.insert(0, str(HERE / "bpu_deploy_x5")) +from bpu_policy import BpuInferLibPolicy + + +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 + from fast_mcu import FastMCUClient +except ImportError as exc: + raise RuntimeError( + "C++ fast LowCmd/LowState 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 + +BPU_MODEL_REGISTRY = { + "15k": HERE / "bpu_quantization" / "mapper_output_15k_gemm" / "policy_15k_int16_gemm.bin", + "25k": HERE / "bpu_quantization" / "mapper_output_25k_gemm" / "policy_25k_int16_gemm.bin", + "30k": HERE / "bpu_quantization" / "mapper_output_30k_gemm" / "policy_30k_int16_gemm.bin", + "35k": HERE / "bpu_quantization" / "mapper_output_35k_gemm" / "policy_35k_int16_gemm.bin", +} +DEFAULT_BPU_ROUND = "35k" +DEFAULT_BPU_MODEL = BPU_MODEL_REGISTRY[DEFAULT_BPU_ROUND] +LOWCMD_BACKEND = "cpp_lowcmd_cpp_lowstate" +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 = 5 +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([2.0, 2.0, 0.25], 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 resolve_bpu_model(args): + if args.bpu_model: + return Path(args.bpu_model).expanduser().resolve() + key = str(args.bpu_round).lower() + if key not in BPU_MODEL_REGISTRY: + choices = ", ".join(sorted(BPU_MODEL_REGISTRY)) + raise ValueError(f"Unknown --bpu-round {args.bpu_round!r}; choices: {choices}") + return BPU_MODEL_REGISTRY[key].expanduser().resolve() + + +def make_policy(args): + model_path = resolve_bpu_model(args) + policy = BpuInferLibPolicy( + model_path, + priority=args.bpu_priority, + bpu_cores=args.bpu_cores, + ) + if policy.input_size != POLICY_INPUT_DIM: + policy.close() + raise ValueError( + f"BPU model input has {policy.input_size} values, expected {POLICY_INPUT_DIM}. " + "Use a Gym 5-frame/225-dim .bin, not a RobotLab 10-frame/450-dim .bin." + ) + if policy.output_size != NUM_ACTIONS: + policy.close() + raise ValueError( + f"BPU model output has {policy.output_size} values, expected exactly {NUM_ACTIONS}. " + "Compile an actions-only ONNX for deployment." + ) + return policy + + +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 225-dim 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"rlgym_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_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"], + "policy_backend": BpuInferLibPolicy.backend_name, + "bpu_model": str(resolve_bpu_model(args)), + "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 = FastMCUClient() + 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 = make_policy(args) + 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 RL-Gym 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 225-dim 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 = make_policy(args) + 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 RL-Gym BPU on Go1 PRO") + parser.add_argument("--bpu-model", default="", + help="Path to a compiled Gym 5-frame BPU .bin; overrides --bpu-round") + parser.add_argument("--bpu-round", default=DEFAULT_BPU_ROUND, + choices=sorted(BPU_MODEL_REGISTRY), + help="Quick-select mapper_output__gemm/policy__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=80.0) + parser.add_argument("--kd", type=float, default=1.0) + 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=1.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(HERE / "logs"), help="JSONL log directory; empty disables logging") + parser.add_argument("--log-flush-every", type=int, default=50) + parser.add_argument("--motor-mode-fault-frames", type=int, default=3, + help="Consecutive non-servo feedback frames required before a motor-mode fault") + 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()