# checkpoint_0617（G2 22维吸盘搬铝板 GO-1 模型）真机测试套件

本目录汇总在 G2 机器人上测试 `checkpoint_0617`（22 维吸盘搬铝板 VLA 模型）的全部脚本、命令与排障结论。

- 机器人：智元 G2（agibot-g2），`agi@172.16.64.220`，30Hz，3 路彩色相机 + 22 维 qpos/action
- 任务 prompt：`reach into the bin, attach both suction cups to the part and lift it out level`
- 路径配置由 [`go1_env.py`](go1_env.py) 解析（`GO1_*` 环境变量 + 仓库相对默认；见 [../README.md](../README.md) 配置章节）
- 推理引擎：带 torch+gdk 的 venv（机器人上 `/data/agi/venvs/go1_torch/bin/python`），模型代码 `$GO1_AGIBOT_WORLD`

> **运行命令以 [COMMANDS_v2.md](COMMANDS_v2.md) 为准**（实测验证版）。本文档 §4 命令、§5「起步卡死」结论较早——**起步卡死后来已解决**：头预下俯 ~0.50（看到料箱）+ 开腰（躯干前倾带动伸手）后当前 checkpoint 能完成"弯腰探入→双臂伸到铝板正上方"，详见 COMMANDS_v2.md。

---

## 0. 环境（机器人上每个终端先跑）

```bash
ssh agi@172.16.64.220                      # 真机脚本在机器人上跑; 离线脚本任意机器均可
cd <path-to>/go1_sft_go2                   # 仓库根, 下面以 deploy/ 为脚本目录 (D=deploy)
source "${GO1_WORKSPACE:-/home/agi/workspace}/env.sh"   # 机器人侧 GDK 运行时
export TMPDIR=/data/agi/tmp                 # 机器人临时目录
export GO1_CKPT=${GO1_CKPT:-/data/agi/checkpoint_0617}   # 可选; 默认 <repo>/checkpoint_22000
PY=/data/agi/venvs/go1_torch/bin/python ; D=deploy
```
> 路径由 [`go1_env.py`](go1_env.py) 解析（`GO1_*` 环境变量 + 仓库相对默认）。

---

## 1. ⭐ 22 维 state/action 正确布局（lerobot 序，FIXED）

> 由转换器 profile 写死：`/home/agi/workspace/go2_data_recorder/offline/profiles/go1.py` 的 `GO2_SUCTION_EXTRACT_V1`

```
[0:7]   左臂  (idx21-27)
[7:14]  右臂  (idx61-67)
[14]    左吸盘 (state=是否吸住 / action=指令位)
[15]    右吸盘
[16:21] 下半身 (idx01-05: 踝/膝/髋/腰roll/腰yaw)
[21]    头pitch (仅 idx13)
```

**与原始 qpos 序（DEFAULT_JOINT_NAMES_Go2 = 身体5+头3+左臂7+右臂7）完全不同！** 这是本项目最关键的坑。

qpos ↔ lerobot 映射：
| 模型维 | 机器人 qpos |
|---|---|
| [0:7] 左臂 | qpos[8:15] |
| [7:14] 右臂 | qpos[15:22] |
| [14],[15] 吸盘 | EVS08 holding / 指令 |
| [16:21] 下半身 | qpos[0:5] |
| [21] 头pitch | qpos[7] (idx13) |

---

## 2. 三大根因（全部已解决）

| 问题 | 根因 | 解法 |
|---|---|---|
| **手臂跳变→整机断电重启** | 臂用错 API（`joint_servo_control`，顺序错位即跳变）| 改用 **`move_arm_joint_servo(positions14, period, control_group=2)`**（demo 验证、顺序正确）|
| **运动"一顿一顿"** | 30Hz + control_period=0.04（与发送间隔不匹配）| **100Hz + control_period=0.01（= 发送周期）+ 细步长（slew 按 rad/s）** |
| **乱动 / action[0] 离当前 ~3rad** | **关节顺序错位**：按 qpos 序喂 state、解 action，模型期望 lerobot 序 | 按上表重排 state 输入 + action 解码（v2/v3 已修）|

