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}>(.*?)", 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 @@ + + + + diff --git a/deploy_45dim_rl_gym/policy_15k.onnx b/deploy_45dim_rl_gym/policy_15k.onnx new file mode 100644 index 0000000..32b4d0b Binary files /dev/null and b/deploy_45dim_rl_gym/policy_15k.onnx differ diff --git a/deploy_45dim_rl_gym/policy_25k.onnx b/deploy_45dim_rl_gym/policy_25k.onnx new file mode 100644 index 0000000..e8e1b26 Binary files /dev/null and b/deploy_45dim_rl_gym/policy_25k.onnx differ diff --git a/deploy_45dim_rl_gym/terrains/flat.xml b/deploy_45dim_rl_gym/terrains/flat.xml new file mode 100644 index 0000000..6fda676 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/flat.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/deploy_45dim_rl_gym/terrains/obstacle/obstacle_1.xml b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_1.xml new file mode 100644 index 0000000..264e3e5 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_1.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/obstacle/obstacle_10.xml b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_10.xml new file mode 100644 index 0000000..ac0bbd6 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_10.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/obstacle/obstacle_2.xml b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_2.xml new file mode 100644 index 0000000..5f29b65 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_2.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/obstacle/obstacle_3.xml b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_3.xml new file mode 100644 index 0000000..37e649b --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_3.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/obstacle/obstacle_4.xml b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_4.xml new file mode 100644 index 0000000..670fdfd --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_4.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/obstacle/obstacle_5.xml b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_5.xml new file mode 100644 index 0000000..e9a0977 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_5.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/obstacle/obstacle_6.xml b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_6.xml new file mode 100644 index 0000000..17ab25f --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_6.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/obstacle/obstacle_7.xml b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_7.xml new file mode 100644 index 0000000..94fba34 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_7.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/obstacle/obstacle_8.xml b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_8.xml new file mode 100644 index 0000000..c3df2ff --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_8.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/obstacle/obstacle_9.xml b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_9.xml new file mode 100644 index 0000000..d51cc22 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/obstacle/obstacle_9.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/slope/slope_1.xml b/deploy_45dim_rl_gym/terrains/slope/slope_1.xml new file mode 100644 index 0000000..966209e --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/slope/slope_1.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/slope/slope_10.xml b/deploy_45dim_rl_gym/terrains/slope/slope_10.xml new file mode 100644 index 0000000..bbdc055 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/slope/slope_10.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/slope/slope_2.xml b/deploy_45dim_rl_gym/terrains/slope/slope_2.xml new file mode 100644 index 0000000..4e44345 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/slope/slope_2.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/slope/slope_3.xml b/deploy_45dim_rl_gym/terrains/slope/slope_3.xml new file mode 100644 index 0000000..8725216 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/slope/slope_3.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/slope/slope_4.xml b/deploy_45dim_rl_gym/terrains/slope/slope_4.xml new file mode 100644 index 0000000..d344938 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/slope/slope_4.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/slope/slope_5.xml b/deploy_45dim_rl_gym/terrains/slope/slope_5.xml new file mode 100644 index 0000000..cf814ba --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/slope/slope_5.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/slope/slope_6.xml b/deploy_45dim_rl_gym/terrains/slope/slope_6.xml new file mode 100644 index 0000000..a21c05e --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/slope/slope_6.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/slope/slope_7.xml b/deploy_45dim_rl_gym/terrains/slope/slope_7.xml new file mode 100644 index 0000000..8b37f3a --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/slope/slope_7.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/slope/slope_8.xml b/deploy_45dim_rl_gym/terrains/slope/slope_8.xml new file mode 100644 index 0000000..2f08d60 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/slope/slope_8.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/slope/slope_9.xml b/deploy_45dim_rl_gym/terrains/slope/slope_9.xml new file mode 100644 index 0000000..0d6ba69 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/slope/slope_9.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/slope_5.xml b/deploy_45dim_rl_gym/terrains/slope_5.xml new file mode 100644 index 0000000..51fc78b --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/slope_5.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/deploy_45dim_rl_gym/terrains/slope_5_old.xml b/deploy_45dim_rl_gym/terrains/slope_5_old.xml new file mode 100644 index 0000000..d89c807 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/slope_5_old.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/deploy_45dim_rl_gym/terrains/stairs/stairs_1.xml b/deploy_45dim_rl_gym/terrains/stairs/stairs_1.xml new file mode 100644 index 0000000..ac9409f --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/stairs/stairs_1.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/stairs/stairs_10.xml b/deploy_45dim_rl_gym/terrains/stairs/stairs_10.xml new file mode 100644 index 0000000..4c47f67 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/stairs/stairs_10.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/stairs/stairs_2.xml b/deploy_45dim_rl_gym/terrains/stairs/stairs_2.xml new file mode 100644 index 0000000..2b4f89b --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/stairs/stairs_2.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/stairs/stairs_3.xml b/deploy_45dim_rl_gym/terrains/stairs/stairs_3.xml new file mode 100644 index 0000000..f0e6aac --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/stairs/stairs_3.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/stairs/stairs_4.xml b/deploy_45dim_rl_gym/terrains/stairs/stairs_4.xml new file mode 100644 index 0000000..ab08763 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/stairs/stairs_4.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/stairs/stairs_5.xml b/deploy_45dim_rl_gym/terrains/stairs/stairs_5.xml new file mode 100644 index 0000000..9062da5 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/stairs/stairs_5.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/stairs/stairs_6.xml b/deploy_45dim_rl_gym/terrains/stairs/stairs_6.xml new file mode 100644 index 0000000..b103418 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/stairs/stairs_6.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/stairs/stairs_7.xml b/deploy_45dim_rl_gym/terrains/stairs/stairs_7.xml new file mode 100644 index 0000000..7c61295 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/stairs/stairs_7.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/stairs/stairs_8.xml b/deploy_45dim_rl_gym/terrains/stairs/stairs_8.xml new file mode 100644 index 0000000..91eba23 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/stairs/stairs_8.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/stairs/stairs_9.xml b/deploy_45dim_rl_gym/terrains/stairs/stairs_9.xml new file mode 100644 index 0000000..3197cc8 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/stairs/stairs_9.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/wall/10x10_wall.xml b/deploy_45dim_rl_gym/terrains/wall/10x10_wall.xml new file mode 100644 index 0000000..130f61b --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/wall/10x10_wall.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/deploy_45dim_rl_gym/terrains/wave/wave.png b/deploy_45dim_rl_gym/terrains/wave/wave.png new file mode 100644 index 0000000..80d6ddb Binary files /dev/null and b/deploy_45dim_rl_gym/terrains/wave/wave.png differ diff --git a/deploy_45dim_rl_gym/terrains/wave/wave_1.xml b/deploy_45dim_rl_gym/terrains/wave/wave_1.xml new file mode 100644 index 0000000..6d5a73f --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/wave/wave_1.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/wave/wave_10.xml b/deploy_45dim_rl_gym/terrains/wave/wave_10.xml new file mode 100644 index 0000000..b4b3861 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/wave/wave_10.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/wave/wave_2.xml b/deploy_45dim_rl_gym/terrains/wave/wave_2.xml new file mode 100644 index 0000000..e557a54 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/wave/wave_2.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/wave/wave_3.xml b/deploy_45dim_rl_gym/terrains/wave/wave_3.xml new file mode 100644 index 0000000..5fc39c0 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/wave/wave_3.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/wave/wave_4.xml b/deploy_45dim_rl_gym/terrains/wave/wave_4.xml new file mode 100644 index 0000000..e622e68 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/wave/wave_4.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/wave/wave_5.xml b/deploy_45dim_rl_gym/terrains/wave/wave_5.xml new file mode 100644 index 0000000..675b2fd --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/wave/wave_5.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/wave/wave_6.xml b/deploy_45dim_rl_gym/terrains/wave/wave_6.xml new file mode 100644 index 0000000..0ea2785 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/wave/wave_6.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/wave/wave_7.xml b/deploy_45dim_rl_gym/terrains/wave/wave_7.xml new file mode 100644 index 0000000..48f5dce --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/wave/wave_7.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/wave/wave_8.xml b/deploy_45dim_rl_gym/terrains/wave/wave_8.xml new file mode 100644 index 0000000..b62af90 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/wave/wave_8.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/deploy_45dim_rl_gym/terrains/wave/wave_9.xml b/deploy_45dim_rl_gym/terrains/wave/wave_9.xml new file mode 100644 index 0000000..4822108 --- /dev/null +++ b/deploy_45dim_rl_gym/terrains/wave/wave_9.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + +