init pro_sdk

2026-06-20 20:02:36 +08:00
commit 5ecd422a59
49 changed files with 4226 additions and 0 deletions
--- a/docs/ARCHITECTURE.md
+++ b/docs/ARCHITECTURE.md
@@ -0,0 +1,142 @@
+# 包结构与数据流
+
+## 模块依赖图
+
+```
+                ┌──────────────────────┐
+                │  MCUClient           │  ← 用户主要 API
+                │  (connection/)       │
+                └──────┬───────────────┘
+                       │
+        ┌──────────────┼──────────────┐
+        ▼              ▼              ▼
+┌─────────────┐  ┌─────────────┐  ┌─────────────┐
+│  codec/     │  │  types/     │  │  safety/    │
+│  Blowfish   │  │  LowCmd     │  │  apply_     │
+│  build_*    │  │  LowState   │  │  safety     │
+│  parse_*    │  │  MotorCmd   │  │             │
+└──────┬──────┘  └──────┬──────┘  └──────┬──────┘
+       │                │                │
+       └────────────────┼────────────────┘
+                        ▼
+                ┌─────────────┐
+                │  utils/     │
+                │  common.py  │ ← CRC, float_hex
+                │  constants  │ ← 关节限位, 默认地址
+                └─────────────┘
+```
+
+## 数据流: 收一帧 → 用户处理 → 发命令
+
+```
+   UDP 858B 密文                 UDP 616B 密文
+       ▲                              │
+       │                              ▼
+   MCU :8007                     MCU :8007
+       │                              ▲
+       ▼                              │
+┌─────────────────────────────────────────────────┐
+│ MCUClient.recv_latest()                         │
+│   socket.recvfrom()                             │
+│   bf.decrypt_ecb(data[:856])  → 856B 明文       │
+│   parse_low_state(...)        → LowState 结构   │
+└─────────────────────────────────────────────────┘
+       │
+       ▼
+┌──────────┐   用户逻辑    ┌──────────┐
+│ LowState │ ──────────► │ LowCmd   │
+└──────────┘  qDes, Kp,  └──────────┘
+              Kd, tau...        │
+                                ▼
+┌─────────────────────────────────────────────────┐
+│ apply_safety(cmd, state, power_factor=1)        │
+│   position_limit(cmd)                           │
+│   power_protect(cmd, state, factor)             │
+│   position_protect(cmd, state, limit_rad)       │
+└─────────────────────────────────────────────────┘
+       │
+       ▼
+┌─────────────────────────────────────────────────┐
+│ MCUClient.send(cmd)                             │
+│   build_low_cmd_plain(cmd)    → 616B 明文       │
+│   bf.encrypt_ecb(plain)       → 616B 密文       │
+│   sock.sendto(cipher, ...)                      │
+└─────────────────────────────────────────────────┘
+```
+
+## 各包职责
+
+### utils/
+
+最底层。纯函数, 无副作用, 无依赖其他模块。
+
+- `common.py`: CRC, float↔hex, tau↔2B, Kp↔2B, Kd↔2B, decode_sn/version
+- `constants.py`: MCU 地址, 关节命名, 限位, TAU_MAX, DAMPING_POSE
+
+### types/
+
+数据结构 (dataclass)。依赖 utils, 不依赖 codec/safety/connection。
+
+- `motor.py`: MotorCmd, MotorState, MotorMode
+- `imu.py`: IMU
+- `bms.py`: BMS
+- `remote.py`: RemoteState, parse_remote
+- `low_cmd.py`: LowCmd (12 motorCmd + 元数据)
+- `low_state.py`: LowState (12 motorState + IMU + BMS + remote 等)
+
+### codec/
+
+加解密和序列化。依赖 types + utils。
+
+- `blowfish.py`: 标准 Blowfish ECB (LE 字节序), 接受预生成 state
+- `lowcmd_builder.py`: LowCmd → 616B 明文 → 加密后 616B
+- `lowstate_parser.py`: 解密后 807B → LowState 结构
+
+### safety/
+
+控制保护层。依赖 types + utils。
+
+- `safety.py`: PositionLimit/PowerProtect/PositionProtect 三个保护 + 统一入口 apply_safety
+
+### connection/
+
+UDP 高层抽象。依赖前面所有。
+
+- `mcu_client.py`: MCUClient (socket + Blowfish + 序列化 + safe_stop)
+
+## 用户三种用法
+
+### Level 1: 高层 (推荐)
+
+```python
+from go1_pro_sdk import MCUClient, LowCmd, MotorCmd, MotorMode
+
+with MCUClient() as client:
+    client.wake_mcu()
+    state = client.recv_state()
+    cmd = LowCmd()
+    cmd.set_motor('FR_1', MotorCmd(mode=MotorMode.Servo, q=1.2, Kp=5, Kd=1))
+    client.send(cmd)
+    client.safe_stop()
+```
+
+### Level 2: 中层 (自己管 socket, 用编解码)
+
+```python
+from go1_pro_sdk import Blowfish, build_low_cmd_encrypted, parse_low_state, LowCmd
+import socket
+
+bf = Blowfish.from_state_file('go1_pro_sdk/_data/blowfish_state.bin')
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+sock.bind(('', 0))
+
+cmd = LowCmd()
+sock.sendto(build_low_cmd_encrypted(cmd, bf), ('192.168.123.10', 8007))
+data, _ = sock.recvfrom(2048)
+state = parse_low_state(bf.decrypt_ecb(data[:856]))
+```
+
+### Level 3: 底层 (诊断/逆向)
+
+直接用 `Blowfish.encrypt_block(b8)` / `Blowfish.decrypt_block(b8)`, 自己处理 8B 块。
+适合协议研究或字节级 diff。
--- a/docs/PROTOCOL.md
+++ b/docs/PROTOCOL.md
@@ -0,0 +1,145 @@
+# Go1 PRO 协议规格
+
+完全实测/抓包验证 (2026-06-20)。
+
+## LowCmd (主控 → MCU)
+
+**616 字节 Blowfish ECB 密文** (明文 616B, 加密后还是 616B)，通过 UDP 发到 `192.168.123.10:8007`。
+
+### 明文 616B 布局
+
+| 偏移 | 字节 | 字段 | 说明 |
+|-----|------|------|------|
+| 0..2 | 2 | head | `fe ef` |
+| 2..3 | 1 | levelFlag | `0xff` |
+| 3..4 | 1 | frameReserve | 0 |
+| 4..12 | 8 | SN | 全 0 (PRO 不填) |
+| 12..20 | 8 | version | 全 0 (PRO 不填) |
+| 20..22 | 2 | bandWidth | **`3a c0` (BE 字节序, 不是 LE!)** |
+| 22..562 | 540 | motorCmd[20] | 每电机 27B (mode + q + dq + tau + Kp + Kd + reserve) |
+| 562..566 | 4 | bms | 0 |
+| 566..606 | 40 | wirelessRemote | 0 |
+| 606..610 | 4 | reserve | 0 |
+| **610..612** | 2 | **固定填充** | **`00 00`** |
+| **612..616** | 4 | **CRC** | **`gen_crc(cmd[:612])` 裸 CRC, 不 XOR** |
+
+### MotorCmd 单电机 27B
+
+```
+0..1:  mode (uint8)
+1..5:  q (float, Unitree 自定义 hex 编码, 不是标准 IEEE 754)
+5..9:  dq (同上)
+9..11: tau (2B 自定义)
+11..13: Kp (2B 自定义)
+13..15: Kd (2B 自定义)
+15..27: reserve (12B = 3 × 4B)
+```
+
+各字段编解码见 `go1_pro_sdk/utils/common.py`。
+
+### 跟 EDU 版差异 (free-dog-sdk lowCmd)
+
+| 项 | EDU | PRO |
+|----|-----|-----|
+| 总长 | 614B | **616B** |
+| CRC 偏移 | 610..614 | **612..616** |
+| CRC 算法 | `encryptCrc(genCrc(cmd[:-6]))` (XOR 0xedcab9de) | **裸 `gen_crc(cmd[:612])`** |
+| bandWidth 字节序 | LE | **BE** |
+| SN/version | 用户提供 | 全 0 |
+| 是否加密 | 否 | **Blowfish ECB** |
+
+
+## LowState (MCU → 主控)
+
+**858 字节 Blowfish ECB 密文** (Blowfish 8B 块, 858 取 8 对齐 = 856B 解密)。
+
+解密后 807B 是有效 LowState，后面 49B 填充/MCU 内部。
+
+### 明文 807B 布局 (跟 free-dog-sdk 的 ucl/lowState.py 一致)
+
+| 偏移 | 字节 | 字段 |
+|-----|------|------|
+| 0..2 | 2 | head |
+| 2..3 | 1 | levelFlag |
+| 3..4 | 1 | frameReserve |
+| 4..12 | 8 | SN |
+| 12..20 | 8 | version |
+| 20..22 | 2 | bandWidth |
+| 22..75 | 53 | IMU (quaternion 16B + gyro 12B + accel 12B + rpy 12B + temp 1B) |
+| 75..715 | 640 | motorState[20] × 32B |
+| 715..739 | 24 | BMS |
+| 739..759 | 20 | footForce / footForceEst (混合) |
+| 759..799 | 40 | **wirelessRemote** (遥控器原始数据) |
+| 799..803 | 4 | reserve |
+| 803..807 | 4 | CRC |
+
+
+## Blowfish
+
+**ECB 模式**，**LE word 字节序** (Unitree ARM64 LDP 读取顺序，不是标准 BE Blowfish)。
+
+state (4168B) = P[18] + S0[256] + S1[256] + S2[256] + S3[256]，每项 4B uint32。
+
+### F 函数
+
+```python
+def F(X):
+    a = (X >> 24) & 0xff
+    b = (X >> 16) & 0xff
+    c = (X >> 8) & 0xff
+    d = X & 0xff
+    return ((((S0[a] + S1[b]) & 0xFFFFFFFF) ^ S2[c]) + S3[d]) & 0xFFFFFFFF
+```
+
+**注意 Python 运算符优先级**: `+` 比 `^` 高，必须完整括号否则结果错。
+
+### 16 轮 Feistel
+
+标准 Blowfish 算法 (见 `go1_pro_sdk/codec/blowfish.py`)，无定制。
+
+### 已知明密文对 (用于验证 state)
+
+```
+encrypt(0000000000000000) = 12aa0354236b66e3
+encrypt(feefff0004020305) = 5d9874ad7eee5851
+encrypt(0802010000010900) = c5ce0887ceb86f91
+```
+
+
+## 遥控器 wirelessRemote (40B)
+
+布局 (Unitree 公开 `xRockerBtnDataStruct`):
+
+| 偏移 | 字节 | 字段 |
+|-----|------|------|
+| 0..2 | 2 | head 0x55 0xAA |
+| 2..4 | 2 | btn bitmap (uint16 LE) |
+| 4..8 | 4 | lx (float, -1..+1) |
+| 8..12 | 4 | rx |
+| 12..16 | 4 | ry |
+| 16..20 | 4 | L2 (0..1, 模拟扳机) |
+| 20..24 | 4 | ly |
+| 24..40 | 16 | reserved |
+
+### 按键 bitmap
+
+| 位 | 按键 | 位 | 按键 |
+|----|------|----|------|
+| 0x0001 | R1 | 0x0100 | A |
+| 0x0002 | L1 | 0x0200 | B |
+| 0x0004 | START | 0x0400 | X |
+| 0x0008 | SELECT | 0x0800 | Y |
+| 0x0010 | R2 | 0x1000 | UP |
+| 0x0020 | L2 | 0x2000 | RIGHT |
+| 0x0040 | F1 | 0x4000 | DOWN |
+| 0x0080 | F2 | 0x8000 | LEFT |
+
+
+## MCU 客户端切换机制
+
+MCU 按"最近发包者的 IP:port"切换客户端。也就是说:
+- 我们 Mac 不需要绑特定端口，任意 OS 分配的端口都行
+- 一旦我们发包，MCU 把后续 LowState 回复给我们 (Legged_sport 不再收到)
+- Legged_sport 没收到 state 就停止 sendto，等价于"挂起"
+
+**含义**: 必须先在 Pi 上杀 `keep_sport_alive.sh`、`Legged_sport`、`appTransit` 三个抢占者，否则会跟 MCU 反复切换导致控制失败。
--- a/docs/REVERSE_ENGINEERING.md
+++ b/docs/REVERSE_ENGINEERING.md
@@ -0,0 +1,103 @@
+# 逆向工程过程精华
+
+完整的探索过程见原项目笔记 `free-dog-sdk/REVERSE_ENGINEERING_NOTES.md` (1700+ 行)。这里是事后整理的精华。
+
+## 起点
+
+Unitree Go1 **PRO** 版机器人, 跑 sportMode 系统:
+- 树莓派 4 (192.168.123.161): MQTT broker + Legged_sport 控制循环 + appTransit
+- 3 台 Jetson Nano (192.168.123.13/14/15): 相机/AI/SLAM
+- MCU (192.168.123.10): 电机控制器 (UDP :8007)
+
+PRO 跟 EDU 的关键区别 (Unitree 故意做的限制):
+- MCU 用 Blowfish 加密 LowState 回包 (EDU 无加密)
+- MCU 要求 LowCmd 也是 Blowfish 加密 (EDU 接受明文)
+- Legged_sport 二进制 UPX 压缩 + 符号剥离, 拿不到密钥
+
+## 走过的弯路 (记录, 避免再踩)
+
+### 弯路 1: 误判加密算法是 TEA
+
+看到 Feistel 结构 + 32 字节常数, 第一反应是 TEA. 花了几天用 Z3 求解 128-bit key, 全部超时.
+
+**纠正**: TEA 的特征是 `lsl #4` + `lsr #5` 在同一函数. Blowfish 是 T-table 查表 + 加法 + 异或, 没有移位混合常数. 反汇编时区分这两点是关键.
+
+### 弯路 2: 试图静态反推密钥
+
+Blowfish 的 key schedule 是:
+1. P[i] = P_initial[i] ^ key_word(i % keylen)
+2. 用此 P 和初始 S-box 加密 (0,0), 用结果替换 P[0..1]
+3. 继续替换 P[2..3] ... 直到 P[16..17]
+4. 同样替换 S-box
+
+**纠正**: 步骤 1 的中间值在步骤 2 中**立刻被覆盖**, 从最终 P-array 无法反推 key. 但**有 state 就够了**, 不需要原始 key. 运行时 state = 4168B = P[18] + S0..S3.
+
+### 弯路 3: 误以为 LowCmd 不加密
+
+heap 搜索没找到 `feefff00` 密文 → 错误结论 "LowCmd 是明文". 实际上加密发生在 sendto 缓冲区里, 加密后立刻被消费掉, 没留在 heap.
+
+**纠正**: 用 `strace -e sendto` 抓到 Legged_sport 真实发送的字节, 解密后是 LowCmd 头, 确认必须加密.
+
+### 弯路 4: 误以为 MCU 锁源端口
+
+之前认为 MCU 锁 Legged_sport 的源端口 8008. 改 sin 测试用 :8008 还是不动.
+
+**纠正**: MCU 实际是按"**最近发包者的 IP:port**"切换. 任意端口都行, 关键是要先停掉 Pi 上的抢占者.
+
+### 弯路 5: 没杀干净 Legged_sport
+
+`killall -9 Legged_sport` 杀完, 进程立刻又活. 看 ps 才发现父进程是 `keep_sport_alive.sh` (PID 970), 每 0.2 秒检查并重启 Legged_sport.
+
+**纠正**: 必须**先杀看门狗**, 再杀 Legged_sport. 顺序错了等于没杀.
+
+## 关键突破
+
+### 突破 1: 内存提取 Blowfish state
+
+```bash
+# 在 Legged_sport 运行时 attach GDB (只读, 不设断点不写内存)
+sudo bash extract_blowfish_key.sh   # < 30 秒, 不影响运动控制
+# 拉回 Mac, 在 heap dump 里搜索 4168B 候选, 用已知明密文对验证
+python analyze_blowfish_dump.py blowfish_dump.tar.gz
+# 命中: rw_7f86a1f000.bin offset 0x232190
+```
+
+### 突破 2: PRO 格式跟 EDU 不一样
+
+用 tcpdump 抓 Legged_sport 真实发送的 616B 包, 解密后跟 free-dog-sdk `lowCmd.buildCmd()` 输出做字节级 diff:
+
+| 字段 | EDU | PRO |
+|------|-----|-----|
+| 总长 | 614B | **616B** (多 2B 填充) |
+| CRC 偏移 | 610..614 | **612..616** |
+| CRC 算法 | `encryptCrc(genCrc(cmd[:608]))` XOR 0xedcab9de | **裸 `gen_crc(cmd[:612])`** |
+| bandWidth 字节序 | LE | **BE** |
+| SN/version | 填实际值 | 全 0 |
+
+### 突破 3: 内存里的 wirelessRemote 数据
+
+LowState 的 759..799 是 40 字节遥控器数据, 跟 EDU `unitree_legged_sdk/comm.h` 公开的 `xRockerBtnDataStruct` 完全一致. 直接 parse 就能拿到按键 + 摇杆 + L2 模拟值.
+
+## 最终性能 (Mac 直连)
+
+| 指标 | 实测 |
+|------|------|
+| 控制频率 | **480 Hz** (96% 目标) |
+| 单步 p50 | 1.74 ms |
+| Blowfish 加密 | 1.02 ms |
+| Blowfish 解密 | 0.67 ms |
+| sendto | 0.02 ms |
+| Blowfish 解码率 | 100% |
+
+## 工具集
+
+| 工具 | 用途 |
+|------|------|
+| `tools/extract_blowfish_key.sh` | 狗端内存 dump |
+| `tools/analyze_blowfish_dump.py` | Mac 端搜索 4168B state |
+| `tools/calibrate_joints.py` | 手动转动法测真实关节限位 |
+| `tools/capture_lowcmd.sh` | 抓 Legged_sport 真实发送的包 |
+| `tools/diff_real_lowcmd.sh` | 抓包 + 字段级 diff |
+| `tools/stop_sportmode.sh` | SSH 杀抢占源 |
+| `tools/run_on_mac.sh` | Mac 端跑测试 (自动停抢占) |
+| `tools/run_on_pi.sh` | Pi 端跑测试 (源 IP 跟 Legged_sport 一致) |
--- a/docs/SAFETY.md
+++ b/docs/SAFETY.md
@@ -0,0 +1,66 @@
+# 安全测试清单
+
+## 上机调试前必读
+
+直接控制 12 个电机是危险操作。第一次上机必须严格按以下清单。
+
+## 物理准备
+
+- [ ] 狗悬挂 (脚架/吊带), 脚不接地
+- [ ] 周围 1 米无障碍物
+- [ ] 准备 **拔电池**, 因为 Legged_sport 被杀后, 遥控器 L2+B 不会工作
+
+## 软件准备
+
+- [ ] `bash tools/stop_sportmode.sh stop` 杀掉 keep_sport_alive + Legged_sport + appTransit
+- [ ] 验证 `pgrep -fl Legged_sport` 没输出
+- [ ] 用 `examples/monitor_state.py` 确认能正常解码 LowState
+- [ ] 第一次跑必须用 `apply_safety(cmd, state, power_factor=1)` (10% 力矩)
+- [ ] sin 振幅 ≤ 0.3 rad, 频率 ≤ 0.5 Hz
+
+## 跑动中的实时监控
+
+每个控制脚本应当包括:
+
+```python
+try:
+    apply_safety(cmd, state,
+                 power_factor=1,            # 1=最严
+                 position_limit_on=True,    # 关节硬限位
+                 position_protect_limit=0.5,  # 目标偏离实际>0.5rad 关 Kp/Kd
+                 raise_on_critical=True)    # 严重超载 raise
+except PowerProtectViolation as e:
+    print(f"⚠️ 立即停机: {e}")
+    client.safe_stop()
+    break
+```
+
+## 异常处理
+
+| 现象 | 立刻做什么 |
+|------|----------|
+| 脚本输出 ⚠️ PowerProtectViolation | 脚本会自动 safe_stop, 你拔电池保险 |
+| 关节抖动 / 嗡嗡声 | Ctrl+C, 脚本自动发 damping |
+| 狗腿快速摆动失控 | **立即拔电池** (按住电源 3 秒) |
+| MCU 不响应 | 检查 Pi 抢占源是否清干净: `ssh pi@192.168.123.161 pgrep -fl Legged_sport` |
+
+## 紧急停机优先级
+
+1. **遥控器 L2+B** — **PRO 模式下杀了 Legged_sport 后不工作**, 不要依赖
+2. **Ctrl+C** — 触发 `client.safe_stop()` 发 50 帧 damping
+3. **拔电池** — 终极停机, 按住电源键 3 秒
+
+## 测试完成后
+
+```bash
+bash tools/stop_sportmode.sh start
+```
+
+重启 sportMode 系统, 狗恢复正常.
+
+## 不要做
+
+- ❌ 不要在 sportMode 还在跑时同时跑你的控制 (会跟 Legged_sport 抢, 抖动)
+- ❌ 不要从 PowerProtectViolation 异常中 silent 恢复 (出问题就是出问题, 不要藏)
+- ❌ 不要使用 `power_factor > 5` 除非你已经成功跑过 factor=1
+- ❌ 不要在狗站立或行走时直接 kill Legged_sport (狗会软瘫倒地)