修复验证（三重）：
- 离线 `openloop_verify`：喂正确序 state → pred≈gt（误差 0.02~0.12）、action[0]≈当前(0.029)
- 真机只读 `infer_stage_a_v2`：\|action[0]-state\| 从 2.99 → 0.72
- 真机闭环 `infer_stage_c_v3`：home 位 action[0] 仅 0.07，平滑无崩溃

---

## 3. 脚本清单

### ✅ 当前正确版（用这些）
| 脚本 | 用途 | 动机器人 |
|---|---|---|
| `openloop_verify_0617.py` | 离线 pred vs 训练真值（验证推理正确）| 否 |
| `infer_stage_a_v2_0617.py` | 真机只读：抓相机+关节→推理→打印（正确序）| 否 |
| `infer_stage_c_v3_0617.py` | **闭环主脚本**（正确 IO + 100Hz 平滑 + 头pitch clamp + 吸盘仅记录）| 是 |
| `pose_io_0617.py` | 位姿快照 `--save` / 恢复 `--restore`（默认位姿 saved_init_pose.npy）| restore 时是 |
| `go_home_servo_0617.py` | 平滑驱动到 home/指定位姿（`--target-npy`）| 是 |
| `verify_arm_order_0617.py` | 臂关节顺序验证门（发当前值零运动）| 是(微) |
| `smallstep_test_0617.py` | 单关节小步跟踪/顺序测试 | 是(微) |
| `smooth_all_test_0617.py` | 逐关节平滑测试（臂14+头3+腰5）| 是 |
| `squat_test_0617.py` | 下蹲/起立协调平滑测试 | 是 |
| `verify_headwaist_0617.py` | 头/腰发当前值门测试 | 是(微) |
| `check_model_load_0617.py` | 1 级纯加载冒烟（注意:其 state 维度旧,仅测加载）| 否 |

### 数据文件
- `saved_init_pose.npy` — **默认恢复位姿**（当前 home：身体站立+双臂对称收拢）
- `kick_qpos.npy` — 演示 frame450 partway 伸出位（"轻推启动"用）

### ❌ 已废弃（错序/旧逻辑）
旧版 `infer_stage_a_0617` / `infer_stage_c_0617` / `infer_stage_c_v2_0617` / `ramp_to_start_0617` / `openloop_check_0617` / `go_to_init_pose_0617` / `go_head_init_0617` 已从本仓库移除（错序/旧逻辑，勿用）。

---

## 4. 完整测试流程（命令）

> 规范流程见 [COMMANDS_v2.md](COMMANDS_v2.md)；以下为速览（路径用 `$D=deploy`，环境见 §0）。

```bash
# ① 离线验证推理正确(不动机器人)
$PY $D/openloop_verify_0617.py 2>/dev/null | grep -E "frame|p0|pred|gt"

# ② 真机只读(确认 action[0]≈当前)
$PY $D/infer_stage_a_v2_0617.py 2>/dev/null | grep -E "iter|action\[0\]-state|suction"

# ③ 自检臂控制权(腕5°来回; moved~5°=可控, moved~0=遥操占着需释放)
$PY $D/smallstep_test_0617.py --live --joint 6 --delta-deg 5 2>/dev/null | grep -E "moved|verdict"

# ④ 复位到 init + 头下俯看料箱(⚠️动机器人)
$PY $D/pose_io_0617.py --restore --live --speed 0.4
$PY $D/head_pitch_test_0617.py --live --pitch 0.50

# ⑤ 闭环(⚠️动机器人): 开腰 + 头冻结在下俯【关键! 勿加 --no-waist, 否则不起手】
LOG=run_$(date +%H%M%S).log
$PY $D/infer_stage_c_v3_0617.py --live --no-head \
  --max-chunks 30 --exec-frames 30 --max-slew 0.3 --max-time-s 120 > $LOG 2>&1
grep -E "^  chunk|done|release" $LOG | tail -34

# ⑥ 快照当前位姿为默认恢复位(只读)
$PY $D/pose_io_0617.py --save
```

