# Go1 PRO ONNX 策略部署测试

## 环境准备

```bash
conda activate free_dog_sdk
cd /Users/chenyouyuan/cyy_ws/deploy_go1_pro
```

---

## 推荐部署命令

```bash
python deploy_onnx_pro_sdk.py --onnx policy.onnx --kill-sport --log-dir logs \
    --kp 80 --kd 1 --kp-cal 20 --kd-cal 1 \
    --power-factor 7 --position-protect-limit 1.0 \
    --action-ema-alpha 0.3 --print-every 10
```

---

## 测试流程

### Step 0 — 观测数据查验 ✓

```bash
python deploy_onnx_pro_sdk.py --obs-check --kill-sport --log-dir logs
```

| obs 段 | 含义 | 期望 | 结果 |
|---|---|---|---|
| `[0:3]` | gyro×0.25 | ~0 | ✅ |
| `[3:6]` | 投影重力 | ~[0,0,-1] | ✅ |
| `[6:18]` | 关节偏差 | ~0（站姿时） | ⚠️ 蹲姿，正常 |
| `[18:30]` | 关节速度 | ~0 | ✅ |
| `[30:42]` | 上步动作 | [0] | ✅ |
| `[42:45]` | 指令×scale | ~0 | ✅ |

结论：关节初始为阻尼蹲姿（膝盖 ~-2.79rad），ramp 渐变到站姿。

### Step 1 — 遥控器测试 ✓

```bash
python deploy_onnx_pro_sdk.py --monitor --kill-sport --log-dir logs
```

全部按钮（R2/L2/L1/A/B/X/Y/START）上升沿/下降沿检测正常，摇杆四轴 ±1.0。

### Step 2 — 起身 ✓

R2 → CALIBRATE（2s 渐变）→ HOLD（伺服站姿）。

### Step 3 — 急停 ✓

L2 → 立即 IDLE（全阻尼），恢复正常。

### Step 4 — RL 联调 ✓

R2 → RL，摇杆控制速度。

---

## 参数调优记录

### 最终工作参数

| 参数 | 值 | 说明 |
|---|---|---|
| `--kp` | **80** | 与 sim 一致，直连 MCU |
| `--kd` | **1** | sim Kd=0.5+被动0.5=1.0 |
| `--kp-cal` | 20 | ramp 阶段 Kp |
| `--kd-cal` | 1.0 | ramp 阶段 Kd |
| `--power-factor` | 7 | hip:16.6, thigh:16.6, knee:24.9 Nm |
| `--position-protect-limit` | 1.0 | RL 阶段位置偏差保护 |
| `--action-ema-alpha` | **0.3** | 平滑 action 高频换向 |

### 调参过程

| 阶段 | 问题 | 原因 | 解决 |
|---|---|---|---|
| 起身 | 力矩被掐断 | `position_protect=0.5` 膝盖差 1.0rad 触发 | ramp/HOLD 阶段禁用 position_protect |
| 起身 | 力不够 | `Kp=5 / power_factor=3` 太保守 | Kp→20-30, power_factor→7 |
| RL | 高频抖动 | policy 本就会每2-3步换向（sim 和 real 一致），Kd=0.3 刹不住 | Kd→1, 加 EMA 滤波 |
| RL | 关节偏软 | Kp 不够大 | Kp→80 与 sim 对齐 |

### Safety 策略分层

| 阶段 | position_protect | 原因 |
|---|---|---|
| Ramp（起身） | ❌ 禁用 | 大范围移动（膝盖 ~1.0rad） |
| HOLD（站姿） | ❌ 禁用 | 维持固定站姿，本身安全 |
| RL（行走） | ✅ limit=1.0rad | 防策略突跳 |

### EMA 滤波

Policy 输出的 action 在 sim 和真机上方向切换率一致（hips ~40%, calves ~25%），这是模型本身特性。sim 中 Kp=80 硬吃掉了，真机需要用 EMA 平滑：

- `--action-ema-alpha 0.3`：`new = 0.3×raw + 0.7×prev`，时间常数 ~30ms
- 0.2：更强平滑但响应慢
- 1.0：不过滤

### Kd 选择依据

| 来源 | Kp | Kd | Kd/Kp |
|---|---|---|---|
| sim（含被动阻尼 0.5） | 80 | 1.0 (0.5+0.5) | 0.0125 |
| WTW LCM deploy | 20 | 0.5 | 0.025 |
| **最终直连 MCU** | **80** | **1** | **0.0125** |

---

## 状态机

```
                    ┌──────────────────────────────────────┐
                    │              L2 (急停)                │
                    ▼                                       │
IDLE ──R2──► CALIBRATE ──auto──► HOLD ──R2──► RL          │
  ▲                                  ▲           │          │
  └──────────── L2(急停) ────────────┘      R2 ──┘          │
  └─────────────────────────────────────────────────────────┘

IDLE:       全阻尼，等待 R2
CALIBRATE:  渐变到站姿（2s），自动→HOLD
HOLD:       伺服保持站姿，等待 R2 启动 RL
RL:         运行 ONNX 策略，摇杆控制
```

## 遥控器

| 按键 | 功能 |
|---|---|
| R2 | 状态切换（IDLE→起身 / HOLD↔RL） |
| L2 | 急停（任意状态→IDLE） |
| 左杆 Y | 前进/后退 |
| 左杆 X | 平移 |
| 右杆 X | 旋转 |