From 8a44857314bd2dc6591018f80e052327b4a68cb6 Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Wed, 29 Jul 2026 21:00:26 +0800 Subject: [PATCH] adapt s100 --- deploy_45dim_rl_gym/bpu_deploy_s100/README.md | 207 +++ .../bpu_deploy_s100/bpu_policy.py | 77 + .../deploy_go1_robotlab_bpu_s100_fastcpp.py | 1280 +++++++++++++++++ .../bpu_deploy_s100/test_bpu_policy.py | 55 + ...policy_robotlab_26000_s100_int16_gemm.yaml | 33 + .../bpu_quantization/quantize_policy_s100.sh | 229 +++ 6 files changed, 1881 insertions(+) create mode 100644 deploy_45dim_rl_gym/bpu_deploy_s100/README.md create mode 100644 deploy_45dim_rl_gym/bpu_deploy_s100/bpu_policy.py create mode 100644 deploy_45dim_rl_gym/bpu_deploy_s100/deploy_go1_robotlab_bpu_s100_fastcpp.py create mode 100644 deploy_45dim_rl_gym/bpu_deploy_s100/test_bpu_policy.py create mode 100644 deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_26000_s100_int16_gemm.yaml create mode 100755 deploy_45dim_rl_gym/bpu_quantization/quantize_policy_s100.sh diff --git a/deploy_45dim_rl_gym/bpu_deploy_s100/README.md b/deploy_45dim_rl_gym/bpu_deploy_s100/README.md new file mode 100644 index 0000000..7aec824 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_deploy_s100/README.md @@ -0,0 +1,207 @@ +# S100 BPU 部署测试 + +这个目录是 S100 平台的隔离部署路径，不覆盖现有 X5 BPU 脚本。 + +当前默认模型： + +```text +deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/policy_robotlab_26000_s100_int16_gemm.hbm +``` + +S100 量化参数： + +- 原始模型：`deploy_45dim_rl_gym/policy_robotlab_26000.onnx` +- 历史长度：RobotLab 10 帧 +- 输入：`obs_4d [1, 1, 1, 450]`，float32 featuremap +- 输出：`actions [1, 12, 1, 1]` +- `march`：`nash-e` +- Docker 镜像：`registry.d-robotics.cc/deliver/ai_toolchain_ubuntu_22_s100_s600_cpu:v3.7.0` + +## 本机量化 + +在 Mac 的仓库根目录执行： + +```bash +cd /Users/chenyouyuan/cyy_ws/deploy_go1_pro +bash deploy_45dim_rl_gym/bpu_quantization/quantize_policy_s100.sh +``` + +等价显式命令： + +```bash +cd /Users/chenyouyuan/cyy_ws/deploy_go1_pro +bash deploy_45dim_rl_gym/bpu_quantization/quantize_policy_s100.sh \ + --policy ../policy_robotlab_26000.onnx \ + --round 26000 \ + --name policy_robotlab_26000 \ + --history-len 10 \ + --samples 64 \ + --min-samples 32 \ + --log-prefix robotlab_go1_deploy \ + --cal-tag robotlab \ + --march nash-e +``` + +输出文件： + +```text +deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/policy_robotlab_26000_s100_int16_gemm.hbm +``` + +## 同步到 S100 + +S100 板端地址： + +```text +root@192.168.11.144 +``` + +如果仓库已经通过 git 同步，直接在板端拉取即可。如果只同步产物，可以从 Mac 执行；Docker 只在 Mac 上用于量化，S100 板端不运行 Docker： + +```bash +scp \ + deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/policy_robotlab_26000_s100_int16_gemm.hbm \ + root@192.168.11.144:/root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/ +``` + +同时确保校准输入存在，离线测速会用到： + +```bash +scp \ + deploy_45dim_rl_gym/bpu_quantization/calibration_data_26000_robotlab_fast64/00000.bin \ + root@192.168.11.144:/root/go1_pro_deploy/deploy_45dim_rl_gym/bpu_quantization/calibration_data_26000_robotlab_fast64/ +``` + +## 板端安装 hbm_runtime + +```bash +ssh root@192.168.11.144 +cd /usr/hobot/lib/hbm_runtime +./build.sh install +``` + +S100 使用官方 `hbm_runtime` Python 绑定加载 `.hbm`，不复用 X5 的 +`/usr/include/dnn/hb_dnn.h` C++ wrapper。 + +## 离线推理测速 + +先用官方 `hrt_model_exec` 看模型信息： + +```bash +cd /root/go1_pro_deploy +/usr/hobot/bin/hrt_model_exec model_info \ + --model_file deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/policy_robotlab_26000_s100_int16_gemm.hbm +``` + +官方 `hrt_model_exec` 稳态测速： + +```bash +cd /root/go1_pro_deploy +/usr/hobot/bin/hrt_model_exec perf \ + --model_file deploy_45dim_rl_gym/bpu_quantization/mapper_output_26000_s100_gemm/policy_robotlab_26000_s100_int16_gemm.hbm \ + --model_name policy_robotlab_26000_s100_int16_gemm \ + --input_file deploy_45dim_rl_gym/bpu_quantization/calibration_data_26000_robotlab_fast64/00000.bin \ + --frame_count 1000 \ + --thread_num 1 +``` + +当前板端 `root@192.168.11.144` 已验证： + +```text +Average latency: 0.394 ms +FPS: 2442.456 +``` + +部署脚本使用的 Python `hbm_runtime` wrapper： + +```bash +cd /root/go1_pro_deploy +PYTHONPATH=/root/go1_pro_sdk:/root/go1_pro_deploy \ +python3 deploy_45dim_rl_gym/bpu_deploy_s100/test_bpu_policy.py \ + --repeat 1000 +``` + +当前板端结果： + +```text +Backend: hbm_runtime_s100 +Input: obs_4d (1, 1, 1, 450) +Output: actions (1, 12) +repeat=1000 avg_ms=0.733020 +``` + +## 离线推理检查 + +这一步会连接 MCU 读取状态，但不会发送电机指令： + +```bash +cd /root/go1_pro_deploy +PYTHONPATH=/root/go1_pro_sdk:/root/go1_pro_deploy \ +python3 deploy_45dim_rl_gym/bpu_deploy_s100/deploy_go1_robotlab_bpu_s100_fastcpp.py \ + --infer-check \ + --log-dir logs \ + --print-every 50 \ + --max-steps 500 +``` + +## 悬空状态机测试 + +先不要加 `--enable-rl`，确认 R2 只能推进到 `INFER_TEST`： + +```bash +cd /root/go1_pro_deploy +PYTHONPATH=/root/go1_pro_sdk:/root/go1_pro_deploy \ +python3 deploy_45dim_rl_gym/bpu_deploy_s100/deploy_go1_robotlab_bpu_s100_fastcpp.py \ + --kill-sport \ + --log-dir logs \ + --kp 28 --kd 0.7 \ + --kp-cal 20 --kd-cal 1.0 \ + --power-factor 7 \ + --position-protect-limit 0.0 \ + --action-clip 5.0 \ + --action-trip-limit 8.0 \ + --action-hard-trip-limit 16.0 \ + --max-target-step 0.025 \ + --max-roll-deg 35 \ + --max-pitch-deg 35 \ + --swap-vy-yaw \ + --rc-vx-scale 0.3 \ + --rc-vy-scale 0.3 \ + --rc-wz-scale 0.6 \ + --log-timing +``` + +## 实际 RL 启动 + +只有悬空测试正常后，再启用 RL： + +```bash +cd /root/go1_pro_deploy +PYTHONPATH=/root/go1_pro_sdk:/root/go1_pro_deploy \ +python3 deploy_45dim_rl_gym/bpu_deploy_s100/deploy_go1_robotlab_bpu_s100_fastcpp.py \ + --kill-sport \ + --enable-rl \ + --log-dir logs \ + --kp 28 --kd 0.7 \ + --kp-cal 20 --kd-cal 1.0 \ + --power-factor 7 \ + --position-protect-limit 0.0 \ + --action-clip 5.0 \ + --action-trip-limit 8.0 \ + --action-hard-trip-limit 16.0 \ + --max-target-step 0.025 \ + --max-roll-deg 35 \ + --max-pitch-deg 35 \ + --swap-vy-yaw \ + --rc-vx-scale 0.3 \ + --rc-vy-scale 0.3 \ + --rc-wz-scale 0.6 \ + --log-timing +``` + +如果要临时指定其它 S100 `.hbm`： + +```bash +python3 deploy_45dim_rl_gym/bpu_deploy_s100/deploy_go1_robotlab_bpu_s100_fastcpp.py \ + --bpu-model /absolute/path/to/model.hbm +``` diff --git a/deploy_45dim_rl_gym/bpu_deploy_s100/bpu_policy.py b/deploy_45dim_rl_gym/bpu_deploy_s100/bpu_policy.py new file mode 100644 index 0000000..e551742 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_deploy_s100/bpu_policy.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python3 +"""S100 HBM policy runtime wrapper.""" + +from pathlib import Path + +import numpy as np + + +class BpuInferLibPolicy: + """S100 backend: official hbm_runtime Python binding.""" + + backend_name = "hbm_runtime_s100" + + def __init__(self, model_path, priority=0, bpu_cores=(0,), cpp_lib=None): + del cpp_lib + self.model_path = Path(model_path).expanduser().resolve() + if not self.model_path.exists(): + raise FileNotFoundError(f"S100 HBM model not found: {self.model_path}") + + try: + from hbm_runtime import HB_HBMRuntime + except ImportError as exc: + raise RuntimeError( + "hbm_runtime is required on S100. Install it on the board with: " + "cd /usr/hobot/lib/hbm_runtime && ./build.sh install" + ) from exc + + self.priority = int(priority) + self.bpu_cores = tuple(int(core) for core in bpu_cores) + self.runtime = HB_HBMRuntime(str(self.model_path)) + self.version = getattr(self.runtime, "version", "") + + model_names = list(self.runtime.model_names) + if len(model_names) != 1: + raise RuntimeError(f"expected one model in {self.model_path}, got {model_names}") + self.model_name = model_names[0] + + input_names = list(self.runtime.input_names[self.model_name]) + output_names = list(self.runtime.output_names[self.model_name]) + if len(input_names) != 1 or len(output_names) != 1: + raise RuntimeError( + f"expected 1 input and 1 output, got {input_names} / {output_names}" + ) + self.input_name = input_names[0] + self.output_name = output_names[0] + + self.input_shape = tuple(int(x) for x in self.runtime.input_shapes[self.model_name][self.input_name]) + self.output_shape = tuple(int(x) for x in self.runtime.output_shapes[self.model_name][self.output_name]) + self.input_size = int(np.prod(self.input_shape)) + self.output_size = int(np.prod(self.output_shape)) + + print(f"[INFO] BPU model: {self.model_path}") + print(f"[INFO] Backend: {self.backend_name}") + print(f"[INFO] Runtime: {self.version}") + print(f"[INFO] Input : {self.input_name} {self.input_shape}") + print(f"[INFO] Output : {self.output_name} {self.output_shape}") + + def close(self): + self.runtime = None + + def __call__(self, flat_input): + arr = np.asarray(flat_input, dtype=np.float32) + if arr.size != self.input_size: + raise ValueError(f"S100 policy input has {arr.size} values, expected {self.input_size}") + input_tensor = np.ascontiguousarray(arr.reshape(self.input_shape), dtype=np.float32) + outputs = self.runtime.run(input_tensor) + action = np.asarray(outputs[self.model_name][self.output_name], dtype=np.float32).reshape(-1) + if action.size != self.output_size: + raise RuntimeError( + f"S100 policy output has {action.size} values, expected {self.output_size}" + ) + if not np.all(np.isfinite(action)): + raise RuntimeError(f"S100 policy output is not finite: {action}") + return action.copy() + + +BpuInferLibPythonPolicy = BpuInferLibPolicy diff --git a/deploy_45dim_rl_gym/bpu_deploy_s100/deploy_go1_robotlab_bpu_s100_fastcpp.py b/deploy_45dim_rl_gym/bpu_deploy_s100/deploy_go1_robotlab_bpu_s100_fastcpp.py new file mode 100644 index 0000000..8c0eae7 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_deploy_s100/deploy_go1_robotlab_bpu_s100_fastcpp.py @@ -0,0 +1,1280 @@ +#!/usr/bin/env python3 +""" +Deploy the RoboGauge Go1 45-dim RobotLab BPU policy on the S100 board. + +This uses go1_pro_sdk direct MCU control, not LCM or the official Unitree SDK. + +Policy: + - policy_robotlab_26000_s100_int16_gemm.hbm by default + - 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 + 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 = { + "26000": HERE.parent / "bpu_quantization" / "mapper_output_26000_s100_gemm" / + "policy_robotlab_26000_s100_int16_gemm.hbm", +} +DEFAULT_BPU_ROUND = "26000" +DEFAULT_BPU_MODEL = BPU_MODEL_REGISTRY[DEFAULT_BPU_ROUND] +LOWCMD_BACKEND = "cpp_lowcmd_cpp_lowstate_s100" +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 resolve_bpu_model(args): + if args.bpu_model: + return Path(args.bpu_model).expanduser().resolve() + key = str(args.bpu_round) + 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 RobotLab 10-frame/450-dim S100 .hbm." + ) + 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 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(resolve_bpu_model(args)), + "bpu_round": args.bpu_round, + "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 = 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 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 = 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 RobotLab BPU on S100") + parser.add_argument("--bpu-model", default="", + help="Path to a compiled RobotLab 10-frame S100 BPU .hbm; overrides --bpu-round") + parser.add_argument("--bpu-round", default=DEFAULT_BPU_ROUND, + choices=sorted(BPU_MODEL_REGISTRY), + help="Quick-select RobotLab BPU model round") + 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_s100/test_bpu_policy.py b/deploy_45dim_rl_gym/bpu_deploy_s100/test_bpu_policy.py new file mode 100644 index 0000000..6b05b04 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_deploy_s100/test_bpu_policy.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +"""Offline BPU policy smoke test for S100. + +This does not connect to the robot. It loads a S100 BPU .hbm and one raw +float32 input file, then runs the hbm_runtime backend repeatedly. +""" + +import argparse +import time +from pathlib import Path + +import numpy as np + +from bpu_policy import BpuInferLibPolicy + + +HERE = Path(__file__).parent.resolve() +DEFAULT_MODEL = ( + HERE.parent / "bpu_quantization" / "mapper_output_26000_s100_gemm" / + "policy_robotlab_26000_s100_int16_gemm.hbm" +) +DEFAULT_INPUT = ( + HERE.parent / "bpu_quantization" / + "calibration_data_26000_robotlab_fast64" / "00000.bin" +) + +def main(): + parser = argparse.ArgumentParser(description="Offline BPU policy smoke test") + parser.add_argument("--bpu-model", default=str(DEFAULT_MODEL)) + parser.add_argument("--input-bin", default=str(DEFAULT_INPUT)) + parser.add_argument("--repeat", type=int, default=1000) + args = parser.parse_args() + + input_path = Path(args.input_bin).expanduser().resolve() + data = np.fromfile(input_path, dtype=np.float32) + policy = BpuInferLibPolicy(args.bpu_model) + if data.size != policy.input_size: + raise ValueError( + f"{input_path} has {data.size} float32 values, " + f"but model expects {policy.input_size}" + ) + action = policy(data) + print("action", np.array2string(action, precision=6)) + print("action_max_abs", float(np.max(np.abs(action)))) + + repeats = max(1, int(args.repeat)) + t0 = time.perf_counter() + for _ in range(repeats): + policy(data) + elapsed_ms = (time.perf_counter() - t0) * 1000.0 + print(f"repeat={repeats} avg_ms={elapsed_ms / repeats:.6f}") + + +if __name__ == "__main__": + main() diff --git a/deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_26000_s100_int16_gemm.yaml b/deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_26000_s100_int16_gemm.yaml new file mode 100644 index 0000000..c751f29 --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_quantization/policy_robotlab_26000_s100_int16_gemm.yaml @@ -0,0 +1,33 @@ +model_parameters: + onnx_model: "./policy_robotlab_26000_bpu4d_gemm.onnx" + march: "nash-e" + layer_out_dump: false + working_dir: "mapper_output_26000_s100_gemm" + output_model_file_prefix: "policy_robotlab_26000_s100_int16_gemm" + +input_parameters: + input_name: "obs_4d" + input_shape: "1x1x1x450" + input_type_rt: "featuremap" + input_type_train: "featuremap" + input_layout_train: "NCHW" + norm_type: "no_preprocess" + separate_batch: false + +calibration_parameters: + cal_data_dir: "./calibration_data_26000_robotlab_fast64" + cal_data_type: "float32" + calibration_type: "max" + quant_config: + model_config: + all_node_type: int16 + activation: + calibration_type: max + per_channel: true + +compiler_parameters: + compile_mode: "latency" + optimize_level: "O2" + core_num: 1 + jobs: 8 + cache_mode: "disable" diff --git a/deploy_45dim_rl_gym/bpu_quantization/quantize_policy_s100.sh b/deploy_45dim_rl_gym/bpu_quantization/quantize_policy_s100.sh new file mode 100755 index 0000000..09aa2ab --- /dev/null +++ b/deploy_45dim_rl_gym/bpu_quantization/quantize_policy_s100.sh @@ -0,0 +1,229 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)" + +POLICY="../policy_robotlab_26000.onnx" +ROUND="26000" +NAME="" +HISTORY_LEN=10 +FLAT_DIM="" +SAMPLES=64 +MIN_SAMPLES=32 +LOG_PREFIX="robotlab_go1_deploy" +CAL_TAG="robotlab" +DOCKER_IMAGE="registry.d-robotics.cc/deliver/ai_toolchain_ubuntu_22_s100_s600_cpu:v3.7.0" +MARCH="nash-e" +COMPARE_LIMIT=64 +RUN_CHECKER=1 +QUANT="int16" + +usage() { + cat <<'EOF' +Usage: + ./quantize_policy_s100.sh [options] + +Default: quantize RobotLab policy_robotlab_26000.onnx as 10-frame/450-dim +S100 int16 Gemm BPU model. + +Options: + --policy PATH ONNX policy path, relative to this directory or absolute + --round NAME round label used in output paths, e.g. 15k/25k/30k/35k + --name NAME model basename; default is policy filename without .onnx + --history-len N observation history length; Gym=5, RobotLab=10 + --flat-dim N flat input dim; default 45 * history-len + --samples N calibration sample count; default 64 for faster mapping + --min-samples N minimum valid samples required; default 32 + --log-prefix PREFIX log dir prefix below logs/, default rlgym_go1_deploy + --cal-tag TAG calibration dir tag, default gym + --docker-image IMAGE D-Robotics S100/S600 CPU toolchain image + --march MARCH S100 march, default nash-e + --compare-limit N float ONNX equivalence sample count, default 64 + --quant int16|int8 int16 uses all_node_type int16; int8 uses default S100 PTQ + --skip-checker accepted for parity with X5 script; hb_compile path ignores it +EOF +} + +while [[ $# -gt 0 ]]; do + case "$1" in + --policy) POLICY="$2"; shift 2 ;; + --round) ROUND="$2"; shift 2 ;; + --name) NAME="$2"; shift 2 ;; + --history-len) HISTORY_LEN="$2"; shift 2 ;; + --flat-dim) FLAT_DIM="$2"; shift 2 ;; + --samples) SAMPLES="$2"; shift 2 ;; + --min-samples) MIN_SAMPLES="$2"; shift 2 ;; + --log-prefix) LOG_PREFIX="$2"; shift 2 ;; + --cal-tag) CAL_TAG="$2"; shift 2 ;; + --docker-image) DOCKER_IMAGE="$2"; shift 2 ;; + --march) MARCH="$2"; shift 2 ;; + --compare-limit) COMPARE_LIMIT="$2"; shift 2 ;; + --quant) QUANT="$2"; shift 2 ;; + --skip-checker) RUN_CHECKER=0; shift ;; + -h|--help) usage; exit 0 ;; + *) echo "Unknown argument: $1" >&2; usage >&2; exit 2 ;; + esac +done + +if [[ -z "${FLAT_DIM}" ]]; then + FLAT_DIM=$((45 * HISTORY_LEN)) +fi + +case "${QUANT}" in + int16|int8) ;; + *) echo "--quant must be int16 or int8, got: ${QUANT}" >&2; exit 2 ;; +esac + +if [[ "${POLICY}" = /* ]]; then + POLICY_ABS="${POLICY}" +else + POLICY_ABS="${SCRIPT_DIR}/${POLICY}" +fi +POLICY_ABS="$(cd "$(dirname "${POLICY_ABS}")" && pwd)/$(basename "${POLICY_ABS}")" + +if [[ ! -f "${POLICY_ABS}" ]]; then + echo "Policy not found: ${POLICY_ABS}" >&2 + exit 2 +fi +case "${POLICY_ABS}" in + "${REPO_ROOT}"/*) POLICY_REL="${POLICY_ABS#${REPO_ROOT}/}" ;; + *) echo "Policy must be inside repo root ${REPO_ROOT}: ${POLICY_ABS}" >&2; exit 2 ;; +esac + +if [[ -z "${NAME}" ]]; then + NAME="$(basename "${POLICY_ABS}" .onnx)" +fi + +CAL_DIR="calibration_data_${ROUND}_${CAL_TAG}_fast${SAMPLES}" +if [[ "${QUANT}" = "int16" ]]; then + OUTPUT_DIR="mapper_output_${ROUND}_s100_gemm" + OUTPUT_PREFIX="${NAME}_s100_int16_gemm" +else + OUTPUT_DIR="mapper_output_${ROUND}_s100_int8_gemm" + OUTPUT_PREFIX="${NAME}_s100_int8_gemm" +fi +YAML_FILE="${OUTPUT_PREFIX}.yaml" + +echo "[INFO] repo : ${REPO_ROOT}" +echo "[INFO] policy : ${POLICY_REL}" +echo "[INFO] name/round : ${NAME} / ${ROUND}" +echo "[INFO] history/shape : ${HISTORY_LEN} / 1x1x1x${FLAT_DIM}" +echo "[INFO] calibration : ${CAL_DIR} (${SAMPLES} samples, prefix ${LOG_PREFIX})" +echo "[INFO] output : ${OUTPUT_DIR}/${OUTPUT_PREFIX}.hbm" +echo "[INFO] quant : ${QUANT}" +echo "[INFO] march : ${MARCH}" +echo "[INFO] docker image : ${DOCKER_IMAGE}" + +docker run --rm --platform linux/amd64 \ + -e POLICY_REL="${POLICY_REL}" \ + -e NAME="${NAME}" \ + -e HISTORY_LEN="${HISTORY_LEN}" \ + -e FLAT_DIM="${FLAT_DIM}" \ + -e SAMPLES="${SAMPLES}" \ + -e MIN_SAMPLES="${MIN_SAMPLES}" \ + -e LOG_PREFIX="${LOG_PREFIX}" \ + -e CAL_DIR="${CAL_DIR}" \ + -e OUTPUT_DIR="${OUTPUT_DIR}" \ + -e OUTPUT_PREFIX="${OUTPUT_PREFIX}" \ + -e YAML_FILE="${YAML_FILE}" \ + -e COMPARE_LIMIT="${COMPARE_LIMIT}" \ + -e RUN_CHECKER="${RUN_CHECKER}" \ + -e QUANT="${QUANT}" \ + -e MARCH="${MARCH}" \ + -v "${REPO_ROOT}:/workspace/deploy_go1_pro" \ + "${DOCKER_IMAGE}" \ + bash -lc ' + set -euo pipefail + cd /workspace/deploy_go1_pro/deploy_45dim_rl_gym/bpu_quantization + + POLICY="/workspace/deploy_go1_pro/${POLICY_REL}" + ACTIONS_ONNX="${NAME}_actions.onnx" + OPSET_ONNX="${NAME}_opset11.onnx" + BPU4D_ONNX="${NAME}_bpu4d.onnx" + GEMM_ONNX="${NAME}_bpu4d_gemm.onnx" + + python3 make_calibration_data.py \ + --logs-root ../../logs \ + --log-prefix "${LOG_PREFIX}" \ + --history-len "${HISTORY_LEN}" \ + --output-dir "${CAL_DIR}" \ + --max-samples "${SAMPLES}" \ + --min-samples "${MIN_SAMPLES}" \ + --overwrite + + python3 keep_actions_output.py \ + --input "${POLICY}" \ + --output "${ACTIONS_ONNX}" + + python3 downgrade_policy_to_opset11.py \ + --input "${ACTIONS_ONNX}" \ + --output "${OPSET_ONNX}" + + python3 make_bpu_4d_onnx.py \ + --input "${OPSET_ONNX}" \ + --output "${BPU4D_ONNX}" \ + --flat-dim "${FLAT_DIM}" + + python3 replace_group_conv_with_gemm.py \ + --input "${BPU4D_ONNX}" \ + --output "${GEMM_ONNX}" + + python3 compare_4d_onnx.py \ + --flat-onnx "${ACTIONS_ONNX}" \ + --bpu4d-onnx "${GEMM_ONNX}" \ + --calibration-dir "${CAL_DIR}" \ + --flat-dim "${FLAT_DIM}" \ + --limit "${COMPARE_LIMIT}" + + if [[ "${QUANT}" = "int16" ]]; then + QUANT_CONFIG=$(cat < "${YAML_FILE}" <