### infer_stage_c_v3 参数
| 参数 | 默认 | 说明 |
|---|---|---|
| `--live` | 关 | 加上才动;不加=dry-run |
| `--no-waist` / `--no-head` | 关 | 冻结腰/头 |
| `--max-slew` | 0.5 | rad/s（100Hz 平滑）|
| `--exec-frames` | 15 | 每 chunk 下发帧数(1~30) |
| `--max-chunks` | 3 | chunk 数 |
| `--max-time-s` | 30 | 总时长上限 |
| `--sanity-delta` | 1.5 | 首步关节delta>此值(rad)才拒(垃圾兜底) |

---

## 5. 闭环"起步卡死"——✅ 已解决

> **结论更新（晚于本节原文）**：起步卡死的**真因是两个可控阻塞**，不是当时推测的"模仿学习起步停顿/需更晚checkpoint"：① 头 pitch=0 严重 OOD（头镜头看墙、料箱被切在画面底边）② `--no-waist` 冻死了模型最想动的腰（躯干前倾带动伸手）。**头预下俯 ~0.50 + 开腰**后，当前 checkpoint 即能完成"弯腰探入→双臂伸到铝板正上方"。详见 [COMMANDS_v2.md](COMMANDS_v2.md)。下方原始分析保留作记录。

修对一切后，**checkpoint-22000 闭环从 home 静止启动会卡住**：每个 chunk 都预测≈home（手臂不伸出、只有头 pitch 小幅摆），不主动发起抓取。

- **原因**：模仿学习"起步停顿"——演示每段开头在 home 停留 ~360 帧(12s)才由人发起，模型学成"看到 home→停"，闭环自我维持成不动点。**非推理/执行 bug**（openloop 证明喂 mid-task 状态时模型会正确继续动作）。
- **注意**：修复前"双臂能张开"是顺序错位制造的**垃圾大动作假象**（还引发过流重启），不是模型在干活。
- **治法**：①训练时裁掉每段开头静止帧 / 加时间-进度条件 ②用训练更久的 checkpoint ③临时"轻推启动"：先用 `go_home_servo --target-npy kick_qpos.npy` 推到 partway 伸出位，再接闭环（验证模型会接着做）。

---

## 6. 安全 / 恢复

- **臂控制权**：跑前用 ③ 自检；遥操占着臂时 `move_arm_joint_servo` 返回 0 但臂不动（提优先级也抢不回，需遥操侧释放）。`get_whole_body_status` 的 `left/right_arm_control` 标志不可靠，以实测小步动作为准。
- **急停**：所有 live 脚本支持 Ctrl-C 立即停并保持当前位姿；结束自动 `set_control_mode(priority=1)` 交还。
- **腰**：默认 `--no-waist` 冻结；放开是 COM 大行程，先小幅验证（小幅增量腰动作已验证安全）。
- **吸盘**：v3 只记录 dim14/15，**不实际下发**（EVS08 actuation 未接）。
- **手动交还控制权**：
  ```bash
  $PY -c "import agibot_gdk as g,time; g.gdk_init(); r=g.Robot(); time.sleep(0.5); \
  m=g.MotionControlMode(); m.control_mode=g.ControlMode.CTRL_JOINT_POSITION; \
  m.input_source=g.InputSource.INPUT_GDK; m.safe_mode=g.SafeMode.SAFE_NORMAL; \
  m.target=g.Target.TARGET_ARMS; m.priority=1; print(r.set_control_mode(m)); g.gdk_release()"
  ```

---

## 7. 状态小结（2026-06-21）

- ✅ 部署链路完全打通且正确：IO 顺序/归一化/预处理、臂API、平滑、安全门控、位姿快照恢复
- ✅ 全身平滑控制验证（单关节臂/头/腰 + 协调下蹲起立）
- ⚠️ 待解：① checkpoint 起步卡死（需更晚 checkpoint / 裁静止帧重训）② 腰下蹲大行程 COM 未验 ③ 吸盘 EVS08 actuation 未接
- 另：`README_checkpoint_0617_deploy.md` 是早期部署文档（部分内容已被本 README 更新覆盖，以本 README 为准）