diff --git a/deploy_45dim_rl_gym/README.md b/deploy_45dim_rl_gym/README.md
new file mode 100644
index 0000000..799d31f
--- /dev/null
+++ b/deploy_45dim_rl_gym/README.md
@@ -0,0 +1,668 @@
+# Go1 RL-Gym 45 维部署说明
+
+本目录包含:
+
+- `policy_15k.onnx`:RoboGauge Go1 RL-Gym 的 ONNX 策略
+- `deploy_go1_onnx_mujoco.py`:MuJoCo 调试脚本
+- `deploy_go1_rlgym_pro_sdk.py`:真机 Go1 PRO 低层部署脚本
+
+该 ONNX 策略使用 45 维单帧观测,以及 5 帧、共 225 维的历史输入,按观测项分组堆叠:
+
+```text
+[角速度历史, 重力投影历史, 指令历史, 关节位置历史, 关节速度历史, 上一动作历史]
+```
+
+关节顺序为 `FR, FL, RR, RL`,与 `go1_pro_sdk` 的电机顺序一致。
+
+## 安全说明
+
+直接低层控电机是危险操作。
+
+- 第一次测试必须悬空
+- 低层控制前先杀掉 sport 进程
+- sport 被杀后,要保留拔电池作为最终停机手段
+- 真机状态机中,`L2` 会返回 `IDLE` 阻尼
+- `Ctrl+C` 会通过 `safe_stop()` 退出
+- 只有显式加 `--enable-rl` 才允许真正进入 RL 发电机目标
+- 脚本启动连接 MCU 前会自动检查 SDK 电机顺序是否为 `FR, FL, RR, RL`
+- 遥控器速度指令默认只保留 deadzone,不再做低通和变化率限制;日志中仍会同时保存原始 `commands_raw` 和实际输入策略的 `commands`
+- RL 刚进入时会先 warmup,只预热观测历史和动作历史;warmup 期间仍保持默认站姿,结束后再按 `max_target_step` 从默认站姿逐步进入 RL 目标
+
+## 环境准备
+
+```bash
+cd /Users/chenyouyuan/cyy_ws/deploy_go1_pro
+conda activate free_dog_sdk
+```
+
+杀掉机器人上的 sport 进程:
+
+```bash
+ssh pi@192.168.123.161 "sudo pkill -9 -f keep_sport_alive; sudo pkill -9 -f Legged_sport; sudo pkill -9 -f appTransit"
+```
+
+或者在部署脚本里直接加 `--kill-sport`。
+
+脚本执行 `--kill-sport` 时,会打印并执行等价的 SSH 命令。如果不加
+`--kill-sport`,请务必手动执行上面的命令。
+
+## 启动前电机顺序检查
+
+真机脚本启动连接 MCU 前会自动检查:
+
+```text
+SDK 顺序必须是:
+FR_0 FR_1 FR_2
+FL_0 FL_1 FL_2
+RR_0 RR_1 RR_2
+RL_0 RL_1 RL_2
+
+策略顺序对应:
+FR_hip FR_thigh FR_calf
+FL_hip FL_thigh FL_calf
+RR_hip RR_thigh RR_calf
+RL_hip RL_thigh RL_calf
+```
+
+如果 SDK 顺序不一致,脚本会直接报错并停止,不会继续连接机器人。
+
+检查通过时会打印类似:
+
+```text
+[INFO] Joint order check passed: SDK and policy both use FR, FL, RR, RL.
+ [00] FR_0 -> FR_hip default=-0.100
+ [01] FR_1 -> FR_thigh default=+0.800
+ [02] FR_2 -> FR_calf default=-1.500
+ ...
+ [11] RL_2 -> RL_calf default=-1.500
+```
+
+连接成功后,脚本还会逐项打印当前电机角度和默认角度:
+
+```text
+[INFO] Initial joint positions (rad):
+ [00] FR_0: q=..., default=-0.100
+ ...
+ [11] RL_2: q=..., default=-1.500
+```
+
+## 第 1 步:只取数据
+
+只读 LowState、IMU、关节和遥控器数据,不发任何电机命令。
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py \
+ --monitor \
+ --kill-sport \
+ --log-dir logs
+```
+
+检查:
+
+- 电量正常
+- 机器人姿态变化时,RPY 会跟着变化
+- 12 个关节位置不是全 0
+- 遥控器按键和摇杆变化能正常打印
+
+## 第 2 步:测试遥控器
+
+继续用 `--monitor`,依次测试:
+
+- 按 `R2`
+- 按 `L2`
+- 左摇杆前后、左右
+- 右摇杆左右
+
+期望结果:
+
+- `pressed`、`rising`、`falling` 会正确变化
+- `lx`、`ly`、`rx` 会正确变化
+- 不会有任何电机动作
+
+## 第 3 步:测试观测构建
+
+只构建 45 维观测和 225 维 ONNX 历史输入,不做 RL 控制。
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py \
+ --obs-check \
+ --kill-sport \
+ --log-dir logs
+```
+
+检查:
+
+- 站立时重力投影接近 `[0, 0, -1]`
+- 指令观测会随遥控器变化
+- `q-qd max` 在合理范围内
+- ONNX 输入形状是 `(1, 225)`
+
+## 第 4 步:只测试 ONNX 推理
+
+只跑 ONNX 推理,不发 RL 电机目标。
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py \
+ --infer-check \
+ --kill-sport \
+ --log-dir logs
+```
+
+检查:
+
+- `action_raw max` 不会突然爆大
+- 零指令时动作应比较稳定
+- 长时间给 yaw 指令时,动作不应持续发散
+
+## 第 5 步:测试状态机和退出
+
+不加 `--enable-rl`,这样只会走校准、保持、观测和推理层,不会真正进入 RL 电机控制。
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py \
+ --kill-sport \
+ --log-dir logs
+```
+
+### 遥控器操作步骤
+
+先把机器人悬空,四条腿下方不要站人,手指始终放在 `L2` 上。每按一次 `R2` 后都要等终端打印出对应状态,并观察 5 到 10 秒再按下一次。
+
+```text
+启动后:
+ 状态应为 IDLE
+ 电机处于阻尼/空闲,不应主动站立
+
+第 1 次按 R2:
+ IDLE -> CALIBRATE -> HOLD
+ 机器人会缓慢回到默认站姿
+ 等终端打印 [STATE] HOLD
+
+第 2 次按 R2:
+ HOLD -> OBS_TEST
+ 电机仍保持默认站姿
+ 只构建观测,不使用 ONNX 动作控制电机
+
+第 3 次按 R2:
+ OBS_TEST -> INFER_TEST
+ 电机仍保持默认站姿
+ 只运行 ONNX 推理,动作只写日志,不控制电机
+
+第 4 次按 R2:
+ 未加 --enable-rl 时会被守卫拦住
+ 终端应打印 RL blocked
+
+任意激活状态按 L2:
+ 立即回到 IDLE 阻尼
+
+Ctrl+C:
+ 调用 safe_stop() 并退出脚本
+```
+
+必须确认:
+
+```text
+R2 每次只前进一层
+L2 在 HOLD / OBS_TEST / INFER_TEST 都能回到 IDLE
+松开 R2/L2 后,日志里 rising/falling 不应连续乱跳
+摇杆不动时,lx/ly/rx/ry 应接近 0
+```
+
+检查:
+
+- 校准会缓慢回到默认站姿
+- `HOLD` 会保持默认站姿
+- `L2` 能回到 `IDLE`
+- `Ctrl+C` 会正常打印 `Safe stopping...` 并退出
+
+## 第 6 步:悬空 RL 测试
+
+只有前 1 到 5 步都稳定后,才加 `--enable-rl` 进行悬空 RL。第一轮必须先隔离遥控器摇杆输入,用 `--no-rc` 固定零指令,只用遥控器按键切状态:
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py \
+ --kill-sport \
+ --enable-rl \
+ --no-rc \
+ --cmd-x 0 --cmd-y 0 --cmd-yaw 0 \
+ --log-dir logs \
+ --kp 60 --kd 1.0 \
+ --kp-cal 20 --kd-cal 1.0 \
+ --power-factor 6 \
+ --position-protect-limit 0.8 \
+ --action-clip 2.5 \
+ --action-trip-limit 8.0 \
+ --max-target-step 0.04 \
+ --max-roll-deg 25 \
+ --max-pitch-deg 25
+```
+
+### RL 遥控器操作步骤
+
+第一轮带 `--no-rc`,摇杆不会进入策略,只用 `R2` 和 `L2` 切状态。每一步都等终端状态确认后再继续。
+
+```text
+启动后:
+ 状态应为 IDLE
+
+第 1 次按 R2:
+ IDLE -> CALIBRATE -> HOLD
+ 等站姿稳定
+
+第 2 次按 R2:
+ HOLD -> OBS_TEST
+ 等 5 到 10 秒,确认 RPY、q 和 dq 稳定
+
+第 3 次按 R2:
+ OBS_TEST -> INFER_TEST
+ 等 5 到 10 秒,确认 action_raw_max 不突然变大
+
+第 4 次按 R2:
+ INFER_TEST -> RL
+ 先只观察 10 到 20 秒,不推摇杆
+
+RL 中再按 R2:
+ RL -> HOLD
+ 用来从 RL 平稳退回保持站姿
+
+任意异常立即按 L2:
+ 回到 IDLE 阻尼
+
+需要退出脚本:
+ 先按 L2 回 IDLE
+ 再 Ctrl+C
+```
+
+悬空 RL 时:
+
+- 先用 `--no-rc` 保持零指令
+- 零指令稳定后,再取消 `--no-rc` 并用很小的摇杆比例
+- 观察 `action_raw_max`、`action_safe_max`、RPY 和关节目标
+- 刚进入 RL 的前 `warmup_steps` 帧不会真正发送 RL 目标,之后目标应按 `max_target_step` 平滑变化
+- 一旦动作或姿态不对,立刻按 `L2`
+
+## 参数逐层强化
+
+建议先用小参数,再逐层加到当前目标值。前一层如果还不稳定,不要跳层。每层至少稳定 1 到 2 分钟再升级。
+
+### 第 0 层:完全不控电机
+
+先跑:
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py --monitor --kill-sport --log-dir logs
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py --obs-check --kill-sport --log-dir logs
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py --infer-check --kill-sport --log-dir logs
+```
+
+只有在状态、遥控、观测和动作日志都正常时,才进入下一层。
+
+### 第 1 层:弱校准和保持
+
+先不进 RL,只测试起立、保持、`R2`、`L2` 和 `Ctrl+C`。
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py \
+ --kill-sport \
+ --log-dir logs \
+ --kp 20 --kd 0.7 \
+ --kp-cal 10 --kd-cal 0.7 \
+ --power-factor 3 \
+ --position-protect-limit 0.4
+```
+
+只有在校准平滑、能稳定保持、`L2` 能可靠回到 `IDLE` 时才升级。
+
+### 第 2 层:中等校准和保持
+
+仍然不进 RL。
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py \
+ --kill-sport \
+ --log-dir logs \
+ --kp 40 --kd 0.8 \
+ --kp-cal 15 --kd-cal 0.8 \
+ --power-factor 5 \
+ --position-protect-limit 0.6
+```
+
+只有在保持姿态稳定、关节不抖时才继续。
+
+### 第 3 层:保守悬空 RL,固定零指令
+
+开始允许 RL,但固定零指令,先确认策略本体能悬空稳定,不测试摇杆速度指令。
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py \
+ --kill-sport \
+ --enable-rl \
+ --no-rc \
+ --cmd-x 0 --cmd-y 0 --cmd-yaw 0 \
+ --log-dir logs \
+ --kp 60 --kd 1.0 \
+ --kp-cal 20 --kd-cal 1.0 \
+ --power-factor 6 \
+ --position-protect-limit 0.8 \
+ --action-clip 2.5 \
+ --action-trip-limit 8.0 \
+ --max-target-step 0.04 \
+ --max-roll-deg 25 \
+ --max-pitch-deg 25
+```
+
+只有在悬空 RL 零指令稳定时才进入下一层。
+
+### 第 3.5 层:保守悬空 RL,小遥控比例
+
+取消 `--no-rc`,但先把摇杆比例限制到小范围。脚本默认还会对遥控器命令做 deadzone、低通和变化率限制。
+
+如果加 `--swap-vy-yaw`,遥控映射会变成:
+
+```text
+左摇杆前后 ly -> vx
+左摇杆左右 lx -> yaw
+右摇杆左右 rx -> vy
+```
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py \
+ --kill-sport \
+ --enable-rl \
+ --log-dir logs \
+ --kp 60 --kd 1.0 \
+ --kp-cal 20 --kd-cal 1.0 \
+ --power-factor 6 \
+ --position-protect-limit 0.8 \
+ --action-clip 2.5 \
+ --action-trip-limit 8.0 \
+ --max-target-step 0.06 \
+ --max-roll-deg 25 \
+ --max-pitch-deg 25 \
+ --swap-vy-yaw \
+ --rc-vx-scale 0.3 \
+ --rc-vy-scale 0.15 \
+ --rc-wz-scale 0.5
+```
+
+这一层不要直接长时间满杆。建议按下面顺序测,每次给很短的摇杆脉冲,松杆回中后等姿态稳定再继续:
+
+```text
+进入 RL 后先零摇杆观察 10 秒
+
+左摇杆前后:
+ ly 轻推到约 1/3 行程,保持 0.5 到 1 秒,松开
+ 观察 roll/pitch、qvel、action_raw_max
+
+左摇杆左右:
+ 加 --swap-vy-yaw 后控制 yaw
+ lx 轻推到约 1/3 行程,保持 0.5 到 1 秒,松开
+ 如果 yaw 仍慢,可以逐步把 --rc-wz-scale 从 0.5 提到 0.7
+
+右摇杆左右:
+ 加 --swap-vy-yaw 后控制 vy
+ rx 轻推到约 1/3 行程,保持 0.5 到 1 秒,松开
+ 观察横向动作是否明显发散
+
+任何时候:
+ action_raw_max 接近 2.0、qvel 明显变大、roll/pitch 持续变大,立即按 L2
+```
+
+这一层重点检查 `commands_raw` 和 `commands`:默认无命令滤波时两者应基本一致。如果手动启用了 `--cmd-ema-alpha` 或 `--max-cmd-step-*`,则 `commands` 会比 `commands_raw` 更慢变化。
+
+### 第 4 层:当前目标参数
+
+这是当前想要的正式参数。只有第 3 层和第 3.5 层都稳定后再用。
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py \
+ --kill-sport \
+ --enable-rl \
+ --log-dir logs \
+ --kp 80 --kd 1.0 \
+ --kp-cal 20 --kd-cal 1.0 \
+ --power-factor 7 \
+ --position-protect-limit 1.0 \
+ --action-clip 4.0 \
+ --action-trip-limit 12.0 \
+ --max-target-step 0.08 \
+ --swap-vy-yaw \
+ --rc-vx-scale 0.5 \
+ --rc-vy-scale 0.25 \
+ --rc-wz-scale 0.7
+```
+
+这些值仍低于全遥控比例,用来逐步靠近正式测试;不要直接用满 `1.0/0.5/1.0`。
+
+## 第 7 步:上地面测试
+
+只有悬空 RL 稳定后,才能落地。
+
+1. 把机器人放到平地上
+2. 零指令进入 RL
+3. 先让它站稳
+4. 再给小指令
+
+建议起始范围:
+
+```text
+ly <= 0.2
+lx <= 0.1
+rx <= 0.2
+```
+
+如果出现以下情况,立即按 `L2`:
+
+- 动作越来越大
+- roll 或 pitch 持续变大
+- 腿乱甩
+- 机器人开始摔倒或打滑
+
+### 15 cm 台阶测试参数
+
+`stairs_3.xml` 的单级高度约 `0.14 m`。从 MuJoCo 采样和真机日志看,上台阶时 pitch 到 `25 deg` 以上、roll 短时超过 `35 deg` 都可能出现。台阶测试不要继续用平地/悬空的 `25 deg` 或 `35 deg` 姿态保护。
+
+建议台阶第一轮用下面这组。它比平地测试放开姿态和动作幅度,但仍保留目标位置变化率限制,避免单帧目标跳变太大:
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py \
+ --kill-sport \
+ --enable-rl \
+ --log-dir logs \
+ --kp 40 --kd 0.8 \
+ --kp-cal 20 --kd-cal 1.0 \
+ --power-factor 6 \
+ --position-protect-limit 1.2 \
+ --action-clip 4.0 \
+ --action-trip-limit 9.0 \
+ --max-target-step 0.15 \
+ --max-roll-deg 45 \
+ --max-pitch-deg 45 \
+ --swap-vy-yaw \
+ --rc-vx-scale 0.5 \
+ --rc-vy-scale 0.5 \
+ --rc-wz-scale 1.0
+```
+
+如果动作幅度仍明显不够,再把 `--max-target-step` 从 `0.15` 加到 `0.20`。只有确认目标变化率限制确实挡住抬腿时,才短时间对照测试 `--max-target-step 0`。
+
+`--action-trip-limit 9.0` 只是把 raw action 的异常停机阈值从 `8.0` 放宽到 `9.0`;实际发给关节目标的动作仍会先被 `--action-clip 4.0` 裁剪,所以它不会直接增加腿部动作幅度。如果 raw action 经常超过 `9.0`,说明观测或姿态已经明显偏离训练分布,应按 `L2` 退回而不是继续放大阈值。
+
+`--max-target-step 0` 的意思是关闭单步目标变化率限制,不是关闭关节硬限位、action clip 或 SDK 安全保护。真机日志 `rlgym_go1_deploy_20260724_172122` 里,`max_target_step=0`、`position_protect_limit=1.5`、`power_factor=9` 时触发的是实测扭矩保护:`tauEst` 已超过 SDK 的 `TAU_MAX`,所以脚本会进入 `FAULT` 并阻尼停机。
+
+这只是放宽姿态保护,不是关闭全部安全保护。`action_trip_limit`、`max_dof_vel`、`max_gyro`、`position_protect_limit`、实测扭矩保护和 `L2` 急停仍然保留。
+
+注意:`--power-factor` 主要限制命令力矩 `tau` 的比例;这个部署脚本发送的是位置控制,命令 `tau=0`。实测 `tauEst` 超过 SDK `TAU_MAX` 时,即使 `power_factor=9` 也会停机。遇到这种停机,优先减小瞬时目标跳变或速度指令,不要继续只增大 `power_factor`。
+
+目前台阶实测后的建议顺序:
+
+```text
+第一轮: position_protect_limit=1.2, max_target_step=0.15, power_factor=6
+第二轮: position_protect_limit=1.2, max_target_step=0.20, power_factor=6
+第三轮: position_protect_limit=1.5, max_target_step=0.20, power_factor=6 或 7
+短时对照: position_protect_limit=1.5, max_target_step=0, power_factor=6 或 7
+```
+
+如果出现 `PowerProtectViolation`,说明已经是实测扭矩过载边缘。下一轮建议回退到上一层参数,或者降低 `--rc-vx-scale` / 避免连续满前进杆。
+
+## MuJoCo 键盘 sim2sim
+
+键盘控制测试直接用本目录的 `deploy_go1_onnx_mujoco.py`。它加载同一个 `policy_15k.onnx`,使用 45 维观测和 5 帧 ONNX 历史,在 MuJoCo 里做位置 PD 控制。
+
+平地启动:
+
+```bash
+cd /Users/chenyouyuan/cyy_ws/deploy_go1_pro
+conda activate free_dog_sdk
+
+mjpython deploy_45dim_rl_gym/deploy_go1_onnx_mujoco.py \
+ --clip-actions 4.0 \
+ --max-target-step 0.15
+```
+
+楼梯 level 3 启动:
+
+```bash
+cd /Users/chenyouyuan/cyy_ws/deploy_go1_pro
+conda activate free_dog_sdk
+
+mjpython deploy_45dim_rl_gym/deploy_go1_onnx_mujoco.py \
+ --terrain terrains/stairs/stairs_3.xml \
+ --spawn-x -0.6 \
+ --spawn-z 0.34 \
+ --clip-actions 4.0 \
+ --max-target-step 0.15
+```
+
+如果你想对照策略原始动作能力,可以短时间把 `--max-target-step` 改成 `0.0`:
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_onnx_mujoco.py \
+ --terrain terrains/stairs/stairs_3.xml \
+ --spawn-x -0.6 \
+ --spawn-z 0.34 \
+ --clip-actions 4.0 \
+ --max-target-step 0.0
+```
+
+键盘控制:
+
+```text
+方向键 ↑ / ↓ : 前进 / 后退
+方向键 ← / → : 左转 / 右转 yaw
+, / . : 左移 / 右移 vy
+K : 零速度指令
+R : 重置机器人
+Esc : 退出
+```
+
+如果键盘没有响应,先点一下 MuJoCo 窗口或终端,让当前窗口获得焦点。macOS 第一次使用 `pynput` 可能需要给终端或 Python 辅助功能权限。
+
+## MuJoCo 楼梯采样
+
+可以先在 MuJoCo 里固定前进指令采样楼梯 level 3。`stairs_3.xml` 的单级高度约 `0.14 m`,接近 `15 cm` 台阶。
+
+默认前进速度、真机放开动作幅度、但保留较严格目标步长限制:
+
+```bash
+conda activate free_dog_sdk
+
+mjpython deploy_45dim_rl_gym/deploy_go1_onnx_mujoco.py \
+ --sample-stairs-level 3 \
+ --sample-seconds 10 \
+ --sample-cmd-x 1.0 \
+ --sample-cmd-y 0 \
+ --sample-cmd-yaw 0 \
+ --clip-actions 4.0 \
+ --max-target-step 0.08 \
+ --sample-log-dir logs
+```
+
+如果 `0.08` 在楼梯上动作太小,可以用下面这个对照测试策略原始能力:
+
+```bash
+mjpython deploy_45dim_rl_gym/deploy_go1_onnx_mujoco.py \
+ --sample-stairs-level 3 \
+ --sample-seconds 10 \
+ --sample-cmd-x 1.0 \
+ --sample-cmd-y 0 \
+ --sample-cmd-yaw 0 \
+ --clip-actions 4.0 \
+ --max-target-step 0.0 \
+ --sample-log-dir logs
+```
+
+采样日志会保存:
+
+```text
+logs/mujoco_stairs_sample_YYYYMMDD_HHMMSS/
+ metadata.json
+ steps.jsonl
+ summary.json
+```
+
+重点看:
+
+- `fallen`
+- `base_x_progress`
+- `rpy_min_deg` / `rpy_max_deg`
+- `action_raw_maxabs`
+- `action_applied_maxabs`
+- `target_offset_maxabs`
+- `dof_vel_maxabs`
+
+## 日志
+
+日志会保存在:
+
+```text
+logs/rlgym_go1_deploy_YYYYMMDD_HHMMSS/
+ metadata.json
+ steps.jsonl
+```
+
+常看字段:
+
+- `commands`
+- `commands_raw`
+- `obs_single`
+- `action_raw`
+- `action_safe`
+- `joint_targets`
+- `dof_pos`
+- `dof_vel`
+- `base_ang_vel`
+- `projected_gravity`
+- `imu_rpy_deg`
+- `state_reason`
+
+## 常用默认值
+
+真机部署脚本使用的默认值如下:
+
+```text
+kp = 80.0
+kd = 1.0
+kp_cal = 20.0
+kd_cal = 1.0
+power_factor = 7
+position_protect_limit = 1.0
+action_ema_alpha = 0.0
+rc_deadzone = 0.05
+cmd_ema_alpha = 1.0
+max_cmd_step_x = 0.0
+max_cmd_step_y = 0.0
+max_cmd_step_yaw = 0.0
+rate_hz = 50.0
+```
+
+这些值来自之前成功的 Go1 PRO 低层部署经验。`rlgym_go1_deploy_20260724_193128` 和 `rlgym_go1_deploy_20260724_193318` 对照显示,当前 45 维策略在真机上关闭遥控器命令低通/变化率限制更稳定;如需重新启用滤波,应只作为单独变量短时对照测试。
+
+RoboGauge 策略自己的参数不要照搬旧的 57 维策略:
+
+```text
+action_scale = 0.25
+default_dof_pos = FR, FL, RR, RL 顺序下的 [-0.1/0.1, 0.8/1.0, -1.5]
+history_len = 5
+onnx_input_dim = 225
+```
diff --git a/deploy_45dim_rl_gym/__pycache__/deploy_go1_onnx_mujoco.cpython-310.pyc b/deploy_45dim_rl_gym/__pycache__/deploy_go1_onnx_mujoco.cpython-310.pyc
new file mode 100644
index 0000000..4c3daf1
Binary files /dev/null and b/deploy_45dim_rl_gym/__pycache__/deploy_go1_onnx_mujoco.cpython-310.pyc differ
diff --git a/deploy_45dim_rl_gym/__pycache__/deploy_go1_onnx_mujoco.cpython-313.pyc b/deploy_45dim_rl_gym/__pycache__/deploy_go1_onnx_mujoco.cpython-313.pyc
new file mode 100644
index 0000000..a5e796f
Binary files /dev/null and b/deploy_45dim_rl_gym/__pycache__/deploy_go1_onnx_mujoco.cpython-313.pyc differ
diff --git a/deploy_45dim_rl_gym/__pycache__/deploy_go1_onnx_mujoco.cpython-314.pyc b/deploy_45dim_rl_gym/__pycache__/deploy_go1_onnx_mujoco.cpython-314.pyc
new file mode 100644
index 0000000..318fc16
Binary files /dev/null and b/deploy_45dim_rl_gym/__pycache__/deploy_go1_onnx_mujoco.cpython-314.pyc differ
diff --git a/deploy_45dim_rl_gym/__pycache__/deploy_go1_rlgym_pro_sdk.cpython-310.pyc b/deploy_45dim_rl_gym/__pycache__/deploy_go1_rlgym_pro_sdk.cpython-310.pyc
new file mode 100644
index 0000000..3e060ee
Binary files /dev/null and b/deploy_45dim_rl_gym/__pycache__/deploy_go1_rlgym_pro_sdk.cpython-310.pyc differ
diff --git a/deploy_45dim_rl_gym/__pycache__/deploy_go1_rlgym_pro_sdk.cpython-314.pyc b/deploy_45dim_rl_gym/__pycache__/deploy_go1_rlgym_pro_sdk.cpython-314.pyc
new file mode 100644
index 0000000..5b0394a
Binary files /dev/null and b/deploy_45dim_rl_gym/__pycache__/deploy_go1_rlgym_pro_sdk.cpython-314.pyc differ
diff --git a/deploy_45dim_rl_gym/assets/calf.stl b/deploy_45dim_rl_gym/assets/calf.stl
new file mode 100644
index 0000000..3e8ac94
Binary files /dev/null and b/deploy_45dim_rl_gym/assets/calf.stl differ
diff --git a/deploy_45dim_rl_gym/assets/hip.stl b/deploy_45dim_rl_gym/assets/hip.stl
new file mode 100644
index 0000000..ae590bf
Binary files /dev/null and b/deploy_45dim_rl_gym/assets/hip.stl differ
diff --git a/deploy_45dim_rl_gym/assets/thigh.stl b/deploy_45dim_rl_gym/assets/thigh.stl
new file mode 100644
index 0000000..353801b
Binary files /dev/null and b/deploy_45dim_rl_gym/assets/thigh.stl differ
diff --git a/deploy_45dim_rl_gym/assets/thigh_mirror.stl b/deploy_45dim_rl_gym/assets/thigh_mirror.stl
new file mode 100644
index 0000000..11ef770
Binary files /dev/null and b/deploy_45dim_rl_gym/assets/thigh_mirror.stl differ
diff --git a/deploy_45dim_rl_gym/assets/trunk.stl b/deploy_45dim_rl_gym/assets/trunk.stl
new file mode 100644
index 0000000..2659eb1
Binary files /dev/null and b/deploy_45dim_rl_gym/assets/trunk.stl differ
diff --git a/deploy_45dim_rl_gym/deploy_go1_onnx_mujoco.py b/deploy_45dim_rl_gym/deploy_go1_onnx_mujoco.py
new file mode 100644
index 0000000..63cb072
--- /dev/null
+++ b/deploy_45dim_rl_gym/deploy_go1_onnx_mujoco.py
@@ -0,0 +1,738 @@
+#!/usr/bin/env python3
+"""
+Go1 MoE CTS ONNX Policy — MuJoCo Simulation Deployment.
+
+Loads the 15k ONNX policy exported from RoboGauge and runs it in MuJoCo
+with PD position control. The ONNX model uses a 5-frame CTS history
+(225-dim stacked-by-terms input, stateless).
+
+Usage:
+ conda activate free_dog_sdk
+ MUJOCO_GL=glfw python deploy_go1_onnx_mujoco.py
+ MUJOCO_GL=glfw python deploy_go1_onnx_mujoco.py --terrain terrains/stairs/stairs_6.xml
+
+Controls (matching RoboGauge keyboard convention):
+ ↑ / ↓ forward / backward
+ ← / → yaw left / right
+ , / . strafe left / right
+ K stop
+ R reset robot
+ Esc quit
+
+Architecture:
+ ONNX input: obs [1, 225] — 5 frames × 45 dims, stacked by TERMS
+ ONNX output: actions [1, 12], weights [1, 8], latent [1, 32]
+ Control: target_q = default_q + 0.25 * action, PD with Kp=28, Kd=0.7
+"""
+
+import argparse
+import json
+import os
+import re
+import signal
+import sys
+import time
+from collections import deque
+from datetime import datetime
+from pathlib import Path
+
+import mujoco
+import numpy as np
+import onnxruntime as ort
+from mujoco import viewer
+
+# ── path setup ──
+SCRIPT_DIR = Path(__file__).resolve().parent
+DEFAULT_ONNX = str(SCRIPT_DIR / "policy_25k.onnx")
+ROBOT_XML = str(SCRIPT_DIR / "go1.xml")
+TERRAINS_DIR = SCRIPT_DIR / "terrains"
+
+# ── policy constants (RoboGauge Go1Config) ──
+NUM_OBS = 45
+NUM_ACTIONS = 12
+HISTORY_LEN = 5 # CTS history frames
+ONNX_INPUT_DIM = 225 # 45 × 5, stacked by terms
+
+ACTION_SCALE = 0.25
+KP = 28.0
+KD = 0.7
+CLIP_ACTIONS = 100.0
+CLIP_OBS = 100.0
+
+ANG_VEL_SCALE = 0.25
+CMD_SCALE = np.array([2.0, 2.0, 0.25], dtype=np.float32)
+
+MAX_LIN_VEL_X = 1.0
+MAX_LIN_VEL_Y = 0.5
+MAX_ANG_VEL = 1.0
+
+DEFAULT_DOF_POS = np.array([
+ -0.1, 0.8, -1.5, # FR_hip, FR_thigh, FR_calf
+ 0.1, 0.8, -1.5, # FL_hip, FL_thigh, FL_calf
+ -0.1, 1.0, -1.5, # RR_hip, RR_thigh, RR_calf
+ 0.1, 1.0, -1.5, # RL_hip, RL_thigh, RL_calf
+], dtype=np.float32)
+
+# ── exit flag ──
+EXIT = False
+
+def _sig_handler(signum, frame):
+ global EXIT
+ EXIT = True
+
+signal.signal(signal.SIGINT, _sig_handler)
+signal.signal(signal.SIGTERM, _sig_handler)
+
+
+# ═══════════════════════════════════════════════════════════════
+# XML builder — merge terrain + robot into one MuJoCo scene
+# ═══════════════════════════════════════════════════════════════
+
+def _xml_inner(text, tag):
+ """Extract inner content of the first ... in text."""
+ m = re.search(rf"<{tag}>(.*?){tag}>", text, re.DOTALL)
+ return m.group(1).strip() if m else ""
+
+
+def build_scene_xml(robot_xml_path, terrain_xml_path=None):
+ """Merge robot XML with optional terrain XML into a single scene model.
+
+ The robot XML (go1.xml) has base_link as the root body with no free joint.
+ We add a free joint and optionally merge terrain worldbody/asset elements.
+ """
+ robot = Path(robot_xml_path).read_text()
+
+ # 1) Add free joint to base_link
+ robot = robot.replace(
+ '
',
+ '\n '
+ )
+
+ if terrain_xml_path is None:
+ # No terrain — add a simple flat floor
+ floor = (
+ '\n \n'
+ )
+ robot = robot.replace(
+ ' into robot
+ terrain_assets = _xml_inner(terrain_text, "asset")
+ if terrain_assets:
+ # Insert before closing of robot (or before if no asset)
+ robot = robot.replace('', '\n' + terrain_assets + '\n ', 1)
+
+ # 3) Merge terrain settings
+ terrain_visual = _xml_inner(terrain_text, "visual")
+ if terrain_visual:
+ robot = robot.replace('', '\n' + terrain_visual + '\n ', 1)
+
+ # 4) Merge terrain worldbody elements (lights, geoms, etc.) before base_link
+ terrain_wb = _xml_inner(terrain_text, "worldbody")
+ if terrain_wb:
+ # Remove elements from terrain worldbody (we only want geoms/lights/cameras)
+ terrain_wb_no_bodies = re.sub(r'', '', terrain_wb, flags=re.DOTALL)
+ robot = robot.replace(
+ '= 0
+ has_imu_quat = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, "Body_Quat") >= 0
+ print(f"[INFO] IMU sensors: Body_Gyro={has_imu_gyro}, Body_Quat={has_imu_quat}")
+
+ # ── Load ONNX ──
+ session = ort.InferenceSession(args.onnx, providers=['CPUExecutionProvider'])
+ inp = session.get_inputs()[0]
+ print(f"[INFO] ONNX input : {inp.name} {inp.shape}")
+ for o in session.get_outputs():
+ print(f"[INFO] ONNX output: {o.name} {o.shape}")
+
+ # ── Init state ──
+ data.qpos[0:3] = np.array([args.spawn_x, args.spawn_y, args.spawn_z], dtype=np.float64)
+ data.qpos[3:7] = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64)
+ data.qpos[7:19] = DEFAULT_DOF_POS.astype(np.float64)
+ data.qvel[:] = 0.0
+ data.ctrl[:] = 0.0
+ mujoco.mj_forward(model, data)
+
+ # ── Control state ──
+ ctrl_dt = 0.02 # 50 Hz
+ sim_dt = model.opt.timestep
+ steps_per_inference = max(1, int(ctrl_dt / sim_dt))
+ print(f"[INFO] control: {ctrl_dt}s ({1/ctrl_dt:.0f}Hz), "
+ f"sim steps per inference: {steps_per_inference}")
+
+ last_action = np.zeros(NUM_ACTIONS, dtype=np.float32)
+ action_raw = np.zeros(NUM_ACTIONS, dtype=np.float32)
+ action = np.zeros(NUM_ACTIONS, dtype=np.float32)
+ target_pos = DEFAULT_DOF_POS.copy()
+ prev_target_pos = DEFAULT_DOF_POS.copy()
+ obs_builder = ObsBuilder()
+ step_count = 0
+ inference_step = 0
+
+ if sample_mode:
+ cmd = np.array([
+ args.sample_cmd_x,
+ args.sample_cmd_y,
+ args.sample_cmd_yaw,
+ ], dtype=np.float32)
+ cmd = np.clip(cmd, [-MAX_LIN_VEL_X, -MAX_LIN_VEL_Y, -MAX_ANG_VEL],
+ [MAX_LIN_VEL_X, MAX_LIN_VEL_Y, MAX_ANG_VEL])
+ logger = SampleLogger(args.sample_log_dir, terrain_path, cmd, args)
+ samples = []
+ max_sim_steps = int(args.sample_seconds / sim_dt)
+ print(
+ f"[INFO] sampling: seconds={args.sample_seconds:.1f} "
+ f"cmd=({cmd[0]:.2f},{cmd[1]:.2f},{cmd[2]:.2f}) "
+ f"spawn=({args.spawn_x:.2f},{args.spawn_y:.2f},{args.spawn_z:.2f})"
+ )
+
+ while step_count < max_sim_steps and not EXIT:
+ if inference_step == 0:
+ imu_quat = read_sensor(model, data, "Body_Quat", 4)
+ imu_ang_vel = read_sensor(model, data, "Body_Gyro", 3)
+ quat_wxyz = imu_quat if imu_quat is not None else data.qpos[3:7].copy()
+ if imu_ang_vel is not None:
+ base_ang_vel_body = imu_ang_vel
+ else:
+ world_ang_vel = data.qvel[3:6].copy()
+ base_ang_vel_body = quat_rotate_inverse(quat_wxyz, world_ang_vel)
+
+ q = data.qpos[7:19].copy()
+ dq = data.qvel[6:18].copy()
+ obs_single = obs_builder.build_single_obs(
+ base_ang_vel_body, quat_wxyz, cmd, q, dq, last_action)
+ onnx_input = obs_builder.build_onnx_input(obs_single)
+ outputs = session.run(None, {'obs': onnx_input})
+ action_raw = outputs[0][0].astype(np.float32)
+ action = np.clip(action_raw, -args.clip_actions, args.clip_actions)
+ last_action = action.copy()
+
+ target_pos_raw = DEFAULT_DOF_POS + action * ACTION_SCALE
+ if args.max_target_step > 0.0:
+ delta = np.clip(
+ target_pos_raw - prev_target_pos,
+ -args.max_target_step,
+ args.max_target_step,
+ )
+ target_pos = prev_target_pos + delta
+ else:
+ target_pos = target_pos_raw
+ prev_target_pos = target_pos.copy()
+
+ current_pos = data.qpos[7:19]
+ current_vel = data.qvel[6:18]
+ torques = KP * (target_pos - current_pos) - KD * current_vel
+ torques = np.clip(torques, -33.5, 33.5)
+ data.ctrl[:] = torques.astype(np.float64)
+
+ mujoco.mj_step(model, data)
+
+ if inference_step == 0:
+ quat_wxyz = data.qpos[3:7].copy()
+ rpy_deg = quat_to_rpy_deg(quat_wxyz)
+ fallen = bool(
+ data.qpos[2] < 0.16
+ or abs(rpy_deg[0]) > 60.0
+ or abs(rpy_deg[1]) > 60.0
+ or not np.all(np.isfinite(data.qpos))
+ )
+ rec = {
+ "step": int(step_count),
+ "control_step": int(step_count // steps_per_inference),
+ "time_sim": float(data.time),
+ "cmd": cmd.astype(float).tolist(),
+ "base_pos": data.qpos[0:3].astype(float).tolist(),
+ "base_quat": quat_wxyz.astype(float).tolist(),
+ "rpy_deg": rpy_deg.astype(float).tolist(),
+ "base_lin_vel": data.qvel[0:3].astype(float).tolist(),
+ "base_ang_vel": data.qvel[3:6].astype(float).tolist(),
+ "dof_pos": current_pos.astype(float).tolist(),
+ "dof_vel": current_vel.astype(float).tolist(),
+ "action_raw": action_raw.astype(float).tolist(),
+ "action": action.astype(float).tolist(),
+ "target_pos": target_pos.astype(float).tolist(),
+ "target_offset": (target_pos - DEFAULT_DOF_POS).astype(float).tolist(),
+ "torques": torques.astype(float).tolist(),
+ "fallen": fallen,
+ }
+ logger.write(rec)
+ samples.append(rec)
+ if rec["control_step"] % 50 == 0:
+ print(
+ f"[sample {rec['control_step']:04d}] "
+ f"t={rec['time_sim']:.2f} x={rec['base_pos'][0]:.2f} "
+ f"z={rec['base_pos'][2]:.2f} rpy={np.round(rpy_deg, 1)} "
+ f"act_max={np.max(np.abs(action)):.2f} "
+ f"target_off={np.max(np.abs(target_pos - DEFAULT_DOF_POS)):.2f}"
+ )
+ if fallen:
+ print(f"[WARN] sample stopped: fallen at t={data.time:.2f}s")
+ break
+
+ step_count += 1
+ inference_step = (inference_step + 1) % steps_per_inference
+
+ summary = summarize_samples(samples)
+ (logger.run_dir / "summary.json").write_text(
+ json.dumps(summary, indent=2, ensure_ascii=False))
+ logger.close()
+ print("[INFO] sample summary:")
+ print(json.dumps(summary, indent=2, ensure_ascii=False))
+ print(f"[INFO] sample saved: {logger.run_dir}")
+ return 0
+
+ # ── Keyboard ──
+ kb = KbReader()
+ kb.start()
+
+ # ── Viewer ──
+ view = viewer.launch_passive(model, data)
+ # Track the robot body
+ body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "base_link")
+ if body_id >= 0:
+ view.cam.type = mujoco.mjtCamera.mjCAMERA_TRACKING
+ view.cam.trackbodyid = body_id
+ view.cam.distance = 2.5
+ view.cam.elevation = -20
+ view.cam.azimuth = 60
+ print("[INFO] viewer launched")
+
+ loop_start = time.time()
+
+ while view.is_running() and not EXIT:
+ held = kb.snapshot()
+
+ # ── Quit ──
+ if 'key.esc' in held:
+ break
+
+ # ── Command from keyboard ──
+ vx, vy, yaw = get_command(held)
+ cmd = np.array([vx, vy, yaw], dtype=np.float32)
+
+ # ── Reset ──
+ if 'r' in held:
+ data.qpos[0:3] = np.array([0.0, 0.0, 0.34], dtype=np.float64)
+ data.qpos[3:7] = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64)
+ data.qpos[7:19] = DEFAULT_DOF_POS.astype(np.float64)
+ data.qvel[:] = 0.0
+ data.ctrl[:] = 0.0
+ last_action = np.zeros(NUM_ACTIONS, dtype=np.float32)
+ action_raw = np.zeros(NUM_ACTIONS, dtype=np.float32)
+ action = np.zeros(NUM_ACTIONS, dtype=np.float32)
+ target_pos = DEFAULT_DOF_POS.copy()
+ prev_target_pos = DEFAULT_DOF_POS.copy()
+ obs_builder.reset()
+ mujoco.mj_forward(model, data)
+ print("[RESET]")
+
+ # ── Inference ──
+ if inference_step == 0:
+ # Match RoboGauge: policy obs uses XML IMU gyro and framequat sensors.
+ # Fall back to qpos/qvel only for XMLs without those sensors.
+ imu_quat = read_sensor(model, data, "Body_Quat", 4)
+ imu_ang_vel = read_sensor(model, data, "Body_Gyro", 3)
+ quat_wxyz = imu_quat if imu_quat is not None else data.qpos[3:7].copy()
+ if imu_ang_vel is not None:
+ base_ang_vel_body = imu_ang_vel
+ else:
+ world_ang_vel = data.qvel[3:6].copy()
+ base_ang_vel_body = quat_rotate_inverse(quat_wxyz, world_ang_vel)
+
+ # Joint state (qpos[7:19] is FR,FL,RR,RL — matches policy order)
+ q = data.qpos[7:19].copy()
+ dq = data.qvel[6:18].copy()
+
+ # Build obs
+ obs_single = obs_builder.build_single_obs(
+ base_ang_vel_body, quat_wxyz, cmd, q, dq, last_action)
+ onnx_input = obs_builder.build_onnx_input(obs_single)
+
+ # Run ONNX
+ outputs = session.run(None, {'obs': onnx_input})
+ action_raw = outputs[0][0].astype(np.float32) # [12]
+ action = np.clip(action_raw, -args.clip_actions, args.clip_actions)
+ last_action = action.copy()
+
+ target_pos_raw = DEFAULT_DOF_POS + action * ACTION_SCALE
+ if args.max_target_step > 0.0:
+ delta = np.clip(
+ target_pos_raw - prev_target_pos,
+ -args.max_target_step,
+ args.max_target_step,
+ )
+ target_pos = prev_target_pos + delta
+ else:
+ target_pos = target_pos_raw
+ prev_target_pos = target_pos.copy()
+
+ # ── PD control ──
+ current_pos = data.qpos[7:19]
+ current_vel = data.qvel[6:18]
+ torques = KP * (target_pos - current_pos) - KD * current_vel
+ torques = np.clip(torques, -33.5, 33.5)
+ data.ctrl[:] = torques.astype(np.float64)
+
+ mujoco.mj_step(model, data)
+ view.sync()
+
+ # Real-time sync — use sim_dt because step_count increments every sim step
+ expected_time = step_count * sim_dt
+ elapsed = time.time() - loop_start
+ if 0 < expected_time - elapsed < ctrl_dt:
+ time.sleep(expected_time - elapsed)
+
+ step_count += 1
+ inference_step = (inference_step + 1) % steps_per_inference
+
+ # Periodic status
+ if step_count % 200 == 0:
+ z = data.qpos[2]
+ lin_vel_abs = np.linalg.norm(data.qvel[0:3])
+ print(f"[{step_count}] cmd=({vx:.1f},{vy:.1f},{yaw:.1f}) "
+ f"z={z:.3f} |v|={lin_vel_abs:.2f} "
+ f"act[0:4]={np.round(action[:4], 3)}")
+
+ kb.stop()
+ view.close()
+ print("[INFO] done.")
+ return 0
+
+
+if __name__ == "__main__":
+ sys.exit(main())
diff --git a/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py b/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py
new file mode 100644
index 0000000..9df64d1
--- /dev/null
+++ b/deploy_45dim_rl_gym/deploy_go1_rlgym_pro_sdk.py
@@ -0,0 +1,1112 @@
+#!/usr/bin/env python3
+"""
+Deploy the RoboGauge Go1 45-dim RL-Gym ONNX policy on Unitree Go1 PRO.
+
+This uses go1_pro_sdk direct MCU control, not LCM or the official Unitree SDK.
+
+Policy:
+ - policy_15k.onnx
+ - single-frame obs: 45 dims
+ - ONNX input: 5-frame history, 225 dims, 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, ONNX 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 time
+from collections import deque
+from datetime import datetime
+from enum import Enum
+from pathlib import Path
+
+import numpy as np
+import onnxruntime as ort
+
+from go1_pro_sdk import (
+ MCUClient, LowCmd, MotorCmd, MotorMode,
+ apply_safety, PowerProtectViolation, JOINT_NAMES,
+)
+
+
+HERE = Path(__file__).parent.resolve()
+DEFAULT_ONNX = HERE / "policy_15k.onnx"
+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
+ONNX_INPUT_DIM = NUM_OBS * HISTORY_LEN
+
+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",
+]
+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 validate_joint_order():
+ sdk_names = list(JOINT_NAMES)
+ if sdk_names != EXPECTED_SDK_JOINT_NAMES:
+ raise RuntimeError(
+ "go1_pro_sdk JOINT_NAMES order mismatch.\n"
+ f" expected: {EXPECTED_SDK_JOINT_NAMES}\n"
+ f" actual : {sdk_names}"
+ )
+ print("[INFO] Joint order check passed: SDK and policy both use FR, FL, RR, RL.")
+ for i, (sdk_name, policy_name, q0) in enumerate(
+ zip(sdk_names, POLICY_JOINT_NAMES, DEFAULT_DOF_POS)):
+ print(f" [{i:02d}] {sdk_name:4s} -> {policy_name:8s} default={q0:+.3f}")
+
+
+def apply_deadzone(value, deadzone):
+ if deadzone <= 0.0:
+ return float(value)
+ mag = abs(float(value))
+ if mag <= deadzone:
+ return 0.0
+ return float(np.sign(value) * (mag - deadzone) / max(1e-6, 1.0 - deadzone))
+
+
+def get_command(state, args):
+ if args.no_rc:
+ cmd = np.array([args.cmd_x, args.cmd_y, args.cmd_yaw], dtype=np.float32)
+ else:
+ r = state.remote
+ ly = apply_deadzone(r.ly, args.rc_deadzone)
+ lx = apply_deadzone(r.lx, args.rc_deadzone)
+ rx = apply_deadzone(r.rx, args.rc_deadzone)
+ if args.swap_vy_yaw:
+ cmd = np.array([
+ ly * args.rc_vx_scale,
+ -rx * args.rc_vy_scale,
+ -lx * args.rc_wz_scale,
+ ], dtype=np.float32)
+ else:
+ cmd = np.array([
+ ly * args.rc_vx_scale,
+ -lx * args.rc_vy_scale,
+ -rx * args.rc_wz_scale,
+ ], dtype=np.float32)
+
+ limits = np.array([MAX_LIN_VEL_X, MAX_LIN_VEL_Y, MAX_ANG_VEL_YAW], dtype=np.float32)
+ return np.clip(cmd, -limits, limits)
+
+
+class CommandFilter:
+ def __init__(self, args):
+ self.alpha = float(args.cmd_ema_alpha)
+ self.max_step = np.array([
+ args.max_cmd_step_x,
+ args.max_cmd_step_y,
+ args.max_cmd_step_yaw,
+ ], dtype=np.float32)
+ self.prev = np.zeros(3, dtype=np.float32)
+
+ def reset(self):
+ self.prev[:] = 0.0
+
+ def update(self, raw_cmd):
+ cmd = np.asarray(raw_cmd, dtype=np.float32)
+ if 0.0 < self.alpha < 1.0:
+ cmd = self.alpha * cmd + (1.0 - self.alpha) * self.prev
+ if np.any(self.max_step > 0.0):
+ limit = np.where(self.max_step > 0.0, self.max_step, np.inf)
+ cmd = self.prev + np.clip(cmd - self.prev, -limit, limit)
+ self.prev = cmd.astype(np.float32)
+ return self.prev.copy()
+
+
+class ObsHistoryBuilder:
+ """Build RoboGauge 45-dim obs and 225-dim term-stacked ONNX 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_onnx_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, ONNX_INPUT_DIM)
+ return np.nan_to_num(obs, nan=0.0, posinf=0.0, neginf=0.0)
+
+
+class OnnxPolicy:
+ def __init__(self, onnx_path):
+ self.session = ort.InferenceSession(str(onnx_path), providers=["CPUExecutionProvider"])
+ self.input_name = self.session.get_inputs()[0].name
+ self.input_shape = self.session.get_inputs()[0].shape
+ self.outputs = [(o.name, o.shape) for o in self.session.get_outputs()]
+ if self.input_shape[-1] != ONNX_INPUT_DIM:
+ raise ValueError(f"ONNX input shape {self.input_shape} does not match {ONNX_INPUT_DIM}")
+ print(f"[INFO] ONNX: {onnx_path}")
+ print(f"[INFO] Input : {self.input_name} {self.input_shape}")
+ print(f"[INFO] Output: {self.outputs}")
+
+ def __call__(self, onnx_input):
+ outputs = self.session.run(None, {self.input_name: onnx_input.astype(np.float32)})
+ return np.asarray(outputs[0][0], dtype=np.float32)
+
+
+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,
+ "onnx_input_dim": ONNX_INPUT_DIM,
+ "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"],
+ }
+ 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):
+ 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 send_damping(client):
+ client.send(LowCmd())
+
+
+def send_hold_cmd(client, state, args):
+ cmd = LowCmd()
+ for j in range(NUM_ACTIONS):
+ cmd.set_motor(j, MotorCmd(
+ mode=MotorMode.Servo,
+ q=float(DEFAULT_DOF_POS[j]),
+ dq=0.0,
+ tau=0.0,
+ Kp=args.kp,
+ Kd=args.kd,
+ ))
+ apply_safety(
+ cmd,
+ state,
+ power_factor=args.power_factor,
+ position_limit_on=True,
+ position_protect_limit=None,
+ )
+ client.send(cmd)
+
+
+def send_position_cmd(client, state, targets, args):
+ cmd = LowCmd()
+ for j in range(NUM_ACTIONS):
+ cmd.set_motor(j, MotorCmd(
+ mode=MotorMode.Servo,
+ q=float(targets[j]),
+ dq=0.0,
+ tau=0.0,
+ Kp=args.kp,
+ Kd=args.kd,
+ ))
+ pp_limit = args.position_protect_limit if args.position_protect_limit > 0 else None
+ apply_safety(
+ cmd,
+ state,
+ power_factor=args.power_factor,
+ position_limit_on=True,
+ position_protect_limit=pp_limit,
+ )
+ client.send(cmd)
+
+
+def ramp_to_default(client, args, state):
+ 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
+
+ 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)
+ cmd = LowCmd()
+ for j in range(NUM_ACTIONS):
+ cmd.set_motor(j, MotorCmd(
+ mode=MotorMode.Servo,
+ q=float(target[j]),
+ dq=0.0,
+ tau=0.0,
+ Kp=args.kp_cal,
+ Kd=args.kd_cal,
+ ))
+ apply_safety(
+ cmd,
+ state,
+ power_factor=args.power_factor,
+ position_limit_on=True,
+ position_protect_limit=None,
+ )
+ client.send(cmd)
+ 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}"
+ )
+ time.sleep(dt)
+
+ for _ in range(max(1, int(0.5 * args.ramp_hz))):
+ if EXIT:
+ return state
+ new_state = client.recv_latest()
+ if new_state is not None:
+ state = new_state
+ send_hold_cmd(client, state, args)
+ time.sleep(dt)
+
+ 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)))
+ if max_abs > args.action_trip_limit:
+ return False, f"action abs {max_abs:.2f} > trip {args.action_trip_limit:.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("[INFO] Connecting to MCU...")
+ client = MCUClient()
+ print("[INFO] Waking MCU...")
+ client.wake_mcu(n_frames=50, dt=0.01)
+ state = client.recv_state(timeout=2.0)
+ if state is None:
+ client.close()
+ raise RuntimeError("No LowState received. Check robot network and sport processes.")
+
+ print(f"[INFO] Connected. Battery={state.bms.SOC}%")
+ print(f"[INFO] RPY deg: {np.round(np.degrees(as_np(state.imu.rpy)), 1)}")
+ print("[INFO] Initial joint positions (rad):")
+ q = motor_pos(state)
+ for i, name in enumerate(JOINT_NAMES):
+ print(f" [{i:02d}] {name:4s}: q={q[i]:+7.3f}, default={DEFAULT_DOF_POS[i]:+7.3f}")
+ return client, state
+
+
+def log_state(logger, step, mode, state, cmd=None, cmd_raw=None, obs_single=None, action_raw=None,
+ action_safe=None, target=None, state_reason="ok"):
+ 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),
+ 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,
+ )
+
+
+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)
+ onnx_input = obs_builder.build_onnx_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" onnx_input shape={onnx_input.shape} min={onnx_input.min():.3f} max={onnx_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 = OnnxPolicy(args.onnx)
+ 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)
+ onnx_input = obs_builder.build_onnx_input(obs_single)
+ action_raw = policy(onnx_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 ONNX 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 ONNX only.
+ No motor command.
+============================================================
+"""
+
+
+def run_deploy(args):
+ print(STARTUP_BANNER)
+ input("Press Enter when ready...")
+
+ policy = OnnxPolicy(args.onnx)
+ 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)
+
+ 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()
+
+ print("[INFO] R2 advances layers. L2 emergency-stops to IDLE.")
+ print("[INFO] Ctrl+C exits with safe_stop.")
+
+ while not EXIT:
+ new_state = client.recv_latest()
+ 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
+ send_damping(client)
+
+ 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)
+ 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:
+ print("\n[R2] HOLD -> OBS_TEST")
+ obs_builder.reset()
+ cmd_filter.reset()
+ last_action[:] = 0.0
+ prev_action[:] = 0.0
+ sm_state = State.OBS_TEST
+
+ elif sm_state == State.OBS_TEST:
+ send_hold_cmd(client, state, args)
+ obs_single = obs_builder.build_single(state, cmd, last_action)
+ obs_builder.build_onnx_input(obs_single)
+ ok, reason = state_ok(state, args)
+ if not ok:
+ print(f"\n[FAULT] OBS_TEST state check failed: {reason}")
+ sm_state = State.FAULT
+ 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)
+ onnx_input = obs_builder.build_onnx_input(obs_single)
+ ok, reason = state_ok(state, args)
+ if ok:
+ action_raw = policy(onnx_input)
+ 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)
+ prev_target = DEFAULT_DOF_POS.copy()
+ rl_step = 0
+ continue
+
+ obs_single = obs_builder.build_single(state, cmd, last_action)
+ onnx_input = obs_builder.build_onnx_input(obs_single)
+ ok, reason = state_ok(state, args)
+ if ok:
+ action_raw = policy(onnx_input)
+ 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)
+ prev_target = DEFAULT_DOF_POS.copy()
+ sm_state = State.HOLD
+ print("[STATE] HOLD")
+
+ 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,
+ )
+
+ 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)}")
+ 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 ONNX on Go1 PRO")
+ parser.add_argument("--onnx", default=str(DEFAULT_ONNX), help="Path to policy_15k.onnx")
+
+ 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 ONNX 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=2.5)
+ parser.add_argument("--ramp-hz", type=float, default=100.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)
+ 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)
+ 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/go1.xml b/deploy_45dim_rl_gym/go1.xml
new file mode 100644
index 0000000..ea60e88
--- /dev/null
+++ b/deploy_45dim_rl_gym/go1.xml
@@ -0,0 +1,197 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+