From 5ecd422a5926eaaf120ba55d3880b8409f4e716d Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Sat, 20 Jun 2026 20:02:36 +0800 Subject: [PATCH] init pro_sdk --- .gitignore | 29 +++ LICENSE | 24 +++ README.md | 129 +++++++++++ docs/ARCHITECTURE.md | 142 ++++++++++++ docs/PROTOCOL.md | 145 +++++++++++++ docs/REVERSE_ENGINEERING.md | 103 +++++++++ docs/SAFETY.md | 66 ++++++ examples/README.md | 61 ++++++ examples/example_position.py | 164 ++++++++++++++ examples/example_remote_control.py | 100 +++++++++ examples/example_sin_leg.py | 111 ++++++++++ examples/monitor_remote.py | 78 +++++++ examples/monitor_state.py | 79 +++++++ go1_pro_sdk/__init__.py | 68 ++++++ go1_pro_sdk/_data/README.md | 11 + go1_pro_sdk/_data/blowfish_state.bin | Bin 0 -> 4168 bytes go1_pro_sdk/codec/__init__.py | 10 + go1_pro_sdk/codec/blowfish.py | 132 ++++++++++++ go1_pro_sdk/codec/lowcmd_builder.py | 82 +++++++ go1_pro_sdk/codec/lowstate_parser.py | 69 ++++++ go1_pro_sdk/connection/__init__.py | 4 + go1_pro_sdk/connection/mcu_client.py | 145 +++++++++++++ go1_pro_sdk/safety/__init__.py | 16 ++ go1_pro_sdk/safety/safety.py | 115 ++++++++++ go1_pro_sdk/types/__init__.py | 14 ++ go1_pro_sdk/types/bms.py | 48 +++++ go1_pro_sdk/types/imu.py | 31 +++ go1_pro_sdk/types/low_cmd.py | 40 ++++ go1_pro_sdk/types/low_state.py | 33 +++ go1_pro_sdk/types/motor.py | 83 +++++++ go1_pro_sdk/types/remote.py | 54 +++++ go1_pro_sdk/utils/__init__.py | 30 +++ go1_pro_sdk/utils/common.py | 170 +++++++++++++++ go1_pro_sdk/utils/constants.py | 90 ++++++++ pyproject.toml | 40 ++++ tests/test_blowfish.py | 48 +++++ tests/test_lowcmd_builder.py | 94 ++++++++ tests/test_safety.py | 82 +++++++ tools/README.md | 79 +++++++ tools/_compare_lowcmd.py | 212 ++++++++++++++++++ tools/_raw_capture.py | 121 +++++++++++ tools/analyze_blowfish_dump.py | 310 +++++++++++++++++++++++++++ tools/calibrate_joints.py | 99 +++++++++ tools/capture_lowcmd.sh | 73 +++++++ tools/diff_real_lowcmd.sh | 76 +++++++ tools/extract_blowfish_key.sh | 208 ++++++++++++++++++ tools/run_on_mac.sh | 83 +++++++ tools/run_on_pi.sh | 104 +++++++++ tools/stop_sportmode.sh | 121 +++++++++++ 49 files changed, 4226 insertions(+) create mode 100644 .gitignore create mode 100644 LICENSE create mode 100644 README.md create mode 100644 docs/ARCHITECTURE.md create mode 100644 docs/PROTOCOL.md create mode 100644 docs/REVERSE_ENGINEERING.md create mode 100644 docs/SAFETY.md create mode 100644 examples/README.md create mode 100644 examples/example_position.py create mode 100644 examples/example_remote_control.py create mode 100644 examples/example_sin_leg.py create mode 100644 examples/monitor_remote.py create mode 100644 examples/monitor_state.py create mode 100644 go1_pro_sdk/__init__.py create mode 100644 go1_pro_sdk/_data/README.md create mode 100644 go1_pro_sdk/_data/blowfish_state.bin create mode 100644 go1_pro_sdk/codec/__init__.py create mode 100644 go1_pro_sdk/codec/blowfish.py create mode 100644 go1_pro_sdk/codec/lowcmd_builder.py create mode 100644 go1_pro_sdk/codec/lowstate_parser.py create mode 100644 go1_pro_sdk/connection/__init__.py create mode 100644 go1_pro_sdk/connection/mcu_client.py create mode 100644 go1_pro_sdk/safety/__init__.py create mode 100644 go1_pro_sdk/safety/safety.py create mode 100644 go1_pro_sdk/types/__init__.py create mode 100644 go1_pro_sdk/types/bms.py create mode 100644 go1_pro_sdk/types/imu.py create mode 100644 go1_pro_sdk/types/low_cmd.py create mode 100644 go1_pro_sdk/types/low_state.py create mode 100644 go1_pro_sdk/types/motor.py create mode 100644 go1_pro_sdk/types/remote.py create mode 100644 go1_pro_sdk/utils/__init__.py create mode 100644 go1_pro_sdk/utils/common.py create mode 100644 go1_pro_sdk/utils/constants.py create mode 100644 pyproject.toml create mode 100644 tests/test_blowfish.py create mode 100644 tests/test_lowcmd_builder.py create mode 100644 tests/test_safety.py create mode 100644 tools/README.md create mode 100644 tools/_compare_lowcmd.py create mode 100644 tools/_raw_capture.py create mode 100644 tools/analyze_blowfish_dump.py create mode 100644 tools/calibrate_joints.py create mode 100644 tools/capture_lowcmd.sh create mode 100644 tools/diff_real_lowcmd.sh create mode 100644 tools/extract_blowfish_key.sh create mode 100644 tools/run_on_mac.sh create mode 100644 tools/run_on_pi.sh create mode 100755 tools/stop_sportmode.sh diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..43ca4c6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,29 @@ +__pycache__/ +*.py[cod] +*$py.class +*.so +.Python +build/ +dist/ +*.egg-info/ +.installed.cfg +*.egg + +# IDE +.vscode/ +.idea/ +*.swp +.DS_Store + +# Test/coverage +.pytest_cache/ +.coverage +htmlcov/ + +# Local data: 默认排除, 但 data/captures/ 是我们历史抓包数据 (显式入库) +data/* +!data/captures/ + +# Other captures (不入库) +captures/ +*.local.pcap diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..1b1894b --- /dev/null +++ b/LICENSE @@ -0,0 +1,24 @@ +MIT License + +Copyright (c) 2026 chenyouyuan + +Based on free-dog-sdk (https://github.com/Bin4ry/free-dog-sdk) +Copyright (c) 2024 Bin4ry (Andreas Makris) + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..fb5cb5f --- /dev/null +++ b/README.md @@ -0,0 +1,129 @@ +# Go1 PRO SDK + +Unitree Go1 **PRO** 机型的低层电机控制 Python SDK。完整逆向 PRO 版的私有协议（Blowfish 加密 + 私有 LowCmd 格式），不依赖官方 C++ SDK，Mac 直连可达 480Hz 控制频率。 + +> 这是为 PRO 准备的 SDK。如果你有 **EDU** 版机器狗，请用原版 [free-dog-sdk](https://github.com/Bin4ry/free-dog-sdk) — PRO 跟 EDU 协议有显著差异，互不兼容。 + +## 关键特性 + +- 完整 Blowfish ECB 加解密（用从机器狗内存提取的 state，不依赖原始 key） +- PRO 私有 LowCmd 格式（616B / CRC@612 / bandWidth BE 字节序），跟 EDU 不同 +- LowState 解析（12 关节 + IMU + BMS + 足端力 + 遥控器） +- 实时遥控器按键/摇杆/L2 读取 +- 三层安全保护（PositionLimit / PowerProtect 1-10 / PositionProtect），实测扭矩过载自动停机 +- 实测频率: **Mac 直连 480Hz**（接近原版 EDU 500Hz） +- 实测延迟: **每帧 1.74ms p50**（Blowfish 加密 1ms + 解密 0.7ms + sendto 0.02ms） + +## 快速上手 + +```python +from go1_pro_sdk import MCUClient, LowCmd, MotorCmd, MotorMode + +with MCUClient() as client: + client.wake_mcu() # 唤醒并切换为我们的客户端 + state = client.recv_state() # 读一帧状态 + print(f"FR_0 q = {state.motorState[0].q}") + print(f"电量: {state.bms.SOC}%") + print(f"遥控器按下: {state.remote.pressed}") + + # 控制 FR 大腿 (注意先悬空 + 安全保护层) + cmd = LowCmd() + cmd.set_motor('FR_1', MotorCmd( + mode=MotorMode.Servo, q=1.2, Kp=5, Kd=1 + )) + client.send(cmd) + + client.safe_stop() # 退出前发 damping +``` + +## 准备工作 + +### 1. 提取 Blowfish 密钥（仅需一次） + +`go1_pro_sdk/_data/blowfish_state.bin` 已包含我们项目用的密钥。如果你的狗用的是不同的 key（很少见，除非固件升级），需要重新提取： + +```bash +# SSH 到狗 (192.168.123.161) +scp tools/extract_blowfish_key.sh pi@192.168.123.161:/tmp/ +ssh pi@192.168.123.161 "sudo bash /tmp/extract_blowfish_key.sh" +scp pi@192.168.123.161:/tmp/blowfish_dump.tar.gz ./data/ + +# Mac 端分析 +python tools/analyze_blowfish_dump.py data/blowfish_dump.tar.gz +# → 输出 go1_pro_sdk/_data/blowfish_state.bin +``` + +### 2. 停掉狗上的抢占源 + +PRO 的 MCU 一次只接受一个客户端的命令。要让 SDK 工作，必须先停掉狗上的: + +```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" +``` + +或用我们提供的: + +```bash +bash tools/stop_sportmode.sh stop +``` + +### 3. 跑示例 + +```bash +# 只读监听 LowState +python examples/monitor_state.py --duration 30 --verbose + +# 监听遥控器 +python examples/monitor_remote.py + +# 单腿 sin 摆动 (狗悬空, 振幅 0.3 rad) +python examples/example_sin_leg.py --amplitude 0.3 --freq 0.5 + +# 复刻原版 example_position(lowlevel).py +python examples/example_position.py +``` + +### 4. 测完恢复 + +```bash +bash tools/stop_sportmode.sh start +``` + +## ⚠️ 安全 + +直接控制 12 个电机是**危险操作**： + +- 第一次测试 **狗必须悬空** (脚架/吊带) +- 用 `apply_safety(cmd, state, power_factor=1)` 限制力矩到 10% +- 准备 **拔电池** 作为终极停机 (Legged_sport 被杀后，遥控器 L2+B 不会工作) +- 见 `docs/SAFETY.md` + +## 包结构 + +``` +go1_pro_sdk/ +├── connection/ # MCUClient: 高层 UDP 客户端 +├── codec/ # Blowfish + LowCmd 序列化 + LowState 解析 +├── types/ # MotorCmd, LowState, IMU, BMS, RemoteState 等 +├── safety/ # PositionLimit / PowerProtect / PositionProtect +├── utils/ # CRC, 浮点编解码, 关节常量 +└── _data/ # blowfish_state.bin +``` + +## 文档 + +- `docs/PROTOCOL.md` — PRO LowCmd/LowState 字节级格式规格 +- `docs/SAFETY.md` — 安全测试清单 +- `docs/REVERSE_ENGINEERING.md` — 逆向工程过程精华 +- `docs/ARCHITECTURE.md` — 包结构与数据流 + +## 致谢 + +- 原版 [free-dog-sdk](https://github.com/Bin4ry/free-dog-sdk) by Bin4ry (Andreas Makris) — 提供了 EDU 版的基础数据结构和 CRC 实现 +- Unitree 官方 [unitree_legged_sdk](https://github.com/unitreerobotics/unitree_legged_sdk) — 公开了 safety.h 的接口定义 + +## License + +MIT (跟 free-dog-sdk 一致) diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md new file mode 100644 index 0000000..770988e --- /dev/null +++ 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。 diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md new file mode 100644 index 0000000..fc98e28 --- /dev/null +++ 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 反复切换导致控制失败。 diff --git a/docs/REVERSE_ENGINEERING.md b/docs/REVERSE_ENGINEERING.md new file mode 100644 index 0000000..0b83e22 --- /dev/null +++ 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 一致) | diff --git a/docs/SAFETY.md b/docs/SAFETY.md new file mode 100644 index 0000000..4fe27d8 --- /dev/null +++ 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 (狗会软瘫倒地) diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 0000000..58d91e2 --- /dev/null +++ b/examples/README.md @@ -0,0 +1,61 @@ +# Examples + +每个示例都假定: +- 你已经从狗上提取了 `blowfish_state.bin` (或用包内置的) +- 已经停掉 Pi 上的 keep_sport_alive + Legged_sport + appTransit +- 狗悬空 (脚架/吊带), 除非明确说不需要 + +## 只读类 (无风险) + +### monitor_state.py — 实时监听状态 + +```bash +python examples/monitor_state.py --duration 30 --verbose +``` + +不触发任何电机, 持续发 damping LowCmd 维持 MCU 回包流, 解码后打印 12 关节角 + IMU + 电量. + +### monitor_remote.py — 实时监听遥控器 + +```bash +python examples/monitor_remote.py --duration 60 +``` + +按下按键/拨动摇杆都会实时输出. 包括 L2 模拟值. + +## 控制类 (需要狗悬空!) + +### example_sin_leg.py — 单腿小幅 sin + +```bash +python examples/example_sin_leg.py --amplitude 0.3 --freq 0.5 --duration 10 +``` + +**保守参数**, 适合首次电机控制. sin 中点 = 当前关节角, 不强行移动到指定位置. + +### example_position.py — 复刻原版 example_position + +```bash +python examples/example_position.py --max-steps 5000 +``` + +跟 free-dog-sdk `example_position(lowlevel).py` **逻辑一模一样**: +- 0..10: 记录 qInit +- 10..400: 插值到 sin_mid_q = [0, 1.2, -2.0] +- 400+: 1Hz sin 摆动 (FR_1 ±0.6, FR_2 ±0.9) + +Mac 上实测 480 Hz. + +### example_remote_control.py — 摇杆控制 FR 腿 + +```bash +python examples/example_remote_control.py +``` + +- 左摇杆 X → 髋外摆 +- 左摇杆 Y → 大腿前后 +- L2 按下 → 退出 + +## 安全 + +所有控制类示例都内置 `apply_safety(cmd, state, power_factor=1)` (10% 力矩限制), 出问题会立即停机. 见 `docs/SAFETY.md`. diff --git a/examples/example_position.py b/examples/example_position.py new file mode 100644 index 0000000..58735a1 --- /dev/null +++ b/examples/example_position.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python3 +"""复刻 free-dog-sdk example_position(lowlevel).py 的 PRO 版本. + +跟原版 EDU 例程逻辑一致: + 0..10 步: 记录初始关节角 qInit + 10..400 步: Kp=5 Kd=1 插值到 sin_mid_q = [0.0, 1.2, -2.0] + 400+ 步: 1Hz sin 摆动 (FR_1 += 0.6*sin, FR_2 += -0.9*sin) + +差异: + - 用 Go1 PRO SDK (Blowfish 加密 / PRO 格式 / 安全保护) + - 集成 safety 层 (power_factor=1, 位置硬限位) + +⚠️ 狗必须悬空 (脚架/吊带). Ctrl+C 紧急停止. +""" +import argparse +import math +import signal +import sys +import time + +from go1_pro_sdk import ( + MCUClient, LowCmd, MotorCmd, MotorMode, + apply_safety, PowerProtectViolation, + JOINT_NAMES, +) + +# 跟原版一致的常量 +SIN_MID = {'hip': 0.0, 'thigh': 1.2, 'knee': -2.0} +DT = 0.002 + +running = True + + +def sigint(s, f): + global running + running = False + + +def interp(a, b, t): + t = max(0.0, min(1.0, t)) + return a * (1 - t) + b * t + + +def main(): + p = argparse.ArgumentParser() + p.add_argument('--state', default=None) + p.add_argument('--max-steps', type=int, default=5000) + p.add_argument('--freq-hz', type=float, default=1.0) + p.add_argument('--power-factor', type=int, default=1, choices=range(1, 11), + help='PowerProtect 1-10, 越大越宽松') + args = p.parse_args() + signal.signal(signal.SIGINT, sigint) + + print('🐕 PRO example_position (Go1 PRO SDK)') + print(f' 控制周期 {DT*1000:.0f}ms ({1/DT:.0f} Hz)') + print(f' sin 中点: {SIN_MID}') + print(f' sin 频率: {args.freq_hz} Hz') + print(f' power_factor: {args.power_factor} (限 {args.power_factor*10}% 力矩)') + print(f' 最大步数: {args.max_steps} (≈{args.max_steps*DT:.1f}s)') + + with MCUClient(state_path=args.state) as client: + print(f'\n[Phase 0] 唤醒 MCU...') + recv = client.wake_mcu(50) + if recv == 0: + print('❌ 无回包. 先停 Pi 上的 keep_sport_alive/Legged_sport/appTransit') + return 1 + state = client.last_state + print(f' 当前 FR_0={state.motorState[0].q:.4f} ' + f'FR_1={state.motorState[1].q:.4f} ' + f'FR_2={state.motorState[2].q:.4f}') + + # 主循环 + qInit = [0.0, 0.0, 0.0] + qDes = [0.0, 0.0, 0.0] + Kp = [0.0, 0.0, 0.0] + Kd = [0.0, 0.0, 0.0] + sin_count = 0 + rate_count = 0 + motiontime = 0 + freq_rad = args.freq_hz * 2 * math.pi + + print(f'\n[主循环]') + loop_start_t = time.time() + loop_times = [] + last_print = time.time() + + try: + while running and motiontime < args.max_steps: + t0 = time.time() + motiontime += 1 + state = client.recv_latest() or state + + if motiontime < 10: + for i in range(3): + qInit[i] = state.motorState[i].q + + if 10 <= motiontime < 400: + rate_count += 1 + rate = rate_count / 200.0 + Kp = [5, 5, 5] + Kd = [1, 1, 1] + qDes[0] = interp(qInit[0], SIN_MID['hip'], rate) + qDes[1] = interp(qInit[1], SIN_MID['thigh'], rate) + qDes[2] = interp(qInit[2], SIN_MID['knee'], rate) + + if motiontime >= 400: + sin_count += 1 + t = DT * sin_count + sin_j1 = 0.6 * math.sin(t * freq_rad) + sin_j2 = -0.9 * math.sin(t * freq_rad) + qDes[0] = SIN_MID['hip'] + qDes[1] = SIN_MID['thigh'] + sin_j1 + qDes[2] = SIN_MID['knee'] + sin_j2 + + # 构造命令: 只控 FR (索引 0,1,2) + cmd = LowCmd() + for i in range(3): + cmd.motorCmd[i] = MotorCmd( + mode=MotorMode.Servo, + q=qDes[i], dq=0, + tau=-0.65 if i == 0 else 0.0, # FR_0 预紧 + Kp=Kp[i], Kd=Kd[i], + ) + + # 安全保护 + try: + apply_safety(cmd, state, power_factor=args.power_factor, + position_limit_on=True, + position_protect_limit=0.5) + except PowerProtectViolation as e: + print(f'\n⚠️ {e}') + break + + client.send(cmd) + loop_times.append(time.time() - t0) + + now = time.time() + if now - last_print >= 0.5: + phase = 'qInit' if motiontime < 10 else 'ramp' if motiontime < 400 else 'sin ' + print(f' t={motiontime*DT:5.2f}s [{phase}] ' + f'des=[{qDes[0]:+.3f} {qDes[1]:+.3f} {qDes[2]:+.3f}] ' + f'act=[{state.motorState[0].q:+.3f} ' + f'{state.motorState[1].q:+.3f} {state.motorState[2].q:+.3f}]') + last_print = now + + # 维持 DT + sleep_t = DT - (time.time() - t0) + if sleep_t > 0: + time.sleep(sleep_t) + + # 时序统计 + if loop_times: + total = time.time() - loop_start_t + avg_ms = sum(loop_times) / len(loop_times) * 1000 + print(f'\n实际频率: {len(loop_times)/total:.1f} Hz, ' + f'每步均值 {avg_ms:.2f}ms') + finally: + print('\n安全停机...') + client.safe_stop() + print('✅ 退出') + + +if __name__ == '__main__': + sys.exit(main() or 0) diff --git a/examples/example_remote_control.py b/examples/example_remote_control.py new file mode 100644 index 0000000..ec2cad2 --- /dev/null +++ b/examples/example_remote_control.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 +"""用遥控器摇杆控制 FR 腿 (Demo: 摇杆 X → 髋外摆, 摇杆 Y → 大腿). + +按键映射: + 左摇杆 X → FR_0 hip (±0.4 rad) + 左摇杆 Y → FR_1 thigh (mid ± 0.5 rad) + L2 按下 → 立即 damping 退出 (软急停) + 其他腿 → Damping + +⚠️ 狗必须悬空 (脚架/吊带) +""" +import argparse +import signal +import sys +import time + +from go1_pro_sdk import ( + MCUClient, LowCmd, MotorCmd, MotorMode, + apply_safety, PowerProtectViolation, +) + +DT = 0.005 # 200 Hz 即可, 摇杆响应慢 +running = True + + +def sigint(s, f): + global running + running = False + + +def main(): + p = argparse.ArgumentParser() + p.add_argument('--state', default=None) + p.add_argument('--duration', type=float, default=60) + p.add_argument('--hip-range', type=float, default=0.4, help='hip 摆幅 (rad)') + p.add_argument('--thigh-range', type=float, default=0.5, help='thigh 摆幅 (rad)') + args = p.parse_args() + signal.signal(signal.SIGINT, sigint) + + print('🎮 摇杆遥控 FR 腿') + print(f' 左 X → hip ±{args.hip_range}, 左 Y → thigh ±{args.thigh_range}') + print(f' L2 按下 → 退出') + + with MCUClient(state_path=args.state) as client: + if client.wake_mcu(50) == 0: + print('❌ 无回包') + return 1 + state = client.last_state + mid = [state.motorState[i].q for i in range(3)] + print(f' 初始角: hip={mid[0]:+.3f} thigh={mid[1]:+.3f} knee={mid[2]:+.3f}') + + start = time.time() + try: + while running and time.time() - start < args.duration: + t0 = time.time() + state = client.recv_latest() or state + r = state.remote + + # L2 按下 → 退出 + if r.is_pressed('L2'): + print('\nL2 → 退出') + break + + # 摇杆 → 目标 q + q_hip = mid[0] + r.lx * args.hip_range + q_thigh = mid[1] + r.ly * args.thigh_range + q_knee = mid[2] # 膝盖保持不变 + + cmd = LowCmd() + for i, q in enumerate([q_hip, q_thigh, q_knee]): + cmd.motorCmd[i] = MotorCmd( + mode=MotorMode.Servo, q=q, dq=0, tau=0, Kp=3, Kd=0.5) + + try: + apply_safety(cmd, state, power_factor=1, + position_protect_limit=0.5) + except PowerProtectViolation as e: + print(f'\n⚠️ {e}') + break + + client.send(cmd) + + # 每秒打印一次 + if int(time.time() * 2) != int((time.time() - DT) * 2): + print(f' L=({r.lx:+.2f},{r.ly:+.2f}) ' + f'des=({q_hip:+.3f},{q_thigh:+.3f}) ' + f'act=({state.motorState[0].q:+.3f},' + f'{state.motorState[1].q:+.3f})') + + sleep_t = DT - (time.time() - t0) + if sleep_t > 0: + time.sleep(sleep_t) + finally: + print('\n安全停机...') + client.safe_stop() + print('✅ 退出') + + +if __name__ == '__main__': + sys.exit(main() or 0) diff --git a/examples/example_sin_leg.py b/examples/example_sin_leg.py new file mode 100644 index 0000000..91e1cb1 --- /dev/null +++ b/examples/example_sin_leg.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +"""安全单腿正弦测试 (保守参数, 小幅摆动). + +跟 example_position.py 的区别: + - 默认振幅小 (±0.3 rad), 频率慢 (0.5 Hz) + - sin 中点保持当前实际关节角, 不强行移到 -2.0 + - 适合首次试电机控制 + +⚠️ 狗悬空, Ctrl+C 紧急停止 +""" +import argparse +import math +import signal +import sys +import time + +from go1_pro_sdk import ( + MCUClient, LowCmd, MotorCmd, MotorMode, + apply_safety, PowerProtectViolation, +) + +DT = 0.002 +running = True + + +def sigint(s, f): + global running + running = False + + +def main(): + p = argparse.ArgumentParser() + p.add_argument('--state', default=None) + p.add_argument('--amplitude', type=float, default=0.3, help='sin 振幅 (rad)') + p.add_argument('--freq', type=float, default=0.5, help='sin 频率 (Hz)') + p.add_argument('--duration', type=float, default=10) + p.add_argument('--power-factor', type=int, default=1) + args = p.parse_args() + signal.signal(signal.SIGINT, sigint) + + print(f'🦿 单腿 sin 测试 — 振幅 {args.amplitude}, 频率 {args.freq}Hz, ' + f'{args.duration}s, power={args.power_factor}') + + with MCUClient(state_path=args.state) as client: + if client.wake_mcu(50) == 0: + print('❌ 无回包') + return 1 + state = client.last_state + + # 用实际值作为 sin 中点 + mid = [state.motorState[i].q for i in range(3)] + print(f' sin 中点 (= 当前角): {[f"{x:+.3f}" for x in mid]}') + + # Phase 1: 软启动 (Kp/Kd 从 0 慢慢升) + ramp_steps = 500 + print(f' 软启动 {ramp_steps*DT:.1f}s...') + + try: + n_steps = int(args.duration / DT) + for step in range(n_steps + ramp_steps): + if not running: + break + t0 = time.time() + state = client.recv_latest() or state + + if step < ramp_steps: + # 软启动: Kp/Kd 从 0 到目标 + ramp = step / ramp_steps + Kp = 3 * ramp + Kd = 0.5 * ramp + qDes = list(mid) + else: + Kp = 3 + Kd = 0.5 + t = (step - ramp_steps) * DT + sin_v = args.amplitude * math.sin(2 * math.pi * args.freq * t) + qDes = [mid[0], mid[1] + sin_v, mid[2] - sin_v * 1.5] + + cmd = LowCmd() + for i in range(3): + cmd.motorCmd[i] = MotorCmd( + mode=MotorMode.Servo, + q=qDes[i], dq=0, tau=0, + Kp=Kp, Kd=Kd, + ) + + try: + apply_safety(cmd, state, power_factor=args.power_factor, + position_protect_limit=0.5) + except PowerProtectViolation as e: + print(f'⚠️ {e}') + break + + client.send(cmd) + + if step % 250 == 0 and step > 0: + actual = [state.motorState[i].q for i in range(3)] + print(f' t={step*DT:5.2f}s des={[f"{x:+.3f}" for x in qDes]} ' + f'act={[f"{x:+.3f}" for x in actual]}') + + sleep_t = DT - (time.time() - t0) + if sleep_t > 0: + time.sleep(sleep_t) + finally: + print('\n安全停机...') + client.safe_stop() + print('✅ 退出') + + +if __name__ == '__main__': + sys.exit(main() or 0) diff --git a/examples/monitor_remote.py b/examples/monitor_remote.py new file mode 100644 index 0000000..57e1d97 --- /dev/null +++ b/examples/monitor_remote.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 +"""实时监听遥控器状态 (按键 + 摇杆 + L2). + +用法: python examples/monitor_remote.py --duration 60 +""" +import argparse +import signal +import sys +import time + +from go1_pro_sdk import MCUClient, LowCmd + +running = True + + +def sigint(s, f): + global running + running = False + + +def main(): + p = argparse.ArgumentParser() + p.add_argument('--state', default=None) + p.add_argument('--duration', type=float, default=60) + p.add_argument('--raw', action='store_true') + args = p.parse_args() + signal.signal(signal.SIGINT, sigint) + + with MCUClient(state_path=args.state) as client: + recv = client.wake_mcu(50) + if recv == 0: + print('❌ 无回包') + return 1 + + damping = LowCmd().all_damping() + last_btn = 0 + last_print = 0 + start = time.time() + + print(f'📡 监听遥控器 {args.duration}s, Ctrl+C 退出\n') + + while running and time.time() - start < args.duration: + client.send(damping) + time.sleep(0.005) + state = client.recv_latest() + if state is None: + continue + + r = state.remote + now = time.time() + + # 按键事件 + if r.btn != last_btn: + pressed_now = set(r.pressed) + pressed_before = set(n for m, n in __import__('go1_pro_sdk').BUTTON_NAMES if last_btn & m) + just_p = pressed_now - pressed_before + just_r = pressed_before - pressed_now + if just_p: + print(f"[{now-start:6.2f}s] ⬇ {'+'.join(sorted(just_p))}") + if just_r: + print(f"[{now-start:6.2f}s] ⬆ {'+'.join(sorted(just_r))}") + last_btn = r.btn + + # 摇杆 (0.2s 一次) + if now - last_print > 0.2: + active = (abs(r.lx) > 0.02 or abs(r.ly) > 0.02 or + abs(r.rx) > 0.02 or abs(r.ry) > 0.02 or r.L2 > 0.02) + if active: + print(f'[{now-start:6.2f}s] L=({r.lx:+.2f},{r.ly:+.2f}) ' + f'R=({r.rx:+.2f},{r.ry:+.2f}) L2={r.L2:+.2f}' + + (f' raw={state.wirelessRemote.hex()}' if args.raw else '')) + last_print = now + + print('\n✅ 退出') + + +if __name__ == '__main__': + sys.exit(main() or 0) diff --git a/examples/monitor_state.py b/examples/monitor_state.py new file mode 100644 index 0000000..3872477 --- /dev/null +++ b/examples/monitor_state.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 +"""实时监听 LowState (只读, 不控制电机). + +用法: + python examples/monitor_state.py --duration 30 --verbose +""" +import argparse +import math +import signal +import sys +import time + +from go1_pro_sdk import MCUClient, JOINT_NAMES, decode_sn + +running = True + + +def sigint(s, f): + global running + running = False + + +def main(): + p = argparse.ArgumentParser() + p.add_argument('--state', default=None, help='Blowfish state 文件 (默认用包内置)') + p.add_argument('--duration', type=float, default=30) + p.add_argument('--rate', type=float, default=5, help='打印频率 Hz') + p.add_argument('--verbose', '-v', action='store_true') + args = p.parse_args() + signal.signal(signal.SIGINT, sigint) + + print('📡 监听 LowState') + print(f' 时长 {args.duration}s, 打印 {args.rate}Hz') + + with MCUClient(state_path=args.state) as client: + recv_count = client.wake_mcu(50) + print(f'唤醒: 收到 {recv_count}/50 帧') + if recv_count == 0: + print('❌ 无回包. 检查 Pi 抢占源或 state 是否正确') + return 1 + + start = time.time() + last_print = 0 + n_decoded = 0 + damping = __import__('go1_pro_sdk').LowCmd().all_damping() + + print(f"\n{'时间':>6} {'电量':>5} {'FR_0':>8} {'FR_1':>8} {'FR_2':>8} {'rpy[°]':>20}") + print('-' * 70) + + while running and time.time() - start < args.duration: + client.send(damping) + time.sleep(0.005) + state = client.recv_latest() + if state is None: + continue + n_decoded += 1 + + now = time.time() + if now - last_print >= 1.0 / args.rate: + t = now - start + rpy_str = (f"{math.degrees(state.imu.rpy[0]):+5.1f}," + f"{math.degrees(state.imu.rpy[1]):+5.1f}," + f"{math.degrees(state.imu.rpy[2]):+5.1f}") + print(f'{t:6.1f} {state.bms.SOC:4d}% ' + f'{state.motorState[0].q:+8.3f} ' + f'{state.motorState[1].q:+8.3f} ' + f'{state.motorState[2].q:+8.3f} ' + f'{rpy_str:>20}') + if args.verbose: + print(f' SN={decode_sn(state.SN)} ' + f'acc_z={state.imu.accelerometer[2]:.2f} m/s² ' + f'BMS current={state.bms.current_a:.2f}A') + last_print = now + + print(f'\n解码 {n_decoded} 帧, 退出.') + + +if __name__ == '__main__': + sys.exit(main() or 0) diff --git a/go1_pro_sdk/__init__.py b/go1_pro_sdk/__init__.py new file mode 100644 index 0000000..e8ad83f --- /dev/null +++ b/go1_pro_sdk/__init__.py @@ -0,0 +1,68 @@ +"""Go1 PRO Python SDK. + +完整的 Unitree Go1 PRO 低层控制 Python 实现, 经实测验证 (Mac 直连 480Hz): + - Blowfish ECB 加解密 (MCU 期待) + - PRO 私有 LowCmd 格式 (616B, CRC@612, bandWidth BE) — 跟 EDU 不同 + - LowState 解析 (12 关节 + IMU + BMS + 足端力 + 遥控器) + - 安全保护层 (PositionLimit / PowerProtect / PositionProtect) + +快速上手: + + from go1_pro_sdk import MCUClient, LowCmd, MotorCmd, MotorMode + + with MCUClient() as client: + client.wake_mcu() # 唤醒 + state = client.recv_state() # 读一帧状态 + print(f"FR_0 q = {state.motorState[0].q}") + + cmd = LowCmd() + cmd.set_motor('FR_1', MotorCmd( + mode=MotorMode.Servo, q=1.2, Kp=5, Kd=1 + )) + client.send(cmd) + + client.safe_stop() # 退出前发 damping +""" +from .connection.mcu_client import MCUClient +from .codec.blowfish import Blowfish, verify_state +from .codec.lowcmd_builder import build_low_cmd_plain, build_low_cmd_encrypted +from .codec.lowstate_parser import parse_low_state +from .types import ( + MotorCmd, MotorState, MotorMode, + IMU, BMS, + RemoteState, parse_remote, BUTTON_NAMES, + LowState, LowCmd, +) +from .safety import ( + apply_safety, position_limit, power_protect, position_protect, + PowerProtectViolation, +) +from .utils import ( + MCU_IP, MCU_PORT, + JOINT_NAMES, JOINT_TYPE, JOINT_LIMITS, JOINT_LIMITS_MEASURED, + TAU_MAX, KT, DAMPING_POSE, + decode_sn, decode_version, +) + +__version__ = '0.1.0' + +__all__ = [ + # 高层 API + 'MCUClient', + # 数据结构 + 'MotorCmd', 'MotorState', 'MotorMode', + 'IMU', 'BMS', + 'RemoteState', 'parse_remote', 'BUTTON_NAMES', + 'LowState', 'LowCmd', + # 编解码 + 'Blowfish', 'verify_state', + 'build_low_cmd_plain', 'build_low_cmd_encrypted', 'parse_low_state', + # 安全 + 'apply_safety', 'position_limit', 'power_protect', 'position_protect', + 'PowerProtectViolation', + # 常量 + 'MCU_IP', 'MCU_PORT', + 'JOINT_NAMES', 'JOINT_TYPE', 'JOINT_LIMITS', 'JOINT_LIMITS_MEASURED', + 'TAU_MAX', 'KT', 'DAMPING_POSE', + 'decode_sn', 'decode_version', +] diff --git a/go1_pro_sdk/_data/README.md b/go1_pro_sdk/_data/README.md new file mode 100644 index 0000000..664971b --- /dev/null +++ b/go1_pro_sdk/_data/README.md @@ -0,0 +1,11 @@ +# _data/ + +## blowfish_state.bin (4168 字节) + +从 Unitree Go1 PRO 内 Legged_sport 进程运行时内存提取的 Blowfish state. +布局: P[18] × 4B + S0[256] × 4B + S1[256] × 4B + S2[256] × 4B + S3[256] × 4B = 4168 字节. + +**等同于一个对称密钥**, 用同一个 state 既能加密 LowCmd 也能解密 LowState. + +提取方法见 `tools/extract_blowfish_key.sh`. 如果你的狗 Blowfish key 不一样 (固件升级后可能), +重新提取一份覆盖. diff --git a/go1_pro_sdk/_data/blowfish_state.bin b/go1_pro_sdk/_data/blowfish_state.bin new file mode 100644 index 0000000000000000000000000000000000000000..a009aeab9020005e33afd4f50fb1ed9c53fd1ca6 GIT binary patch literal 4168 zcmV-O5V!9)hJMfGD-4-;F*Uq?`2u=l&JGQ!XL}AexKJj{4NbAd#sO9jkAF4SNX=af zGgFNS*!U=ddbB#cwkZWQE(YLln)!6!$1PHUW!n2b&+O}yvLI5zY@6gRxh?9IEbhB2 zlBVVp2r(m@`+uyAuJ32@lX$k^7ERl{DT5THG-Wd0U@A61-gH4+y9|`TBN1rzYq0|y zNelF{^K%UhCwjWg*4xe%O{~U0p!S`7;CoCaBBkF)d0lPqGUitX#u2NH(6NDLPbDg3 ze*~+3K_%4a;v`2_W!n+ZY+Y{&OG0nl{Hhch($B!BFOditQ-Ka&0GfuI)4(ORk}VV( z)O!8z@3QWRvvr;APr5AaNGM~sY}9R9Hp?b-bs+YfbYIsxyM1}pEJOS6ankdl%(Dsx zn;Gz%);Ii=#>Eod%Gc~CTpKGzTH~D1rcQP%%bmj!PoOJ|Q?F=)RbMi9yC`Faz}~4V z=a%ei(LmlS^O2AU>?<4gF2}+0L6>Wsj(r%M19*^TMqPZYO!y`;3WAKOM79`xA%n)T zJlOmZV@v<*wzlI8Yk{6GEXDWI!cPedC%8u&!f%l@IZhMNPN3QXwWjs~&W@!|YHp{$ zm*jCCaGB`ENc4L{z2Yhbd9b?P>cUib-pL^5EPJU5Y+N_Ckwq;7Q%7zjtB>a5?&Qcy zuCu^^=tVPtW&F)8PA$TYQnVWy%7|NV{pwxVo{2r~O=9f%oIvO()9EGvOym=h#A3L? zZSq*T#>4)WO5?h_*z1b^0B)BOlbB$NL0oz01r1nN*j;pX=j*n0J=k704Z?KBpK!-@ znF`Naa!%2E`6m)`fgF)28{Eagkx`4?@4Ux$;X>T^$K7#ds9E>YcZtrUB?{ik5jA~9 z{Ur6M$)8?qk)-B~F0>rfV2{0&<9fsn>hiyzYL3xOHnxTBsvZ$BZ{VqNtspr6ySFfb z-cn%>0EjG{Cc*Ggte^G2y@6>}hD7CQ9n$(_)^o7OwDLCu=U;H>lR{8ak$+3UGggnx zoBwLZiI%#~#FxE0(>wz5@%|Q{c8tT5hjn;Pfn9{tXWa^sCm|F9HCM+mytzF!{=RSB znF(9eEHB7E${${T-DD3_c()EYhw6342B<3jh-<{+fzDH=5D3Ez=~esAfkAaluQw!X z2v}vXAkz!Y@x)_SbR*fq8<-l!{vqOLng4$UB(8^YU~$w1+Np%YeFiW%@v?^MW_e^@ zb_5lrd%_9Zl`JkEDf+#y4XkDQvz2|l#Vh1JJahYNWCHYQLz$kdtN#B8m8g1|2J(h% z_Cw6u==2TQh6z&%(NM+MR3r>%p#<{wzOaSrzWeL8g*kmUYQ#*q`zSVEbG`+-y7(eb zL1F?`FRmrzAmT^3l_Rlbs2eB53c7u`);tlK-8%i?bb7SN^QAJ@aN%ESfeY?|Z?zG& z+%EauEbs#R^HI{IU9T1EywK6Dk}O0m%qni4Xt2)| zl<=*H^%U_L)#3GTK3=<(KqM4{ep;e@Qq;i~fSi)T?#IXpFj{sj+kSxAh2hje8ov}J zOyzhfWC>6=Y)qOyQMR=!95`0dpq|VHB?Cggd`Z^;k2eHylEo+opU)+Q)av z;3jempmM$bQ^~XPo7K4O9AVT_1G>yUg8XU|?okN%jx07`?R+#Muq6IulelGiyUFqP z9S^#mhPttOdG9Ef}+3qI^go|;-P3- zN9Qr9G^B*o3j>58VT_c{0P{!Jp2;0|$+vk1_fd&A-+iWl%T zugWpS(nzDZ4U3gyHu#sAO`LZr4k*itiWM-)jm&h_e!pPm;uUuAh#F^Vb#^@qmO;1B zWLu)}JWne%zdExLTKQq1T)WFBjwDV{H+IVc7{pBi4 zjusI74BWO?i;``NikKO)ph%9sVrFF(FBqWws}o@Vc_jyUG?k3zr)Wylt^P-`d$}ao zY#{lDH~RC*8bK(cPm{|G^aTkau6hTTmgj8cAy=BmW(J9ug^YHVoZ=#UXHI)}V_>W@ zT+pf^#1S`^r00!(jiYl^UfDznwQf8`ftVSKiDg0cW&_>$ct7&`K+|^F_b1F4da*QK zWjA55Zx+gpVqjURuobjWK*0BUaJh{XYAuV!tKp(>`9KBtfwwut2>yws^+Yb-yc7=u z_|4+N$CkZ$g;yEy{NGT|lma}1sDB=T>ZvJHTovfQFvH7;G1vyKWKd7>iO`Z5FPcCu zbBt6(qhmW`Y)8r*K0k~l%n-_E6jTy8!&m}?t#iC`NslPh?KcHV-xW=>GSvt z&Uawa-X9-jr;QT`7o-9WD4R&oAF_ZqPVC~@E@cp%cUbg$Utdvm5udHHdi!VzNcrM| ze}Mk7rKdkVV261eHt<5CPKCnYdYX35J1FRjyb!INe;jd!ATN#MPw_c;3`moKM*jFa zNl7y!Ym|Q4Vu`-kp2A~D!4jhW-xW8aa5r$>HWrkk%m3%7f~yCrIVgzxl$y>{vWVh< zT5B8FPdSka-Z-l^B*S~qUU-g^O{np&bUU;f^wh4BfovBp@Yrw!1T+zNVE4&%%IHaX zfgWcB7a%&T8+VlD2k+dJ)Xt`5#-G!2;rC6h1KmKDIceSI&OX2dAb(^j`2$}VK~GH>dU75qb{5rv z-yVc>RIHBs8SDt-=bcj&Xak_~Ekq zqvWJheG+=#CLz^q8kC(!Ta~Dz{*aSHk2{0}Dl2;C*6 zAnfLSH@|`mebloa$VJ=G-x0ove0;vwrOX7@O*+&Z;?QZb!{%{%F+v!OvR)k~jKMI7 zf(oUln+K!6yLJiO%Nsw~GuwFL_ zLaE?Mui%t^izwiBg<0`AOBRG$f$s~?B9(O9)qTD19k&cg0g=j*Er;$6mEjP=a{lD) z!lctCl?)6qMR*^&AVDX(Z&ldHV~kR_Ki9s0yBYC*j?IcWys$5T`^djt0B2UjdTjFI z>IXfY1BeG5t%BC%`nEeAu3|Pmge0K;-&EX%mi$1g9nR z`MAgRk^pjO8Ic?ECH>@=>S~$m4_KaQzYm!A?q9&vSBWu(S=MR#JkF1#)CbSb(8OJ_ zGTml+uloB?eK!4u_$JlpwaL2clP70AxE1K+OL;7vv!-%#-Mg1VRy3eCVoAGqKOfkG zZ6~3RW}mFF!s<$5u2$}!p!T^B<|mtMKy7QPadrWLj+$}l$x}Jo8ch4O6Z!#3y!O#M zO^ZdX9x48h`6)XIwu(lYPlOwR$!z~^K2MtRXKUO+=wdIzG_afqw!4>~Uc@~BDT&r8 zCf@Tg3(}@EJ6A5^?)Jn z8}z%jeAL@|ug7aXRs>FlWV^5b+p@c5m7Xe|56tYPMQRAj2!zE735A3so5x(RM#CP?OMXWxnt>ZBIg9=|gs! zuzL)eANOtN=Y>bW$l6=%mg9Dz5UZApFVGfR9)>37RO=tKq!TyFryx<{@_`u+S)*ID zYhrXWkbl+z3~-5Q6xO~>aP*QZ<>+fpQxMA`kI7TDE#C=+%<-Ol$~fx%K9M0$Th`T( z>P;{&gOz>i4MC4EALd0{>LfnxYjud<5-iX-CGE}Z`x~z7(;Pa8&zh-RFpw(l_SME? zuRxOYRSkFqZsT2KgW_>}6dknWo3bon3}TbA3nW<%hz<;0ZXhzUFZXbKQfk2+@blID zR)5~ox-GU%^b9|R?{e~Mo6oF7ey31R61uWnrzzf`EzgKQiP~$a_I01e@}~Jrq5}u7 zp-GI#+)vJ9GWi-F`NV58dKBpV$3mqTR!}0-sK=K-YQ%#%+wqKE=31Seh9Zo(W6G%R zkr$?@0gcsU2MSKb)^>II' + + @classmethod + def from_state_buffer(cls, buf: bytes, endian: str = 'little') -> 'Blowfish': + """从 4168 字节内存 dump 构造.""" + assert len(buf) >= 4168, f'state 至少 4168 字节, got {len(buf)}' + words = struct.unpack('<1042I', buf[:4168]) + return cls( + words[0:18], + words[18:274], + words[274:530], + words[530:786], + words[786:1042], + endian=endian, + ) + + @classmethod + def from_state_file(cls, path: str, endian: str = 'little') -> 'Blowfish': + """从文件加载 state.""" + with open(path, 'rb') as f: + buf = f.read() + return cls.from_state_buffer(buf, endian=endian) + + def _F(self, X: int) -> int: + """F-function: ((S0[a]+S1[b]) ^ S2[c]) + S3[d]. + + 注意 Python 运算符优先级: + 高于 ^, 必须完整括号! + """ + a = (X >> 24) & 0xff + b = (X >> 16) & 0xff + c = (X >> 8) & 0xff + d = X & 0xff + return ((((self.S[0][a] + self.S[1][b]) & 0xFFFFFFFF) + ^ self.S[2][c]) + self.S[3][d]) & 0xFFFFFFFF + + def encrypt_block(self, plaintext: bytes) -> bytes: + """单个 8B 块加密.""" + assert len(plaintext) == 8 + L, R = struct.unpack(self._pack, plaintext) + for i in range(16): + L ^= self.P[i] + R ^= self._F(L) + L, R = R, L + L, R = R, L + R ^= self.P[16] + L ^= self.P[17] + return struct.pack(self._pack, L, R) + + def decrypt_block(self, ciphertext: bytes) -> bytes: + """单个 8B 块解密.""" + assert len(ciphertext) == 8 + L, R = struct.unpack(self._pack, ciphertext) + for i in range(17, 1, -1): + L ^= self.P[i] + R ^= self._F(L) + L, R = R, L + L, R = R, L + R ^= self.P[1] + L ^= self.P[0] + return struct.pack(self._pack, L, R) + + def encrypt_ecb(self, data: bytes) -> bytes: + """ECB 加密. 长度必须 8 的倍数.""" + assert len(data) % 8 == 0 + out = bytearray() + for i in range(0, len(data), 8): + out.extend(self.encrypt_block(data[i:i+8])) + return bytes(out) + + def decrypt_ecb(self, data: bytes) -> bytes: + """ECB 解密. 长度必须 8 的倍数.""" + assert len(data) % 8 == 0 + out = bytearray() + for i in range(0, len(data), 8): + out.extend(self.decrypt_block(data[i:i+8])) + return bytes(out) + + +# 已知明密文对, 用于验证 state 是否正确 +KNOWN_PAIRS = [ + (bytes.fromhex('0000000000000000'), bytes.fromhex('12aa0354236b66e3')), + (bytes.fromhex('feefff0004020305'), bytes.fromhex('5d9874ad7eee5851')), + (bytes.fromhex('0802010000010900'), bytes.fromhex('c5ce0887ceb86f91')), +] + + +def verify_state(bf: Blowfish) -> bool: + """用已知明密文对验证 state 是否正确. 返回是否 3/3 通过.""" + for pt, ct_expected in KNOWN_PAIRS: + if bf.encrypt_block(pt) != ct_expected: + return False + return True diff --git a/go1_pro_sdk/codec/lowcmd_builder.py b/go1_pro_sdk/codec/lowcmd_builder.py new file mode 100644 index 0000000..01faec7 --- /dev/null +++ b/go1_pro_sdk/codec/lowcmd_builder.py @@ -0,0 +1,82 @@ +"""LowCmd 序列化 (PRO 格式 616B + Blowfish 加密). + +PRO 格式跟 free-dog-sdk EDU 版的关键差异: + - 总长 616B (EDU 614B) + - CRC 偏移 612..616 (EDU 在 610..614) + - CRC 前有 2 字节 0x0000 固定填充 (位置 610..612) + - CRC 算法用裸 gen_crc, 不 XOR (EDU 用 encryptCrc XOR 0xedcab9de) + - bandWidth 字节序是 BE (EDU 用 LE) + - SN/version 全 0 (EDU 填实际值) + +发送前必须 Blowfish 加密 (MCU 期待加密数据). + +布局表 (616 字节明文): + offset bytes 字段 + 0..2 2 head 0xfeef + 2..3 1 levelFlag 0xff + 3..4 1 frameReserve + 4..12 8 SN (全 0) + 12..20 8 version (全 0) + 20..22 2 bandWidth 0x3ac0 BE + 22..562 540 motorCmd[20] × 27B + 562..566 4 bms (全 0) + 566..606 40 wirelessRemote (全 0) + 606..610 4 reserve (全 0) + 610..612 2 固定填充 0x0000 + 612..616 4 CRC = gen_crc(cmd[:612]) +""" +import struct +from typing import Optional +from ..types.low_cmd import LowCmd +from ..types.motor import MotorCmd +from ..utils.common import gen_crc +from .blowfish import Blowfish + + +def build_low_cmd_plain(lowcmd: LowCmd) -> bytes: + """构造 PRO 格式 616B 明文 LowCmd.""" + cmd = bytearray(616) + + # 0..2 head + cmd[0:2] = lowcmd.head + # 2..3 levelFlag + cmd[2] = lowcmd.levelFlag + # 3..4 frameReserve + cmd[3] = lowcmd.frameReserve + # 4..12 SN + cmd[4:12] = lowcmd.SN + # 12..20 version + cmd[12:20] = lowcmd.version + # 20..22 bandWidth BE + cmd[20:22] = struct.pack('>H', lowcmd.bandWidth) + # 22..562 motorCmd[20] × 27B + motor_bytes = b''.join(m.to_bytes() for m in lowcmd.motorCmd) + assert len(motor_bytes) == 540, f'motorCmd 序列化长度错: {len(motor_bytes)}' + cmd[22:562] = motor_bytes + # 562..566 bms (默认 0) + # 566..606 wirelessRemote + cmd[566:606] = lowcmd.wirelessRemote + # 606..610 reserve + cmd[606:610] = lowcmd.reserve + # 610..612 固定填充 0x0000 + # 612..616 CRC + cmd[612:616] = gen_crc(bytes(cmd[:612])) + + return bytes(cmd) + + +def build_low_cmd_encrypted(lowcmd: LowCmd, bf: Blowfish) -> bytes: + """构造并加密 LowCmd (PRO 真实发包格式). + + Returns: + 616 字节 Blowfish ECB 密文, 直接 sendto MCU :8007. + """ + plain = build_low_cmd_plain(lowcmd) + return bf.encrypt_ecb(plain) + + +def make_damping_cmd() -> LowCmd: + """快捷: 全 damping 的 LowCmd (常用于初始化/紧急停).""" + cmd = LowCmd() + cmd.all_damping() + return cmd diff --git a/go1_pro_sdk/codec/lowstate_parser.py b/go1_pro_sdk/codec/lowstate_parser.py new file mode 100644 index 0000000..67cd6cf --- /dev/null +++ b/go1_pro_sdk/codec/lowstate_parser.py @@ -0,0 +1,69 @@ +"""LowState 解析 (Blowfish 解密后的 807B 明文 → 结构化 LowState). + +LowState 布局 (807B, 跟 free-dog-sdk ucl/lowState.py 一致, 实测可用): + 0..2 head (uint16 LE) + 2..3 levelFlag + 3..4 frameReserve + 4..12 SN + 12..20 version + 20..22 bandWidth + 22..75 IMU (53B) + 75..715 motorState[20] × 32B = 640B + 715..739 BMS (24B) + 739..755 footForce + footForceEst (混合区间, 参考原版) + 755..759 mode + 759..799 wirelessRemote (40B) ← 遥控器数据 + 799..803 reserve + 803..807 crc +""" +from ..types.low_state import LowState +from ..types.motor import MotorState +from ..types.imu import IMU +from ..types.bms import BMS + + +def parse_low_state(decrypted: bytes) -> LowState: + """解析 Blowfish 解密后的 LowState (至少 807B).""" + assert len(decrypted) >= 807, f'LowState 至少 807 字节, got {len(decrypted)}' + data = decrypted + + ls = LowState() + ls.head = int.from_bytes(data[0:2], 'little') + ls.levelFlag = data[2] + ls.frameReserve = data[3] + ls.SN = bytes(data[4:12]) + ls.version = bytes(data[12:20]) + ls.bandWidth = int.from_bytes(data[20:22], 'little') + + # IMU (22..75) + ls.imu = IMU.from_bytes(data[22:75]) + + # motorState[20] × 32B (75..715) + ls.motorState = [ + MotorState.from_bytes(data[75 + i*32 : 75 + (i+1)*32]) + for i in range(20) + ] + + # BMS (715..739) + ls.bms = BMS.from_bytes(data[715:739]) + + # 足端力 / 估计力 (位置参考 free-dog-sdk 原版) + ls.footForce = ( + int.from_bytes(data[739:741], 'little'), + int.from_bytes(data[751:753], 'little'), + int.from_bytes(data[753:755], 'little'), + int.from_bytes(data[755:757], 'little'), + ) + ls.footForceEst = ( + int.from_bytes(data[747:749], 'little'), + int.from_bytes(data[759:761], 'little'), + int.from_bytes(data[761:763], 'little'), + int.from_bytes(data[763:765], 'little'), + ) + + # wirelessRemote (759..799, 40B) + ls.wirelessRemote = bytes(data[759:799]) + ls.reserve = bytes(data[799:803]) + ls.crc = bytes(data[803:807]) + + return ls diff --git a/go1_pro_sdk/connection/__init__.py b/go1_pro_sdk/connection/__init__.py new file mode 100644 index 0000000..11499c6 --- /dev/null +++ b/go1_pro_sdk/connection/__init__.py @@ -0,0 +1,4 @@ +"""高层 UDP 客户端 (隐藏 socket/Blowfish 细节).""" +from .mcu_client import MCUClient + +__all__ = ['MCUClient'] diff --git a/go1_pro_sdk/connection/mcu_client.py b/go1_pro_sdk/connection/mcu_client.py new file mode 100644 index 0000000..1b70bd6 --- /dev/null +++ b/go1_pro_sdk/connection/mcu_client.py @@ -0,0 +1,145 @@ +"""高层 MCU 客户端: 一个对象封装 socket + Blowfish + LowCmd 序列化 + LowState 解析.""" +import os +import socket +import time +from typing import Optional +from ..codec.blowfish import Blowfish +from ..codec.lowcmd_builder import build_low_cmd_encrypted +from ..codec.lowstate_parser import parse_low_state +from ..types.low_cmd import LowCmd +from ..types.low_state import LowState +from ..utils.constants import MCU_IP, MCU_PORT, RCVBUF_SIZE + + +def _default_state_path() -> str: + """查找 _data/blowfish_state.bin 的默认路径.""" + here = os.path.dirname(os.path.abspath(__file__)) + return os.path.join(here, '..', '_data', 'blowfish_state.bin') + + +class MCUClient: + """跟 Go1 PRO MCU 的高层 UDP 客户端. + + 用法: + with MCUClient() as client: + 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) + """ + + def __init__(self, + state_path: Optional[str] = None, + mcu_ip: str = MCU_IP, + mcu_port: int = MCU_PORT, + local_port: int = 0, # 0 = OS 分配 + endian: str = 'little'): + self.mcu_ip = mcu_ip + self.mcu_port = mcu_port + self.bf = Blowfish.from_state_file(state_path or _default_state_path(), endian=endian) + + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.setblocking(False) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, RCVBUF_SIZE) + sock.bind(('', local_port)) + self.sock = sock + self.local_port = sock.getsockname()[1] + + self._last_state: Optional[LowState] = None + + def __enter__(self): + return self + + def __exit__(self, *args): + self.close() + + def close(self): + if self.sock: + self.sock.close() + self.sock = None + + # ===== 发送 ===== + + def send(self, cmd: LowCmd) -> int: + """加密并发送 LowCmd. 返回发送字节数 (固定 616).""" + cipher = build_low_cmd_encrypted(cmd, self.bf) + self.sock.sendto(cipher, (self.mcu_ip, self.mcu_port)) + return len(cipher) + + def send_raw(self, raw_cipher: bytes) -> int: + """发送已加密的字节 (高级用途, 自己构造 cipher).""" + self.sock.sendto(raw_cipher, (self.mcu_ip, self.mcu_port)) + return len(raw_cipher) + + # ===== 接收 ===== + + def recv_latest(self) -> Optional[LowState]: + """非阻塞收所有积压的包, 只解析最新一帧. + + Returns: + LowState 或 None (没有新包). + """ + last_data = None + while True: + try: + data, _ = self.sock.recvfrom(2048) + last_data = data + except (BlockingIOError, socket.timeout): + break + + if last_data is None: + return None + + aligned = (len(last_data) // 8) * 8 + decrypted = self.bf.decrypt_ecb(last_data[:aligned]) + if decrypted[:4] != bytes.fromhex('feefff00') or len(decrypted) < 807: + return None + + self._last_state = parse_low_state(decrypted) + return self._last_state + + def recv_state(self, timeout: float = 1.0) -> Optional[LowState]: + """阻塞等待最多 timeout 秒, 直到收到至少一帧 LowState. + + Returns: + LowState 或 None (超时). + """ + deadline = time.time() + timeout + while time.time() < deadline: + state = self.recv_latest() + if state is not None: + return state + time.sleep(0.001) + return None + + @property + def last_state(self) -> Optional[LowState]: + """最近一次 recv_latest/recv_state 拿到的状态.""" + return self._last_state + + # ===== 便捷工具 ===== + + def wake_mcu(self, n_frames: int = 50, dt: float = 0.01) -> int: + """发 N 帧 damping 唤醒 MCU 并切到我们这个客户端. + + Returns: + 实际收到的有效 LowState 帧数. + """ + damping = LowCmd().all_damping() + recv_count = 0 + for _ in range(n_frames): + self.send(damping) + time.sleep(dt) + if self.recv_latest() is not None: + recv_count += 1 + return recv_count + + def safe_stop(self, n_frames: int = 50, dt: float = 0.002): + """发送 N 帧 damping 退出 (紧急停机/退出前调用).""" + damping = LowCmd().all_damping() + for _ in range(n_frames): + try: + self.send(damping) + except Exception: + pass + time.sleep(dt) diff --git a/go1_pro_sdk/safety/__init__.py b/go1_pro_sdk/safety/__init__.py new file mode 100644 index 0000000..f30f1ce --- /dev/null +++ b/go1_pro_sdk/safety/__init__.py @@ -0,0 +1,16 @@ +"""安全保护层 (复刻 Unitree C SDK Safety 类).""" +from .safety import ( + apply_safety, + position_limit, + power_protect, + position_protect, + PowerProtectViolation, +) + +__all__ = [ + 'apply_safety', + 'position_limit', + 'power_protect', + 'position_protect', + 'PowerProtectViolation', +] diff --git a/go1_pro_sdk/safety/safety.py b/go1_pro_sdk/safety/safety.py new file mode 100644 index 0000000..a340295 --- /dev/null +++ b/go1_pro_sdk/safety/safety.py @@ -0,0 +1,115 @@ +"""Safety 层: PositionLimit / PowerProtect / PositionProtect. + +跟 Unitree C SDK `safety.h` 三个保护接口语义对齐: + - PositionLimit: 关节角硬限位 + - PowerProtect: factor 1-10 限制力矩 ((factor/10) * TAU_MAX), 严重超限 raise + - PositionProtect: 目标位置偏离实测过大就把 Kp/Kd 归零 +""" +from typing import Optional, TYPE_CHECKING +from ..utils.constants import ( + JOINT_LIMITS, JOINT_NAMES, JOINT_TYPE, TAU_MAX, CRITICAL_FACTOR, +) + +if TYPE_CHECKING: + from ..types import LowCmd, LowState + + +class PowerProtectViolation(Exception): + """严重过载, 应该立即停机 (类比 Unitree C SDK PowerProtect 返回负数 + exit -1).""" + pass + + +def position_limit(lowcmd: 'LowCmd') -> int: + """关节硬限位 (in-place clamp). 返回被 clamp 的数量.""" + clamped = 0 + for i in range(12): + jt = JOINT_TYPE[i] + lo, hi = JOINT_LIMITS[jt] + m = lowcmd.motorCmd[i] + if m.q < lo: + m.q = lo; clamped += 1 + elif m.q > hi: + m.q = hi; clamped += 1 + return clamped + + +def power_protect(lowcmd: 'LowCmd', lstate: 'LowState', factor: int) -> int: + """力矩限制. + + Args: + factor: 1-10, 越大越宽松. 实际 tau 限 = TAU_MAX * factor / 10 + Returns: + clamp 的数量 + Raises: + PowerProtectViolation: 命令离谱 (> CRITICAL_FACTOR × max) 或电机实测已过载 + """ + assert 1 <= factor <= 10, f'factor 必须 1-10, got {factor}' + clamped = 0 + for i in range(12): + jt = JOINT_TYPE[i] + m = lowcmd.motorCmd[i] + tau_lim = TAU_MAX[jt] * factor / 10.0 + critical = TAU_MAX[jt] * CRITICAL_FACTOR + + if abs(m.tau) > critical: + raise PowerProtectViolation( + f'严重超限! 电机 {JOINT_NAMES[i]} 命令 tau={m.tau:.2f}, ' + f'超过 critical {critical:.2f} ({CRITICAL_FACTOR}x max)') + + if lstate is not None: + try: + actual_tau = lstate.motorState[i].tauEst + if abs(actual_tau) > TAU_MAX[jt]: + raise PowerProtectViolation( + f'电机 {JOINT_NAMES[i]} 实测 tauEst={actual_tau:.2f} 已超 max {TAU_MAX[jt]}, ' + f'立即停机!') + except (AttributeError, IndexError): + pass + + if m.tau > tau_lim: + m.tau = tau_lim; clamped += 1 + elif m.tau < -tau_lim: + m.tau = -tau_lim; clamped += 1 + return clamped + + +def position_protect(lowcmd: 'LowCmd', lstate: 'LowState', limit_rad: float) -> int: + """软位置保护: 目标 q 跟实测偏差 > limit, 关 Kp/Kd 防止拉伤. 返回被关的电机数.""" + affected = 0 + for i in range(12): + m = lowcmd.motorCmd[i] + try: + actual_q = lstate.motorState[i].q + except (AttributeError, IndexError): + continue + if abs(m.q - actual_q) > limit_rad: + m.Kp = 0; m.Kd = 0 + affected += 1 + return affected + + +def apply_safety(lowcmd: 'LowCmd', lstate: Optional['LowState'] = None, *, + power_factor: Optional[int] = 1, + position_limit_on: bool = True, + position_protect_limit: Optional[float] = None, + raise_on_critical: bool = True) -> 'LowCmd': + """统一入口: 一次调用应用所有保护. In-place. + + 推荐用法 (开发阶段): + apply_safety(cmd, state, power_factor=1, position_protect_limit=0.5) + """ + if position_limit_on: + position_limit(lowcmd) + if power_factor is not None and lstate is not None: + try: + power_protect(lowcmd, lstate, power_factor) + except PowerProtectViolation: + if raise_on_critical: + raise + # 降级: 全部 damping + for i in range(12): + m = lowcmd.motorCmd[i] + m.tau = 0; m.Kp = 0; m.Kd = 0; m.mode = 0 + if position_protect_limit is not None and lstate is not None: + position_protect(lowcmd, lstate, position_protect_limit) + return lowcmd diff --git a/go1_pro_sdk/types/__init__.py b/go1_pro_sdk/types/__init__.py new file mode 100644 index 0000000..9dab98c --- /dev/null +++ b/go1_pro_sdk/types/__init__.py @@ -0,0 +1,14 @@ +"""数据结构 (MotorCmd, MotorState, LowState, LowCmd, IMU, BMS, Remote).""" +from .motor import MotorCmd, MotorState, MotorMode +from .imu import IMU +from .bms import BMS +from .remote import RemoteState, parse_remote, BUTTON_NAMES +from .low_state import LowState +from .low_cmd import LowCmd + +__all__ = [ + 'MotorCmd', 'MotorState', 'MotorMode', + 'IMU', 'BMS', + 'RemoteState', 'parse_remote', 'BUTTON_NAMES', + 'LowState', 'LowCmd', +] diff --git a/go1_pro_sdk/types/bms.py b/go1_pro_sdk/types/bms.py new file mode 100644 index 0000000..82dd0cc --- /dev/null +++ b/go1_pro_sdk/types/bms.py @@ -0,0 +1,48 @@ +"""电池管理系统 (BMS) 数据.""" +import struct +from dataclasses import dataclass, field +from typing import List + + +@dataclass +class BMS: + """BMS 反馈.""" + version_h: int = 0 + version_l: int = 0 + bms_status: int = 0 + SOC: int = 0 # 电量百分比 0-100 + current: int = 0 # mA (负值=放电) + cycle: int = 0 # 充放电循环数 + BQ_NTC: List[int] = field(default_factory=lambda: [0, 0]) # 电池温度 °C + MCU_NTC: List[int] = field(default_factory=lambda: [0, 0]) # 控制板温度 °C + cell_vol: List[int] = field(default_factory=lambda: [0]*10) # 单串电压 mV (10 串) + + @property + def voltage_mv(self) -> int: + """整组电压 (mV) = 10 串单电压之和.""" + return sum(self.cell_vol) + + @property + def voltage_v(self) -> float: + return self.voltage_mv / 1000.0 + + @property + def current_a(self) -> float: + return self.current / 1000.0 + + @classmethod + def from_bytes(cls, data: bytes) -> 'BMS': + """从 LowState 中的 34 字节解析 (offset 835..869 in highState, 类似在 lowState).""" + bms = cls() + bms.version_h = data[0] + bms.version_l = data[1] + bms.bms_status = data[2] + bms.SOC = data[3] + # current int32 LE (signed) + bms.current = struct.unpack(' 'IMU': + """从 LowState 中的 53 字节解析 (offset 22..75).""" + imu = cls() + # quaternion 4×4 = 16B + imu.quaternion = tuple(hex_to_float(data[i*4:(i+1)*4]) for i in range(4)) + # gyroscope 3×4 = 12B + imu.gyroscope = tuple(hex_to_float(data[16 + i*4:16 + (i+1)*4]) for i in range(3)) + # accelerometer 3×4 = 12B + imu.accelerometer = tuple(hex_to_float(data[28 + i*4:28 + (i+1)*4]) for i in range(3)) + # rpy 3×4 = 12B + imu.rpy = tuple(hex_to_float(data[40 + i*4:40 + (i+1)*4]) for i in range(3)) + # temperature 1B + imu.temperature = data[52] + return imu diff --git a/go1_pro_sdk/types/low_cmd.py b/go1_pro_sdk/types/low_cmd.py new file mode 100644 index 0000000..4bcf19c --- /dev/null +++ b/go1_pro_sdk/types/low_cmd.py @@ -0,0 +1,40 @@ +"""LowCmd — 主控发给 MCU 的控制命令 (PRO 格式 616B 明文 / Blowfish 加密后 616B).""" +from dataclasses import dataclass, field +from typing import List +from .motor import MotorCmd + + +@dataclass +class LowCmd: + """完整低层控制命令. + + PRO 实际只在意 motorCmd[0..11] 和 bandWidth, 其他字段都是 0. + """ + head: bytes = b'\xfe\xef' + levelFlag: int = 0xff + frameReserve: int = 0 + SN: bytes = b'\x00' * 8 # PRO 不填, 全 0 + version: bytes = b'\x00' * 8 # PRO 不填, 全 0 + bandWidth: int = 0x3ac0 # 真包实测值 + motorCmd: List[MotorCmd] = field(default_factory=lambda: [MotorCmd() for _ in range(20)]) + wirelessRemote: bytes = b'\x00' * 40 + reserve: bytes = b'\x00' * 4 + + def set_motor(self, index_or_name, cmd: MotorCmd): + """设置某个电机命令. + + index_or_name: 0-19 (数字) 或 'FR_0', 'FL_1' 等字符串. + """ + from ..utils.constants import JOINT_NAMES + if isinstance(index_or_name, str): + idx = JOINT_NAMES.index(index_or_name) + else: + idx = int(index_or_name) + self.motorCmd[idx] = cmd + return self + + def all_damping(self): + """快捷: 把所有电机设为 damping (mode=0, 全零).""" + for i in range(20): + self.motorCmd[i] = MotorCmd() # 默认全 0 + Damping + return self diff --git a/go1_pro_sdk/types/low_state.py b/go1_pro_sdk/types/low_state.py new file mode 100644 index 0000000..6ba447d --- /dev/null +++ b/go1_pro_sdk/types/low_state.py @@ -0,0 +1,33 @@ +"""LowState — MCU 解密后的状态包 (807B).""" +import struct +from dataclasses import dataclass, field +from typing import List, Tuple +from .motor import MotorState +from .imu import IMU +from .bms import BMS +from .remote import RemoteState, parse_remote + + +@dataclass +class LowState: + """完整 MCU 状态. 由 lowstate_parser.parse_low_state() 填充.""" + head: int = 0 + levelFlag: int = 0 + frameReserve: int = 0 + SN: bytes = b'\x00' * 8 + version: bytes = b'\x00' * 8 + bandWidth: int = 0 + imu: IMU = field(default_factory=IMU) + motorState: List[MotorState] = field(default_factory=lambda: [MotorState() for _ in range(20)]) + footForce: Tuple[int, int, int, int] = (0, 0, 0, 0) + footForceEst: Tuple[int, int, int, int] = (0, 0, 0, 0) + bms: BMS = field(default_factory=BMS) + tick: int = 0 + wirelessRemote: bytes = b'\x00' * 40 + reserve: bytes = b'\x00' * 4 + crc: bytes = b'\x00' * 4 + + @property + def remote(self) -> RemoteState: + """解析后的遥控器状态 (每次访问重新解析, 始终最新).""" + return parse_remote(self.wirelessRemote) diff --git a/go1_pro_sdk/types/motor.py b/go1_pro_sdk/types/motor.py new file mode 100644 index 0000000..4c28398 --- /dev/null +++ b/go1_pro_sdk/types/motor.py @@ -0,0 +1,83 @@ +"""电机数据结构: MotorCmd, MotorState, MotorMode.""" +from dataclasses import dataclass, field +from enum import IntEnum +from typing import List +from ..utils.common import ( + float_to_hex, hex_to_float, tau_to_hex, hex_to_tau, + kp_to_hex, hex_to_kp, kd_to_hex, hex_to_kd, +) + + +class MotorMode(IntEnum): + """电机模式.""" + Damping = 0x00 # 阻尼模式 (电机失能, 关节自由) + Servo = 0x0A # 伺服模式 (PD 控制) + Overheat = 0x08 # 过热保护 + + +@dataclass +class MotorCmd: + """单电机命令. + + 实际控制律: tau_output = tau + Kp*(q-q_actual) + Kd*(dq-dq_actual) + """ + mode: int = MotorMode.Damping + q: float = 0.0 # 目标角度 (rad) + dq: float = 0.0 # 目标速度 (rad/s) + tau: float = 0.0 # 前馈力矩 (N·m) + Kp: float = 0.0 # 位置 stiffness + Kd: float = 0.0 # 速度 damping + reserve: List[int] = field(default_factory=lambda: [0, 0, 0]) + + def to_bytes(self) -> bytes: + """序列化为 27 字节 (Unitree 私有格式).""" + mode = int(self.mode) + return ( + mode.to_bytes(1, 'little') + + float_to_hex(self.q) # 4B + + float_to_hex(self.dq) # 4B + + tau_to_hex(self.tau) # 2B + + bytes(kp_to_hex(self.Kp)) # 2B + + bytes(kd_to_hex(self.Kd)) # 2B + + self.reserve[0].to_bytes(4, 'little') + + self.reserve[1].to_bytes(4, 'little') + + self.reserve[2].to_bytes(4, 'little') + ) + + +@dataclass +class MotorState: + """单电机反馈.""" + mode: int = 0 + q: float = 0.0 + dq: float = 0.0 + ddq: float = 0.0 + tauEst: float = 0.0 # 估算扭矩 (N·m) + q_raw: float = 0.0 + dq_raw: float = 0.0 + ddq_raw: float = 0.0 + temperature: int = 0 + reserve: List[int] = field(default_factory=lambda: [0, 0]) + + @classmethod + def from_bytes(cls, data: bytes) -> 'MotorState': + """从 32 字节反解 (Unitree LowState 单电机布局, 跟 free-dog-sdk 对齐).""" + ms = cls() + ms.mode = data[0] + ms.q = hex_to_float(data[1:5]) + ms.dq = hex_to_float(data[5:9]) + ms.ddq = float(int.from_bytes(data[9:11], 'little', signed=True)) + ms.tauEst = float(int.from_bytes(data[11:13], 'little', signed=True)) * 0.00390625 + ms.q_raw = hex_to_float(data[13:17]) + ms.dq_raw = hex_to_float(data[17:21]) + ms.ddq_raw = float(int.from_bytes(data[21:23], 'little', signed=True)) + ms.temperature = data[24] + ms.reserve = [ + int.from_bytes(data[24:28], 'little'), + int.from_bytes(data[28:32], 'little'), + ] + return ms + + def estimated_current(self, kt: float = 0.066) -> float: + """估算电机电流 (A) = tauEst / Kt.""" + return self.tauEst / kt diff --git a/go1_pro_sdk/types/remote.py b/go1_pro_sdk/types/remote.py new file mode 100644 index 0000000..47d6a87 --- /dev/null +++ b/go1_pro_sdk/types/remote.py @@ -0,0 +1,54 @@ +"""Unitree 遥控器状态解码.""" +import struct +from dataclasses import dataclass, field +from typing import List + + +# 按键 bitmap (Unitree 公开 xRockerBtnDataStruct) +BUTTON_NAMES = [ + (0x0001, 'R1'), (0x0002, 'L1'), (0x0004, 'START'), (0x0008, 'SELECT'), + (0x0010, 'R2'), (0x0020, 'L2'), (0x0040, 'F1'), (0x0080, 'F2'), + (0x0100, 'A'), (0x0200, 'B'), (0x0400, 'X'), (0x0800, 'Y'), + (0x1000, 'UP'), (0x2000, 'RIGHT'), (0x4000, 'DOWN'), (0x8000, 'LEFT'), +] + + +@dataclass +class RemoteState: + """遥控器实时状态. + + head: 通常 [0x55, 0xAA] 包头 + btn: 按键 bitmap (uint16) + lx/ly/rx/ry: 摇杆 -1..+1 + L2: 模拟扳机 0..1 + """ + head: bytes = b'' + btn: int = 0 + lx: float = 0.0 + ly: float = 0.0 + rx: float = 0.0 + ry: float = 0.0 + L2: float = 0.0 + pressed: List[str] = field(default_factory=list) + + def is_pressed(self, name: str) -> bool: + return name in self.pressed + + def any_button(self) -> bool: + return self.btn != 0 + + +def parse_remote(data40: bytes) -> RemoteState: + """从 40 字节 wirelessRemote 字段解析.""" + rs = RemoteState() + if len(data40) < 24: + return rs + rs.head = bytes(data40[:2]) + rs.btn = struct.unpack(' bytes: + """CRC-32 with polynomial 0x04c11db7 (custom 实现, 跟 Unitree MCU 对齐). + + 跟 Python zlib.crc32 不同 (zlib 用反射 + 0xEDB88320), 所以这个手动实现保留. + """ + crc = 0xFFFFFFFF + for j in struct.unpack("<%dI" % (len(data) // 4), data): + for b in range(32): + x = (crc >> 31) & 1 + crc <<= 1 + crc &= 0xFFFFFFFF + if x ^ (1 & (j >> (31 - b))): + crc ^= 0x04c11db7 + return struct.pack(' bytes: + return (struct.unpack('>I', struct.pack('>f', f))[0]).to_bytes(4, 'little') + + +def hex_to_float(data: bytes) -> float: + i = int.from_bytes(data, 'little') + return struct.unpack('>f', struct.pack('>I', i))[0] + + +# ===== 力矩 tau ↔ 2 字节 ===== + +def _fraction_to_hex(fraction, neg=False): + if fraction == 0.0: + neg = False + hex_value = int(fraction * 256) + if neg: + hex_value = 255 + hex_value + 1 + return hex_value.to_bytes(1, 'little') + + +def _hex_to_fraction(hex_byte, neg=False): + if neg: + return -1 + round(hex_byte / 256, 2) + return round(hex_byte / 256, 2) + + +def tau_to_hex(tau: float) -> bytes: + tau = round(tau, 2) + integer_part = int(tau) + fractional_part = tau - integer_part + neg = False + if tau < 0: + neg = True + integer_part = 255 + integer_part + return _fraction_to_hex(fractional_part, neg) + integer_part.to_bytes(1, 'little') + + +def hex_to_tau(data: bytes) -> float: + ip = data[1:] + int_val = int.from_bytes(ip, 'little') + neg = False + if int_val > 126: + neg = True + int_val = -255 + int_val + return int_val + _hex_to_fraction(data[0], neg) + + +# ===== Kp ↔ 2 字节 ===== + +def kp_to_hex(Kp: float) -> bytearray: + base, frac = divmod(Kp, 1) + base = int(base) + frac = int(round(frac, 1) * 10) + val = 0 + if frac < 5: + val = (base * 32) + frac * 3 + if frac >= 5: + val = (base * 32) + ((frac - 1) * 3) + 4 + val = f'%04x' % val + kp = bytearray(bytes.fromhex(val)) + kp.reverse() + return kp + + +def hex_to_kp(byte_arr: bytes) -> float: + hex_bytes = binascii.hexlify(byte_arr) + h = bytearray(4) + h[0] = hex_bytes[2] + h[1] = hex_bytes[3] + h[2] = hex_bytes[0] + h[3] = hex_bytes[1] + val = int(h, 16) + base = val // 32 + remainder = val % 32 + if remainder < 15: + frac = remainder / 3 + else: + frac = (remainder - 4) / 3 + 1 + return base + round(frac, 1) / 10 + + +# ===== Kd ↔ 2 字节 ===== + +_KD_FRAC_TO_HEX = {0.0: '0', 0.1: '1', 0.2: '3', 0.3: '4', 0.4: '6', + 0.5: '8', 0.6: '9', 0.7: 'b', 0.8: 'c', 0.9: 'e'} +_KD_HEX_TO_FRAC = {v: k for k, v in _KD_FRAC_TO_HEX.items()} + + +def kd_to_hex(Kd: float) -> bytearray: + integer_part = int(Kd) + fractional_part = round(Kd - integer_part, 1) + hex_fractional_part = _KD_FRAC_TO_HEX.get(fractional_part, '0') + hex_integer_part = f'%03x' % integer_part + kd = bytearray(bytes.fromhex(hex_integer_part + hex_fractional_part)) + kd.reverse() + return kd + + +def hex_to_kd(byte_arr: bytes) -> float: + hex_bytes = binascii.hexlify(byte_arr) + h = bytearray(4) + h[0] = hex_bytes[2] + h[1] = hex_bytes[3] + h[2] = hex_bytes[0] + h[3] = hex_bytes[1] + int_part = int(h[:3], 16) + frac_part = _KD_HEX_TO_FRAC.get(chr(h[3]), 0.0) + return int_part + frac_part + + +# ===== SN / Version 解码 ===== + +_TYPE_NAMES = {1: 'Laikago', 2: 'Aliengo', 3: 'A1', 4: 'Go1', 5: 'B1'} +_MODEL_NAMES = {1: 'AIR', 2: 'PRO', 3: 'EDU', 4: 'PC', 5: 'XX'} + + +def decode_sn(data: bytes): + """SN 8 字节 → (product, id) 例如 ('Go1_PRO', '3-5-8[2]').""" + type_name = _TYPE_NAMES.get(data[0], 'UNKNOWN') + model_name = _MODEL_NAMES.get(data[1], 'UNKNOWN') + product = f'{type_name}_{model_name}' + id_str = f'{data[2]}-{data[3]}-{data[4]}[{data[5]}]' + return product, id_str + + +def decode_version(data: bytes): + """Version 8 字节 → (hw, sw) 例如 ('0.1.9', '0.2.0').""" + hw = f'{data[0]}.{data[1]}.{data[2]}' + sw = f'{data[3]}.{data[4]}.{data[5]}' + return hw, sw + + +def byte_print(data: bytes) -> str: + """格式化字节为 hex 字符串.""" + return ''.join('{:02x}'.format(x) for x in data) diff --git a/go1_pro_sdk/utils/constants.py b/go1_pro_sdk/utils/constants.py new file mode 100644 index 0000000..be6adaf --- /dev/null +++ b/go1_pro_sdk/utils/constants.py @@ -0,0 +1,90 @@ +"""Go1 PRO 常量集中定义. + +来源: tools/onboard/* 实测 + Unitree URDF + 官方电机规格. +""" + +# ===== 网络 ===== +MCU_IP = '192.168.123.10' +MCU_PORT = 8007 +"""MCU UDP 端口 (从这里发/收 LowCmd/LowState)""" + + +# ===== 关节命名/索引 ===== + +JOINT_NAMES = [ + 'FR_0', 'FR_1', 'FR_2', # 0..2 右前 (髋/大腿/小腿) + 'FL_0', 'FL_1', 'FL_2', # 3..5 左前 + 'RR_0', 'RR_1', 'RR_2', # 6..8 右后 + 'RL_0', 'RL_1', 'RL_2', # 9..11 左后 +] + +JOINT_TYPE = {i: ('hip' if i % 3 == 0 else 'thigh' if i % 3 == 1 else 'knee') + for i in range(12)} + +# motorCmdArray 用命名属性 (FR_0, FR_1, ...) 而不是 list, 这个表用于 getattr 索引 +JOINT_ATTR = JOINT_NAMES # 命名一致, 可直接 getattr + + +# ===== 关节限位 (rad) — 2026-06-20 实测校准 ===== + +JOINT_LIMITS = { + # 推荐安全范围 (实测极限 + 5° 余量) + 'hip': (-0.78, 0.78), # _0 (实测 ~±0.85~0.89) + 'thigh': (-0.60, 3.50), # _1 (实测 -0.69 ~ +3.95, 上限保守取 3.5) + 'knee': (-2.70, -0.95), # _2 (实测 -2.79 ~ -0.88, 注意是负值) +} + +JOINT_LIMITS_URDF = { + # Unitree 官方 URDF 标称值, 实际可能偏保守 + 'hip': (-0.863, 0.863), + 'thigh': (-0.686, 4.501), + 'knee': (-2.818, -0.888), +} + +JOINT_LIMITS_MEASURED = { + # 前两条腿 (FR/FL) 完整转动 90s 实测 (2026-06-20) + 'FR_0': (-0.8627, +0.8515), + 'FR_1': (-0.6930, +3.9173), + 'FR_2': (-2.7648, -0.8847), + 'FL_0': (-0.8922, +0.8229), + 'FL_1': (-0.6676, +3.9508), + 'FL_2': (-2.7850, -0.9001), + # 后腿 (RR/RL) 实测时未充分转动, 参考 FR/FL 对称 +} + + +# ===== 电机最大力矩 (N·m) — Unitree 官方规格 ===== + +TAU_MAX = { + 'hip': 23.7, + 'thigh': 23.7, + 'knee': 35.55, +} + +KT = 0.066 # 力矩常数 N·m/A, 用于 tau ↔ 电流 换算 (Go1 GO-M8010-6) + +CRITICAL_FACTOR = 5.0 # 命令 tau > CRITICAL_FACTOR × TAU_MAX = 离谱, 触发紧急停机 + + +# ===== 趴地 damping 姿态参考 (2026-06-20 实测, 4 腿对称) ===== + +DAMPING_POSE = { + 'FR_0': -0.37, 'FR_1': +1.18, 'FR_2': -2.77, + 'FL_0': +0.43, 'FL_1': +1.14, 'FL_2': -2.75, + 'RR_0': -0.37, 'RR_1': +1.18, 'RR_2': -2.76, + 'RL_0': +0.41, 'RL_1': +1.15, 'RL_2': -2.76, +} + +# 半起立姿态 (example_position 用的 sin 中点, 不是真趴地) +HALF_STAND_POSE = { + '_0': 0.0, # hip 中位 + '_1': 1.2, # thigh 前蹬 + '_2': -2.0, # knee 半弯曲 (比 damping 的 -2.77 伸开 44°) +} + + +# ===== 控制相关 ===== + +DT_DEFAULT = 0.002 # 控制周期 (s), Mac 上可达 480Hz +SEND_DECIMATION = 1 # 发包降频比例 (1 = 全速发, 5 = 100Hz) +RCVBUF_SIZE = 4096 # socket RCVBUF, 小=自动丢弃积压 diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..b6403fe --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,40 @@ +[build-system] +requires = ["setuptools>=61.0", "wheel"] +build-backend = "setuptools.build_meta" + +[project] +name = "go1_pro_sdk" +version = "0.1.0" +description = "Unitree Go1 PRO low-level motor control SDK (Python, with Blowfish encryption)" +readme = "README.md" +license = {text = "MIT"} +requires-python = ">=3.8" +authors = [{name = "chenyouyuan"}] +keywords = ["unitree", "go1", "robotics", "low-level-control"] +classifiers = [ + "Development Status :: 4 - Beta", + "License :: OSI Approved :: MIT License", + "Programming Language :: Python :: 3", + "Programming Language :: Python :: 3 :: Only", + "Programming Language :: Python :: 3.8", + "Topic :: Scientific/Engineering :: Robotics", +] +dependencies = [] # 纯标准库 + +[project.optional-dependencies] +mqtt = ["paho-mqtt>=2.0.0"] # 用 MQTT 切换 sportMode 模式时需要 + +[project.urls] +Homepage = "https://github.com/your-username/go1_pro_sdk" +Documentation = "https://github.com/your-username/go1_pro_sdk/blob/main/docs/" + +[tool.setuptools] +packages = ["go1_pro_sdk", + "go1_pro_sdk.codec", + "go1_pro_sdk.connection", + "go1_pro_sdk.safety", + "go1_pro_sdk.types", + "go1_pro_sdk.utils"] + +[tool.setuptools.package-data] +"go1_pro_sdk" = ["_data/*.bin", "_data/*.md"] diff --git a/tests/test_blowfish.py b/tests/test_blowfish.py new file mode 100644 index 0000000..d894e54 --- /dev/null +++ b/tests/test_blowfish.py @@ -0,0 +1,48 @@ +"""Blowfish 加解密测试 (用已知明密文对).""" +import os +import pytest +from go1_pro_sdk import Blowfish, verify_state +from go1_pro_sdk.codec.blowfish import KNOWN_PAIRS + + +STATE_FILE = os.path.join( + os.path.dirname(__file__), '..', 'go1_pro_sdk', '_data', 'blowfish_state.bin' +) + + +@pytest.fixture +def bf(): + return Blowfish.from_state_file(STATE_FILE) + + +def test_state_loads(bf): + assert len(bf.P) == 18 + assert len(bf.S) == 4 + assert all(len(s) == 256 for s in bf.S) + + +def test_state_verify(bf): + """3/3 已知明密文对应该全部匹配.""" + assert verify_state(bf) is True + + +def test_encrypt_known_pairs(bf): + for pt, ct in KNOWN_PAIRS: + assert bf.encrypt_block(pt) == ct + + +def test_decrypt_known_pairs(bf): + for pt, ct in KNOWN_PAIRS: + assert bf.decrypt_block(ct) == pt + + +def test_encrypt_decrypt_roundtrip(bf): + data = b'\x12\x34\x56\x78' * 100 # 400B + encrypted = bf.encrypt_ecb(data) + decrypted = bf.decrypt_ecb(encrypted) + assert data == decrypted + + +def test_encrypt_size_must_be_8x(bf): + with pytest.raises(AssertionError): + bf.encrypt_ecb(b'\x00' * 7) diff --git a/tests/test_lowcmd_builder.py b/tests/test_lowcmd_builder.py new file mode 100644 index 0000000..43657b1 --- /dev/null +++ b/tests/test_lowcmd_builder.py @@ -0,0 +1,94 @@ +"""LowCmd 序列化测试 (PRO 格式).""" +import os +import pytest +from go1_pro_sdk import ( + Blowfish, LowCmd, MotorCmd, MotorMode, + build_low_cmd_plain, build_low_cmd_encrypted, +) + + +STATE_FILE = os.path.join( + os.path.dirname(__file__), '..', 'go1_pro_sdk', '_data', 'blowfish_state.bin' +) + + +@pytest.fixture +def bf(): + return Blowfish.from_state_file(STATE_FILE) + + +def test_plain_length_616(): + cmd = LowCmd() + plain = build_low_cmd_plain(cmd) + assert len(plain) == 616 + + +def test_plain_head(): + cmd = LowCmd() + plain = build_low_cmd_plain(cmd) + assert plain[:2] == b'\xfe\xef' + assert plain[2] == 0xff + + +def test_plain_sn_version_all_zero(): + """PRO 真实包 SN/version 全 0.""" + cmd = LowCmd() + plain = build_low_cmd_plain(cmd) + assert plain[4:12] == b'\x00' * 8 # SN + assert plain[12:20] == b'\x00' * 8 # version + + +def test_plain_bandwidth_be(): + """bandWidth 是 BE 字节序 (3a c0, 不是 c0 3a).""" + cmd = LowCmd() + plain = build_low_cmd_plain(cmd) + assert plain[20:22] == b'\x3a\xc0' + + +def test_plain_padding_zeros(): + """610..612 是固定 0000.""" + cmd = LowCmd() + plain = build_low_cmd_plain(cmd) + assert plain[610:612] == b'\x00\x00' + + +def test_plain_crc_at_612(): + """CRC 在 612..616, 不是 EDU 的 610..614.""" + cmd = LowCmd() + plain = build_low_cmd_plain(cmd) + from go1_pro_sdk.utils.common import gen_crc + expected_crc = gen_crc(plain[:612]) + assert plain[612:616] == expected_crc + + +def test_encrypted_damping_matches_real(bf): + """全 damping 加密后, 前 16B 应跟真实 Legged_sport 抓包一致.""" + cmd = LowCmd() + enc = build_low_cmd_encrypted(cmd, bf) + assert len(enc) == 616 + # 真实抓包前 16B (strace 抓的 Legged_sport sendto 内容) + expected = bytes.fromhex('e79e1ccc4bae9cf812aa0354236b66e3') + assert enc[:16] == expected + + +def test_set_motor_by_name(): + cmd = LowCmd() + cmd.set_motor('FR_1', MotorCmd(mode=MotorMode.Servo, q=1.2, Kp=5, Kd=1)) + assert cmd.motorCmd[1].mode == MotorMode.Servo + assert cmd.motorCmd[1].q == 1.2 + assert cmd.motorCmd[1].Kp == 5 + + +def test_set_motor_by_index(): + cmd = LowCmd() + cmd.set_motor(5, MotorCmd(q=2.0)) + assert cmd.motorCmd[5].q == 2.0 + + +def test_all_damping(): + cmd = LowCmd() + cmd.motorCmd[0].mode = MotorMode.Servo + cmd.motorCmd[0].q = 1.5 + cmd.all_damping() + assert cmd.motorCmd[0].mode == MotorMode.Damping + assert cmd.motorCmd[0].q == 0.0 diff --git a/tests/test_safety.py b/tests/test_safety.py new file mode 100644 index 0000000..9837898 --- /dev/null +++ b/tests/test_safety.py @@ -0,0 +1,82 @@ +"""Safety 保护层测试.""" +import pytest +from go1_pro_sdk import ( + LowCmd, MotorCmd, MotorMode, + apply_safety, position_limit, power_protect, + PowerProtectViolation, + TAU_MAX, +) + + +class _FakeMotor: + tauEst = 0.0 + q = 0.0 + + +class _FakeState: + def __init__(self): + self.motorState = [_FakeMotor() for _ in range(20)] + + +def test_position_limit_clamps_hip_over(): + cmd = LowCmd() + cmd.motorCmd[0].q = 5.0 # FR_0 hip 超限 + position_limit(cmd) + assert cmd.motorCmd[0].q == 0.78 + + +def test_position_limit_clamps_thigh_under(): + cmd = LowCmd() + cmd.motorCmd[1].q = -3.0 + position_limit(cmd) + assert cmd.motorCmd[1].q == -0.60 + + +def test_power_protect_clamps(): + cmd = LowCmd() + for i in range(12): + cmd.motorCmd[i].tau = 10.0 + state = _FakeState() + n = power_protect(cmd, state, factor=1) + # factor=1 → 限制到 TAU_MAX * 0.1 + assert cmd.motorCmd[0].tau == TAU_MAX['hip'] * 0.1 + assert cmd.motorCmd[2].tau == TAU_MAX['knee'] * 0.1 + + +def test_power_protect_critical_command_raises(): + cmd = LowCmd() + cmd.motorCmd[2].tau = 200.0 # > 5 × 35.55 + with pytest.raises(PowerProtectViolation): + power_protect(cmd, _FakeState(), factor=5) + + +def test_power_protect_overload_actual_raises(): + """实测 tauEst 已超 max, 也应 raise.""" + cmd = LowCmd() + cmd.motorCmd[2].tau = 1.0 + state = _FakeState() + state.motorState[2].tauEst = 40.0 # > 35.55 + with pytest.raises(PowerProtectViolation): + power_protect(cmd, state, factor=1) + + +def test_apply_safety_degraded_mode(): + """raise_on_critical=False 应当降级到全 damping.""" + cmd = LowCmd() + cmd.motorCmd[2].tau = 200.0 + cmd.motorCmd[2].Kp = 10 + apply_safety(cmd, _FakeState(), power_factor=5, + position_limit_on=False, raise_on_critical=False) + assert cmd.motorCmd[0].tau == 0 + assert cmd.motorCmd[0].Kp == 0 + assert cmd.motorCmd[0].mode == 0 + + +def test_apply_safety_normal_flow(): + """正常命令应能通过. 返回 LowCmd.""" + cmd = LowCmd() + cmd.motorCmd[0].q = 0.5 + cmd.motorCmd[0].tau = 1.0 + result = apply_safety(cmd, _FakeState(), power_factor=5) + assert result is cmd + assert cmd.motorCmd[0].q == 0.5 # 在限位内, 不变 diff --git a/tools/README.md b/tools/README.md new file mode 100644 index 0000000..7e82e76 --- /dev/null +++ b/tools/README.md @@ -0,0 +1,79 @@ +# Tools + +一次性诊断/校准工具. 不是 SDK 核心. + +## 密钥提取 + +### extract_blowfish_key.sh + +在狗上 (Pi) 运行, 从 Legged_sport 进程内存提取 Blowfish state. + +```bash +scp tools/extract_blowfish_key.sh pi@192.168.123.161:/tmp/ +ssh pi@192.168.123.161 "sudo bash /tmp/extract_blowfish_key.sh" +scp pi@192.168.123.161:/tmp/blowfish_dump.tar.gz ./ +``` + +### analyze_blowfish_dump.py + +Mac 端解析 dump, 找出 4168 字节 state. + +```bash +python tools/analyze_blowfish_dump.py blowfish_dump.tar.gz +``` + +## 校准 + +### calibrate_joints.py + +手动转动法测每个关节真实活动范围. + +```bash +# 狗悬空 + L2+B damping, 然后: +python tools/calibrate_joints.py --duration 90 +# 慢慢把每条腿的每个关节转到极限 +# 退出时输出 JOINT_LIMITS_MEASURED 字典, 可粘贴到 utils/constants.py +``` + +## 抢占源管理 + +### stop_sportmode.sh + +```bash +bash tools/stop_sportmode.sh stop # 杀 keep_sport_alive + Legged_sport + appTransit +bash tools/stop_sportmode.sh start # 恢复 +bash tools/stop_sportmode.sh status # 看当前状态 +``` + +### run_on_mac.sh + +一键流程: 停抢占源 → 跑测试 → (手动恢复). + +```bash +bash tools/run_on_mac.sh example +bash tools/run_on_mac.sh monitor +bash tools/run_on_mac.sh stop +bash tools/run_on_mac.sh start +``` + +### run_on_pi.sh + +把测试脚本部署到 Pi 上跑 (源 IP 跟 Legged_sport 一致, 现在不需要了, 但保留作为备选). + +## 协议研究 + +### capture_lowcmd.sh + _raw_capture.py + +Pi 端 tcpdump 抓 Legged_sport 真实发送的 LowCmd 流量. + +```bash +bash tools/capture_lowcmd.sh 3 +``` + +### diff_real_lowcmd.sh + _compare_lowcmd.py + +抓真实包 + 跟本地 buildCmd 输出做字节级 diff. 用于发现协议细节差异. + +```bash +bash tools/diff_real_lowcmd.sh 3 +``` diff --git a/tools/_compare_lowcmd.py b/tools/_compare_lowcmd.py new file mode 100644 index 0000000..d01a8f0 --- /dev/null +++ b/tools/_compare_lowcmd.py @@ -0,0 +1,212 @@ +#!/usr/bin/env python3 +""" +解析 pcap, 取出真实 LowCmd 密文 → Blowfish 解密 → 跟本地 buildCmd() 字节对比 +找出剩余差异 (SN/version/frameReserve/CRC/padding 等) +""" +import sys, os, struct, argparse + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '../..')) +sys.path.insert(0, os.path.dirname(__file__)) +from blowfish_codec import Blowfish +from ucl.lowCmd import lowCmd +from ucl.complex import motorCmd, motorCmdArray + + +# LowCmd 字段布局 (基于 buildCmd 源码) +LAYOUT = [ + ( 0, 2, 'head'), + ( 2, 3, 'levelFlag'), + ( 3, 4, 'frameReserve'), + ( 4, 12, 'SN'), + ( 12, 20, 'version'), + ( 20, 22, 'bandWidth'), + ( 22, 562, 'motorCmd[20]'), + (562, 566, 'bms'), + (566, 606, 'wirelessRemote'), + (606, 610, 'reserve'), + (610, 614, 'CRC'), + (614, 616, 'padding'), +] + + +def field_at(offset): + """根据 offset 返回字段名""" + for start, end, name in LAYOUT: + if start <= offset < end: + return name, start, end + return '?', -1, -1 + + +def parse_pcap(path): + """提取所有 UDP payload (兼容 SLL / Ethernet)""" + packets = [] + with open(path, 'rb') as f: + gh = f.read(24) + if len(gh) < 24: return packets + magic = struct.unpack('' + linktype = struct.unpack(f'{endian}I', gh[20:24])[0] + + while True: + ph = f.read(16) + if len(ph) < 16: break + ts_sec, ts_usec, caplen, _ = struct.unpack(f'{endian}IIII', ph) + pkt = f.read(caplen) + if len(pkt) < caplen: break + + if linktype == 1: + if len(pkt) < 14 or struct.unpack('>H', pkt[12:14])[0] != 0x0800: continue + ip_off = 14 + elif linktype == 113: # SLL + if len(pkt) < 16 or struct.unpack('>H', pkt[14:16])[0] != 0x0800: continue + ip_off = 16 + elif linktype == 276: # SLL2 + if len(pkt) < 20 or struct.unpack('>H', pkt[:2])[0] != 0x0800: continue + ip_off = 20 + else: + continue + + if len(pkt) < ip_off + 20: continue + ihl = (pkt[ip_off] & 0x0f) * 4 + if pkt[ip_off + 9] != 17: continue + udp_off = ip_off + ihl + if len(pkt) < udp_off + 8: continue + _, _, ulen, _ = struct.unpack('>HHHH', pkt[udp_off:udp_off+8]) + payload = pkt[udp_off+8:udp_off+ulen] + packets.append(payload) + return packets + + +def build_local_lowcmd(encrypted=True): + """本地生成 LowCmd (跟 test_sin_leg / lowcmd_codec 一致)""" + lcmd = lowCmd() + lcmd.SN = bytearray.fromhex('0402030508020100') + lcmd.version = bytearray.fromhex('000109000200fc7f') + mca = motorCmdArray() + for i in range(20): + mca.setMotorCmd(i, motorCmd(mode=0, q=0, dq=0, tau=0, Kp=0, Kd=0)) + lcmd.motorCmd = mca + plain = lcmd.buildCmd(debug=False) # 614B + if not encrypted: + return plain + bf = Blowfish.from_state_file('data/lowlevel_captures/blowfish_state.bin') + padded = plain + b'\x00' * (8 - len(plain) % 8) + return padded, bf.encrypt_ecb(padded) # (明文616, 密文616) + + +def hex_dump(b, indent=' '): + for i in range(0, len(b), 32): + print(f"{indent}{i:04x}: {b[i:i+32].hex()}") + + +def diff(a, b, label_a='REAL', label_b='LOCAL'): + """对比两段字节, 按字段分组报告差异""" + n = min(len(a), len(b)) + print(f"\n{'='*70}") + print(f"对比 {label_a} ({len(a)}B) vs {label_b} ({len(b)}B)") + print(f"{'='*70}") + if len(a) != len(b): + print(f"⚠️ 长度不一致") + + # 按字段分组 + diffs_by_field = {} + for i in range(n): + if a[i] != b[i]: + name, _, _ = field_at(i) + diffs_by_field.setdefault(name, []).append(i) + + if not diffs_by_field: + print(f"✅ 完全一致") + return + + print(f"\n字段级差异:") + for name in [n for _, _, n in LAYOUT]: + if name not in diffs_by_field: continue + idxs = diffs_by_field[name] + start, end = None, None + for s, e, nm in LAYOUT: + if nm == name: start, end = s, e; break + seg_a = a[start:end] + seg_b = b[start:end] + print(f"\n 📍 {name} [{start}..{end}] ({len(idxs)}/{end-start} 字节不同):") + print(f" {label_a}: {seg_a.hex()}") + print(f" {label_b}: {seg_b.hex()}") + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('pcap') + parser.add_argument('--cipher-out', help='保存第一帧密文') + parser.add_argument('--plain-out', help='保存第一帧解密后明文') + parser.add_argument('--state', default='data/lowlevel_captures/blowfish_state.bin') + args = parser.parse_args() + + print(f"\n📥 解析 {args.pcap}...") + packets = parse_pcap(args.pcap) + print(f" 共 {len(packets)} 个 UDP 包") + if not packets: + print(" ❌ 空 pcap") + return 1 + + # 长度统计 + from collections import Counter + sizes = Counter(len(p) for p in packets) + print(f" 包大小分布:") + for sz, cnt in sorted(sizes.items()): + print(f" {sz}B: {cnt}") + + # 取第一帧 + real_cipher = packets[0] + print(f"\n📦 真实 LowCmd 密文 ({len(real_cipher)}B):") + hex_dump(real_cipher[:64]) + print(f" ...") + hex_dump(real_cipher[-32:]) + + # 解密 + bf = Blowfish.from_state_file(args.state) + aligned = (len(real_cipher) // 8) * 8 + real_plain = bf.decrypt_ecb(real_cipher[:aligned]) + print(f"\n🔓 解密后 ({len(real_plain)}B):") + hex_dump(real_plain[:64]) + print(f" ...") + hex_dump(real_plain[-32:]) + + if not real_plain.startswith(b'\xfe\xef\xff'): + print(f"\n❌ 解密失败! 包头不是 feef ff") + print(f" Blowfish state 可能不对, 或者不是 LowCmd 流") + return 1 + print(f"\n✅ 包头匹配 feef ff") + + # 保存 + if args.cipher_out: + with open(args.cipher_out, 'wb') as f: + f.write(real_cipher) + print(f"💾 密文 → {args.cipher_out}") + if args.plain_out: + with open(args.plain_out, 'wb') as f: + f.write(real_plain) + print(f"💾 明文 → {args.plain_out}") + + # 本地生成对比 + local_plain, local_cipher = build_local_lowcmd(encrypted=True) + print(f"\n🛠 本地生成 LowCmd (明文 614B padded 616B):") + hex_dump(local_plain[:64]) + print(f" ...") + hex_dump(local_plain[-32:]) + + # 明文对比 (核心!) + diff(real_plain, local_plain, '真实(解密后)', '本地(buildCmd)') + + # 密文对比 (验证加密是否一致) + diff(real_cipher, local_cipher, '真实密文', '本地加密') + + # 看连续两帧真实包的变化 (说明哪些字段是动态的) + if len(packets) > 1: + p2 = bf.decrypt_ecb(packets[1][:aligned]) + diff(real_plain, p2, '真实#0', '真实#1') + + return 0 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/tools/_raw_capture.py b/tools/_raw_capture.py new file mode 100644 index 0000000..7b74796 --- /dev/null +++ b/tools/_raw_capture.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 +""" +纯Python 抓包工具 (在Pi上运行, 无需 tcpdump) + +用 AF_PACKET 原始套接字抓所有以太网包, 过滤出指定的 UDP 流, 写成标准 pcap 格式 +用法: sudo python3 _raw_capture.py <输出pcap> <时长秒> [--filter-host IP] [--filter-port PORT] +""" +import socket, struct, sys, time, argparse + +ETH_P_ALL = 0x0003 + +PCAP_MAGIC = 0xa1b2c3d4 +LINKTYPE_ETHERNET = 1 + + +def write_pcap_global_header(f): + f.write(struct.pack('H', pkt[12:14])[0] + if eth_type != 0x0800: return None # 只要IPv4 + + # IP 头 + if len(pkt) < 14 + 20: return None + ihl = (pkt[14] & 0x0f) * 4 + proto = pkt[14 + 9] + if proto != 17: return None # 只要 UDP + src_ip = '.'.join(str(b) for b in pkt[14+12:14+16]) + dst_ip = '.'.join(str(b) for b in pkt[14+16:14+20]) + + # UDP 头 + udp_off = 14 + ihl + if len(pkt) < udp_off + 8: return None + src_port, dst_port = struct.unpack('>HH', pkt[udp_off:udp_off+4]) + return src_ip, dst_ip, src_port, dst_port + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('output', help='输出pcap路径') + parser.add_argument('duration', type=float, help='时长(秒)') + parser.add_argument('--filter-host', help='只抓涉及此IP的') + parser.add_argument('--filter-port', type=int, help='只抓涉及此端口的') + parser.add_argument('--iface', default=None, help='绑定接口名(如eth0)') + args = parser.parse_args() + + sock = socket.socket(socket.AF_PACKET, socket.SOCK_RAW, socket.htons(ETH_P_ALL)) + if args.iface: + sock.bind((args.iface, 0)) + print(f" 抓包接口: {args.iface}", flush=True) + else: + print(f" 抓包接口: ALL", flush=True) + sock.settimeout(0.5) + + out = open(args.output, 'wb') + write_pcap_global_header(out) + + end = time.time() + args.duration + n_total = 0 + n_match = 0 + n_ipv4 = 0 + n_udp = 0 + from collections import Counter + flow_counter = Counter() # 统计 UDP 流向 + + print(f" 抓包 {args.duration}s...", flush=True) + while time.time() < end: + try: + pkt, _ = sock.recvfrom(65535) + except socket.timeout: + continue + n_total += 1 + # 粗略判断: 14字节以太网头 + IP头 + if len(pkt) >= 14: + eth_type = struct.unpack('>H', pkt[12:14])[0] + if eth_type == 0x0800: + n_ipv4 += 1 + if len(pkt) >= 14+20 and pkt[14+9] == 17: + n_udp += 1 + info = parse_packet(pkt) + if info is None: continue + src_ip, dst_ip, sp, dp = info + flow_counter[(src_ip, sp, dst_ip, dp)] += 1 + + if args.filter_host: + if args.filter_host not in (src_ip, dst_ip): continue + if args.filter_port: + if args.filter_port not in (sp, dp): continue + # 每命中第一个新流就打印一下, 方便诊断 + flow = (src_ip, sp, dst_ip, dp) + if not hasattr(main, '_seen'): + main._seen = set() + if flow not in main._seen: + main._seen.add(flow) + print(f" 新流: {src_ip}:{sp} → {dst_ip}:{dp}", flush=True) + + write_pcap_packet(out, time.time(), pkt) + n_match += 1 + + out.close() + sock.close() + print(f" 总包: {n_total}, IPv4: {n_ipv4}, UDP: {n_udp}, 命中: {n_match}", flush=True) + print(f" Top UDP 流 (前20):", flush=True) + for flow, cnt in flow_counter.most_common(20): + sip, spt, dip, dpt = flow + print(f" {sip}:{spt} → {dip}:{dpt} ({cnt})", flush=True) + + +if __name__ == '__main__': + main() diff --git a/tools/analyze_blowfish_dump.py b/tools/analyze_blowfish_dump.py new file mode 100644 index 0000000..7bf3d52 --- /dev/null +++ b/tools/analyze_blowfish_dump.py @@ -0,0 +1,310 @@ +#!/usr/bin/env python3 +""" +分析从狗上提取的Blowfish密钥dump +1. 在内存中搜索Blowfish P-array (key schedule后的运行时值) +2. 提取P-array和S-box,反推原始密钥 +3. 用提取的P-array+S-box解密已知密文,验证正确性 +""" +import os +import sys +import struct +import tarfile +import argparse +from pathlib import Path + +# Blowfish标准初始常数 (从π推导) +P_INITIAL = [ + 0x243f6a88, 0x85a308d3, 0x13198a2e, 0x03707344, + 0xa4093822, 0x299f31d0, 0x082efa98, 0xec4e6c89, + 0x452821e6, 0x38d01377, 0xbe5466cf, 0x34e90c6c, + 0xc0ac29b7, 0xc97c50dd, 0x3f84d5b5, 0xb5470917, + 0x9216d5d9, 0x8979fb1b +] + +S0_INITIAL_FIRST = 0xd1310ba6 +S1_INITIAL_FIRST = 0x4b7a70e9 +S2_INITIAL_FIRST = 0xe93d5a68 +S3_INITIAL_FIRST = 0x3a39ce37 + +# 已知明文-密文对 (用于验证) +KNOWN_PAIRS = [ + (bytes.fromhex('0000000000000000'), bytes.fromhex('12aa0354236b66e3')), + (bytes.fromhex('feefff0004020305'), bytes.fromhex('5d9874ad7eee5851')), + (bytes.fromhex('0802010000010900'), bytes.fromhex('c5ce0887ceb86f91')), +] + + +def find_pattern_locations(data, pattern_bytes): + """在data中查找pattern的所有位置""" + positions = [] + pos = 0 + while True: + p = data.find(pattern_bytes, pos) + if p < 0: + break + positions.append(p) + pos = p + 1 + return positions + + +def looks_like_blowfish_buffer(data, offset): + """ + 检查offset处是否看起来像Blowfish state: + 布局假设: P[18] + S0[256] + S1[256] + S2[256] + S3[256] + 总大小: (18 + 4*256) * 4 = 4168 字节 + """ + BUF_SIZE = 4168 + if offset + BUF_SIZE > len(data): + return False + + # 取出整个buffer + buf = data[offset:offset+BUF_SIZE] + + # 解释为uint32列表 + words = struct.unpack(f'<{BUF_SIZE//4}I', buf) + + # 检查S-box是否包含至少部分初始值 + # 运行时,P和S-box已被key schedule修改,但所有1042个值的统计特性应该接近随机 + # 这里只能做粗略的"看起来像加密表"的检查 + + # 唯一性检查: 1042个32位值应该几乎都不同 + unique = len(set(words)) + if unique < 1000: # 太多重复说明不是表 + return False + + # 零值检查: 不应该有很多0 + zeros = sum(1 for w in words if w == 0) + if zeros > 50: + return False + + # 熵检查: 字节分布应该均匀 + byte_counts = [0] * 256 + for b in buf: + byte_counts[b] += 1 + max_count = max(byte_counts) + if max_count > len(buf) // 50: # 任何字节出现频率超过2%可疑 + return False + + return True + + +def encrypt_with_state(plaintext_8, P, S0, S1, S2, S3, endian='<'): + """ + 用给定的P-array + S-box加密8字节明文 + endian: '<' (LE - Unitree style) 或 '>' (BE - standard) + """ + pack_fmt = endian + 'II' + L, R = struct.unpack(pack_fmt, plaintext_8) + + for i in range(16): + L ^= P[i] + # F(L) + a = (L >> 24) & 0xff + b = (L >> 16) & 0xff + c = (L >> 8) & 0xff + d = L & 0xff + F = (((S0[a] + S1[b]) & 0xFFFFFFFF) ^ S2[c]) & 0xFFFFFFFF + F = (F + S3[d]) & 0xFFFFFFFF + R ^= F + L, R = R, L + + L, R = R, L + R ^= P[16] + L ^= P[17] + + return struct.pack(pack_fmt, L, R) + + +def try_buffer_as_blowfish(buf_data, label="?"): + """ + 把一个4168字节的buffer解释为Blowfish state + 用已知明密文对验证 + """ + if len(buf_data) < 4168: + return None + + # 假设布局1: P[0..17] + S0[0..255] + S1[0..255] + S2[0..255] + S3[0..255] + words = struct.unpack('<1042I', buf_data[:4168]) + P = list(words[0:18]) + S0 = list(words[18:18+256]) + S1 = list(words[18+256:18+512]) + S2 = list(words[18+512:18+768]) + S3 = list(words[18+768:18+1024]) + + # 测试已知对 - 同时尝试LE和BE两种字节序 + best_matches = 0 + best_endian = None + for endian in ['<', '>']: + matches = 0 + for pt, ct_expected in KNOWN_PAIRS: + try: + ct = encrypt_with_state(pt, P, S0, S1, S2, S3, endian=endian) + if ct == ct_expected: + matches += 1 + except Exception: + pass + if matches > best_matches: + best_matches = matches + best_endian = endian + + return best_matches, best_endian + + +def search_blowfish_state(data, source_name, verbose=False, save_candidates=False): + """在内存dump中搜索Blowfish状态""" + print(f"\n=== 在 {source_name} 中搜索 Blowfish state ===") + print(f" 大小: {len(data)} 字节") + candidates_looking_like_bf = [] + + # 策略1: 找S-box[0]首字 (运行时S-box会被修改,但偶尔可能未变) + # 实际上key schedule后S-box几乎肯定变化,所以这条不会找到 + s0_pos = find_pattern_locations(data, struct.pack(' {cand_path}") + + result = try_buffer_as_blowfish(data[offset:offset+4168]) + if result is None: + continue + matches, endian = result + if matches and matches > 0: + endian_name = 'LE' if endian == '<' else 'BE' + print(f"\n ⭐ offset 0x{offset:x}: {matches}/3 已知对匹配! ({endian_name})") + found.append((offset, matches, endian)) + if matches == 3: + return found, data[offset:offset+4168] + + if candidates_looking_like_bf: + print(f" 共找到 {len(candidates_looking_like_bf)} 个通过启发式过滤的候选位置") + + return found, None + + +def extract_key_from_state(state_buf): + """从运行时P-array+S-box反推原始密钥 (这是Blowfish key schedule的逆过程) + + 技术: 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 + + 要反推key: 由于步骤2-4都是确定性的,只要知道key就能复现 + 而key只在步骤1出现, P[i] XOR P_INITIAL[i] = key_word(i % keylen) + """ + words = struct.unpack('<1042I', state_buf[:4168]) + P_runtime = list(words[0:18]) + + # 但是! P-array在步骤2-4也被修改了, 所以不能直接 P XOR P_INITIAL + # 实际上, 步骤1后的P[i]立即在步骤2中被覆盖, 我们看到的是覆盖后的值 + # 这意味着无法从最终state直接反推key + + # 但我们已经有了完整的state, 不需要key就能加密/解密! + print("\n=== 提取的密钥state ===") + print(f"P-array (18 uint32):") + for i in range(18): + print(f" P[{i:2d}] = 0x{P_runtime[i]:08x}") + + return P_runtime + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('input', help='blowfish_dump.tar.gz 或 包含.bin文件的目录') + parser.add_argument('--verbose', '-v', action='store_true') + parser.add_argument('--save-candidates', action='store_true', + help='保存所有"看起来像Blowfish state"的候选 (即使不能验证已知对)') + args = parser.parse_args() + + input_path = Path(args.input) + + # 提取/找到所有bin文件 + bin_files = [] + if input_path.is_file() and input_path.suffix == '.gz': + # tar.gz - 解压到临时目录 + import tempfile + tmpdir = tempfile.mkdtemp(prefix='blowfish_analyze_') + print(f"解压到 {tmpdir}...") + with tarfile.open(input_path) as tf: + tf.extractall(tmpdir) + for f in Path(tmpdir).rglob('*.bin'): + bin_files.append(f) + elif input_path.is_dir(): + for f in input_path.rglob('*.bin'): + bin_files.append(f) + else: + print(f"❌ 不支持的输入: {input_path}") + return 1 + + print(f"找到 {len(bin_files)} 个 .bin 文件") + for f in bin_files: + size = f.stat().st_size + print(f" {f.name} ({size} 字节)") + + # 按大小排序,先搜索小的 (更快) + bin_files.sort(key=lambda f: f.stat().st_size) + + found_state = None + for f in bin_files: + with open(f, 'rb') as fp: + data = fp.read() + results, state = search_blowfish_state(data, f.name, verbose=args.verbose, save_candidates=args.save_candidates) + if state is not None: + found_state = state + print(f"\n🎉 在 {f.name} 中找到完整匹配!") + break + elif results: + print(f"\n⚠️ 在 {f.name} 中找到 {len(results)} 个部分匹配位置") + + if found_state: + P = extract_key_from_state(found_state) + # 保存 + outpath = 'data/lowlevel_captures/blowfish_state.bin' + os.makedirs('data/lowlevel_captures', exist_ok=True) + with open(outpath, 'wb') as fp: + fp.write(found_state) + print(f"\n✅ Blowfish state已保存到 {outpath}") + print(f" 可以用此state直接加密/解密通信数据") + return 0 + else: + print("\n❌ 未找到完整匹配的Blowfish state") + print(" 可能原因:") + print(" 1. 状态不在已dump的内存区域") + print(" 2. 状态被分散存储 (不连续)") + print(" 3. dump时机不对 (state还未初始化)") + return 1 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/tools/calibrate_joints.py b/tools/calibrate_joints.py new file mode 100644 index 0000000..3f9c737 --- /dev/null +++ b/tools/calibrate_joints.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +"""关节活动范围校准 - 手动转动法. + +流程: + 1. 狗悬空 + L2+B damping + 2. 跑这个脚本 + 3. 慢慢把每条腿的每个关节转到极限两端 (软力, 听到阻力就停) + 4. 输出每个关节实测的 min/max/range +""" +import argparse +import os +import signal +import sys +import time + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) +from go1_pro_sdk import MCUClient, LowCmd, JOINT_NAMES +from go1_pro_sdk.utils.constants import JOINT_LIMITS_URDF + + +running = True + + +def sigint(s, f): + global running + running = False + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--state', default=None) + parser.add_argument('--duration', type=float, default=60) + args = parser.parse_args() + signal.signal(signal.SIGINT, sigint) + + print('🦾 关节活动范围校准') + print(f' 采样时长: {args.duration}s') + print() + print('操作:') + print(' 1. 狗悬空, L2+B damping (电机失能)') + print(' 2. 按顺序逐条腿/逐个关节转到极限') + print(' 3. Ctrl+C 提前结束') + input('回车开始采样...') + + with MCUClient(state_path=args.state) as client: + recv = client.wake_mcu(50) + if recv == 0: + print('❌ 无回包') + return 1 + + damping = LowCmd().all_damping() + q_min = [float('inf')] * 12 + q_max = [float('-inf')] * 12 + + start = time.time() + last_print = 0 + n_decoded = 0 + + while running and time.time() - start < args.duration: + client.send(damping) + time.sleep(0.01) + state = client.recv_latest() + if state is None: + continue + n_decoded += 1 + for i in range(12): + q = state.motorState[i].q + if q < q_min[i]: q_min[i] = q + if q > q_max[i]: q_max[i] = q + + now = time.time() + if now - last_print >= 2.0: + print(f'\r⏱ {now-start:5.1f}/{args.duration:.0f}s 解码 {n_decoded} 帧', + end='', flush=True) + last_print = now + + print('\n\n实测范围 vs URDF 标称:') + print('-' * 80) + print(f'{"关节":8} {"实测min":>9} {"实测max":>9} {"范围":>8} ' + f'{"URDF min":>9} {"URDF max":>9}') + for i, name in enumerate(JOINT_NAMES): + jt = ('hip' if i % 3 == 0 else 'thigh' if i % 3 == 1 else 'knee') + urdf_lo, urdf_hi = JOINT_LIMITS_URDF[jt] + lo, hi = q_min[i], q_max[i] + if lo == float('inf'): + print(f'{name:8} {"未采到":>9}') + continue + print(f'{name:8} {lo:+9.4f} {hi:+9.4f} {hi-lo:8.4f} ' + f'{urdf_lo:+9.4f} {urdf_hi:+9.4f}') + + print('\nJOINT_LIMITS_MEASURED = {') + for i, name in enumerate(JOINT_NAMES): + if q_min[i] == float('inf'): continue + print(f" '{name}': ({q_min[i]:+.4f}, {q_max[i]:+.4f}),") + print('}') + + +if __name__ == '__main__': + sys.exit(main() or 0) diff --git a/tools/capture_lowcmd.sh b/tools/capture_lowcmd.sh new file mode 100644 index 0000000..a35ef3d --- /dev/null +++ b/tools/capture_lowcmd.sh @@ -0,0 +1,73 @@ +#!/bin/bash +# 抓 Legged_sport 发给 MCU :8007 的 LowCmd UDP 包, 拉回本地解析 +# +# 用法: bash tools/onboard/capture_lowcmd.sh [seconds] +# +# 流程: +# 1. SSH 到狗启动 tcpdump 抓包 (持续N秒) +# 2. 拉回 pcap 到本地 +# 3. 用 Python 解析 pcap, 提取 LowCmd 字节 +# 4. 跟本地 buildCmd() 的输出对比 + +PI_HOST="${PI_HOST:-192.168.123.161}" +PI_USER="${PI_USER:-pi}" +DURATION="${1:-5}" # 抓包秒数 + +PCAP_REMOTE=/tmp/legged_sport.pcap +PCAP_LOCAL=data/lowlevel_captures/legged_sport_$(date +%Y%m%d_%H%M%S).pcap + +echo "============================================" +echo "🔍 抓 Legged_sport → MCU 的 LowCmd 包" +echo "============================================" +echo " 抓包时长: ${DURATION}s" +echo " 远端 pcap: ${PCAP_REMOTE}" +echo " 本地保存: ${PCAP_LOCAL}" +echo "" + +# 先确认 Legged_sport 在运行 (否则抓不到包) +echo "[1/4] 确认 Legged_sport 在运行..." +if ssh ${PI_USER}@${PI_HOST} "pgrep Legged_sport > /dev/null 2>&1"; then + echo " ✅ Legged_sport 在运行" +else + echo " ⚠️ Legged_sport 未运行! 先恢复 sportMode:" + echo " bash tools/onboard/stop_sportmode.sh start" + exit 1 +fi + +# 抓包: 用 tcpdump (已在 Pi 上安装) +echo "" +echo "[2/4] 启动 tcpdump 抓包 (${DURATION}s, eth0)..." + +ssh ${PI_USER}@${PI_HOST} bash -s </tmp/tcpdump.log +echo "tcpdump 退出码: \$?" +if [ -f ${PCAP_REMOTE} ]; then + sudo chmod 644 ${PCAP_REMOTE} + echo "pcap 大小: \$(stat -c%s ${PCAP_REMOTE}) 字节" + cat /tmp/tcpdump.log +else + echo "❌ pcap 未生成!" + cat /tmp/tcpdump.log + exit 1 +fi +EOF + +echo " ✅ 抓包完成" + +# 拉回本地 +echo "" +echo "[3/4] 拉回本地..." +mkdir -p $(dirname ${PCAP_LOCAL}) +if ! scp ${PI_USER}@${PI_HOST}:${PCAP_REMOTE} ${PCAP_LOCAL}; then + echo "❌ 拉取失败" + exit 1 +fi +echo " ✅ ${PCAP_LOCAL} ($(stat -f%z ${PCAP_LOCAL} 2>/dev/null || stat -c%s ${PCAP_LOCAL}) 字节)" + +# 解析+对比 +echo "" +echo "[4/4] 解析+对比..." +conda run -n free_dog_sdk python tools/onboard/diff_lowcmd.py ${PCAP_LOCAL} diff --git a/tools/diff_real_lowcmd.sh b/tools/diff_real_lowcmd.sh new file mode 100644 index 0000000..d7e0d7e --- /dev/null +++ b/tools/diff_real_lowcmd.sh @@ -0,0 +1,76 @@ +#!/bin/bash +# 抓 Legged_sport 真实 LowCmd → 解密 → 跟本地脚本生成的对比 +# +# 前提: Legged_sport 必须在跑且正常向 MCU 发包 (狗在工作状态, 不能是idle) +# 用法: bash tools/onboard/diff_real_lowcmd.sh [seconds] +# +# 输出: +# data/lowlevel_captures/real_lowcmd_.pcap - 抓的原包 +# data/lowlevel_captures/real_lowcmd_.bin - 一帧密文 (616B) +# data/lowlevel_captures/real_lowcmd_dec_.bin - 解密后 (616B) +# 字节级 diff 输出到屏幕 + +PI_HOST="${PI_HOST:-192.168.123.161}" +PI_USER="${PI_USER:-pi}" +DURATION="${1:-3}" +TS=$(date +%Y%m%d_%H%M%S) + +PCAP_REMOTE=/tmp/real_lowcmd.pcap +PCAP_LOCAL=data/lowlevel_captures/real_lowcmd_${TS}.pcap +CIPHER_BIN=data/lowlevel_captures/real_lowcmd_${TS}.bin +PLAIN_BIN=data/lowlevel_captures/real_lowcmd_dec_${TS}.bin + +mkdir -p $(dirname ${PCAP_LOCAL}) + +echo "============================================" +echo "🔍 抓真实 LowCmd 跟本地对比" +echo "============================================" +echo "" + +# 0. 确认 Legged_sport 在发包 (而不是 idle) +echo "[0/4] 确认 Legged_sport 在向 MCU 发包..." +COUNT=$(ssh ${PI_USER}@${PI_HOST} \ + "sudo timeout 1 /usr/sbin/tcpdump -i any -nn -c 5 'udp and dst host 192.168.123.10 and dst port 8007' 2>/dev/null | grep -c 'IP 192' || true") +if [ "$COUNT" -lt 1 ]; then + echo " ❌ Legged_sport 没在发 LowCmd!" + echo " 可能原因:" + echo " - 狗在 idle/damping 待机, 需要遥控器唤醒 (L2+A 让它站起来)" + echo " - MCU 被其他客户端 (我们 Mac 的脚本) 抢占了" + echo " - sportMode 服务挂了" + exit 1 +fi +echo " ✅ 检测到 ${COUNT} 个发往 MCU 的包" +echo "" + +# 1. 抓包 +echo "[1/4] tcpdump 抓 ${DURATION}s ..." +ssh ${PI_USER}@${PI_HOST} bash -s </tmp/tcpdump.log +sudo chmod 644 ${PCAP_REMOTE} +echo "pcap 大小: \$(stat -c%s ${PCAP_REMOTE}) 字节" +EOF + +# 2. 拉回 +echo "" +echo "[2/4] 拉回本地..." +scp ${PI_USER}@${PI_HOST}:${PCAP_REMOTE} ${PCAP_LOCAL} +echo " ✅ ${PCAP_LOCAL}" + +# 3. 解析+解密+对比 +echo "" +echo "[3/4] 解析+解密..." +conda run -n free_dog_sdk python tools/onboard/_compare_lowcmd.py \ + ${PCAP_LOCAL} \ + --cipher-out ${CIPHER_BIN} \ + --plain-out ${PLAIN_BIN} + +echo "" +echo "============================================" +echo "完成. 文件:" +echo " pcap: ${PCAP_LOCAL}" +echo " 密文: ${CIPHER_BIN}" +echo " 明文: ${PLAIN_BIN}" +echo "============================================" diff --git a/tools/extract_blowfish_key.sh b/tools/extract_blowfish_key.sh new file mode 100644 index 0000000..70cd8bf --- /dev/null +++ b/tools/extract_blowfish_key.sh @@ -0,0 +1,208 @@ +#!/bin/bash +# Blowfish密钥提取脚本 - 在树莓派上以root运行 +# 用法: sudo bash extract_blowfish_key.sh [PID] +# +# 安全保证: +# - 只读取内存,不修改任何寄存器或内存 +# - GDB attach时间最短化 (<30秒) +# - 自动detach,即使中途异常 + +set -e + +PID=${1:-$(pgrep -f Legged_sport | head -1)} +OUTDIR=/tmp/blowfish_dump_$$ +mkdir -p $OUTDIR + +if [ -z "$PID" ]; then + echo "❌ 找不到 Legged_sport 进程" + exit 1 +fi + +# 检查PID存在 +if [ ! -d "/proc/$PID" ]; then + echo "❌ PID $PID 不存在" + exit 1 +fi + +echo "============================================" +echo "🐕 Blowfish密钥提取" +echo "============================================" +echo "PID: $PID" +echo "输出: $OUTDIR" +echo "" + +# 检查当前狗的状态 (通过/tmp下的状态文件,如果有的话) +# 提示用户确认安全状态 +echo "⚠️ 确认狗的状态:" +echo " - 趴下(damping)或在脚架上" +echo " - 当前是高级模式 (未在执行运动指令)" +echo "" +read -p "确认安全状态? (yes/no): " confirm +if [ "$confirm" != "yes" ]; then + echo "❌ 用户取消" + rm -rf $OUTDIR + exit 1 +fi + +# ===== 第1步: 保存进程内存映射 ===== +echo "" +echo "[1/5] 保存进程内存映射..." +cat /proc/$PID/maps > $OUTDIR/maps.txt +echo " ✅ /proc/$PID/maps -> $OUTDIR/maps.txt" + +# 找出代码段范围 (anonymous r-x 段,是UPX解压后的代码) +ANON_RX=$(awk '/r-xp/ && !/\// {print $1}' $OUTDIR/maps.txt | head -1) +if [ -n "$ANON_RX" ]; then + ANON_RX_START=$(echo $ANON_RX | cut -d- -f1) + ANON_RX_END=$(echo $ANON_RX | cut -d- -f2) + echo " 📌 匿名代码段(UPX解压): 0x$ANON_RX_START - 0x$ANON_RX_END" +fi + +# 找堆段 +HEAP=$(awk '/\[heap\]/ {print $1}' $OUTDIR/maps.txt | head -1) +if [ -n "$HEAP" ]; then + HEAP_START=$(echo $HEAP | cut -d- -f1) + HEAP_END=$(echo $HEAP | cut -d- -f2) + echo " 📌 堆: 0x$HEAP_START - 0x$HEAP_END" +fi + +# ===== 第2步: 准备GDB脚本 (只读,不修改) ===== +echo "" +echo "[2/5] 生成GDB脚本..." + +cat > $OUTDIR/gdb_script.gdb <<'GDBSCRIPT' +# GDB脚本: 只读模式提取Blowfish密钥 +# 严格规定: 不修改任何寄存器、不写入任何内存、不调用任何函数 + +set pagination off +set print pretty on +set logging redirect on +set logging overwrite on + +# 1. 进程基本信息 +printf "=== 进程信息 ===\n" +info proc mappings + +printf "\n=== 寄存器状态 ===\n" +info registers + +printf "\n=== 调用栈 ===\n" +bt 5 + +# 2. 不设断点,只dump关键内存 + +# 完成 +printf "\n=== 完成,准备detach ===\n" +GDBSCRIPT + +echo " ✅ $OUTDIR/gdb_script.gdb" + +# ===== 第3步: GDB attach (最短时间) ===== +echo "" +echo "[3/5] GDB attach (目标<5秒)..." + +START_TS=$(date +%s) + +# 用-batch模式,自动执行后立即退出 +timeout 30 gdb -batch \ + -p $PID \ + -x $OUTDIR/gdb_script.gdb \ + > $OUTDIR/gdb_output.txt 2>&1 + +END_TS=$(date +%s) +ELAPSED=$((END_TS - START_TS)) +echo " ✅ GDB完成,耗时 ${ELAPSED}s" + +# 检查狗是否还在线 +if ! kill -0 $PID 2>/dev/null; then + echo " ⚠️ 警告: Legged_sport进程消失!" +fi + +# ===== 第4步: dump堆内存 (用/proc/PID/mem直接读取,比GDB快) ===== +echo "" +echo "[4/5] 直接读取堆内存 (不用GDB)..." + +if [ -n "$HEAP_START" ]; then + HEAP_SIZE=$((0x$HEAP_END - 0x$HEAP_START)) + HEAP_OFFSET=$((0x$HEAP_START)) + echo " 读取堆 ${HEAP_SIZE} 字节..." + + # 用dd直接从/proc/$PID/mem读 (bs=65536大块读取,比bs=1快1000x) + dd if=/proc/$PID/mem of=$OUTDIR/heap.bin \ + bs=65536 count=$HEAP_SIZE skip=$HEAP_OFFSET \ + iflag=skip_bytes,count_bytes 2>/dev/null || \ + echo " ⚠️ /proc/mem读取失败,可能需要ptrace" + + if [ -f $OUTDIR/heap.bin ]; then + HEAPACTUAL=$(stat -c%s $OUTDIR/heap.bin) + echo " ✅ heap.bin ($HEAPACTUAL 字节)" + fi +fi + +# 也dump匿名代码段 (UPX解压后的代码,含运行时S-box) +if [ -n "$ANON_RX_START" ]; then + RX_SIZE=$((0x$ANON_RX_END - 0x$ANON_RX_START)) + RX_OFFSET=$((0x$ANON_RX_START)) + echo " 读取匿名代码段 ${RX_SIZE} 字节..." + + dd if=/proc/$PID/mem of=$OUTDIR/anon_code.bin \ + bs=65536 count=$RX_SIZE skip=$RX_OFFSET \ + iflag=skip_bytes,count_bytes 2>/dev/null + + if [ -f $OUTDIR/anon_code.bin ]; then + CODEACTUAL=$(stat -c%s $OUTDIR/anon_code.bin) + echo " ✅ anon_code.bin ($CODEACTUAL 字节)" + fi +fi + +# 也dump所有匿名rw段 (含运行时S-box和P-array) +echo " 读取所有匿名rw段..." +RW_COUNT=0 +awk '/rw-p/ && !/\// && !/\[stack\]/ && !/\[vvar\]/ {print $1}' $OUTDIR/maps.txt | while read range; do + START=$(echo $range | cut -d- -f1) + END=$(echo $range | cut -d- -f2) + SIZE=$((0x$END - 0x$START)) + OFFSET=$((0x$START)) + + # 只dump合理大小的段 (1KB - 64MB) + if [ $SIZE -gt 1024 ] && [ $SIZE -lt 67108864 ]; then + OUTFILE=$OUTDIR/rw_${START}.bin + dd if=/proc/$PID/mem of=$OUTFILE \ + bs=65536 count=$SIZE skip=$OFFSET \ + iflag=skip_bytes,count_bytes 2>/dev/null + + if [ -f $OUTFILE ] && [ -s $OUTFILE ]; then + echo " rw_${START}.bin ($SIZE 字节)" + RW_COUNT=$((RW_COUNT + 1)) + else + rm -f $OUTFILE + fi + fi +done + +# ===== 第5步: 打包 ===== +echo "" +echo "[5/5] 打包输出..." +cd /tmp +TARBALL=blowfish_dump_$(date +%Y%m%d_%H%M%S).tar.gz +tar czf $TARBALL -C $OUTDIR . +mv $TARBALL /tmp/blowfish_dump.tar.gz +echo " ✅ /tmp/blowfish_dump.tar.gz ($(stat -c%s /tmp/blowfish_dump.tar.gz) 字节)" + +# 检查狗是否还在线 +echo "" +if kill -0 $PID 2>/dev/null; then + echo "✅ Legged_sport仍在运行 (PID $PID)" +else + echo "❌ Legged_sport已退出!" +fi + +echo "" +echo "============================================" +echo "完成!" +echo "============================================" +echo "数据位置: /tmp/blowfish_dump.tar.gz" +echo "中间文件: $OUTDIR/" +echo "" +echo "从Mac拉取:" +echo " scp pi@192.168.123.161:/tmp/blowfish_dump.tar.gz ./data/lowlevel_captures/" diff --git a/tools/run_on_mac.sh b/tools/run_on_mac.sh new file mode 100644 index 0000000..ee6142a --- /dev/null +++ b/tools/run_on_mac.sh @@ -0,0 +1,83 @@ +#!/bin/bash +# 在 Mac 上直连 MCU 跑控制 (需要先停 Pi 上的抢占源) +# +# 用法: +# bash tools/onboard/run_on_mac.sh example # 跑 example_position_pro.py +# bash tools/onboard/run_on_mac.sh sin # 跑 test_sin_leg.py +# bash tools/onboard/run_on_mac.sh monitor # 只读监听 +# bash tools/onboard/run_on_mac.sh stop # 只停 Pi 上的抢占源 +# bash tools/onboard/run_on_mac.sh start # 恢复 sportMode + +PI_HOST="${PI_HOST:-192.168.123.161}" +PI_USER="${PI_USER:-pi}" +TEST="${1:-example}" + +stop_competitors() { + echo "=== 停 Pi 上的 MCU:8007 抢占源 ===" + ssh ${PI_USER}@${PI_HOST} bash <<'EOF' || true +set +e +if sudo pkill -9 -f keep_sport_alive 2>/dev/null; then echo ' ✅ keep_sport_alive.sh'; else echo ' (无看门狗)'; fi +sleep 0.3 +if sudo pkill -9 -f Legged_sport 2>/dev/null; then echo ' ✅ Legged_sport'; else echo ' (无)'; fi +if sudo pkill -9 -f appTransit 2>/dev/null; then echo ' ✅ appTransit'; else echo ' (无)'; fi +sleep 0.5 +echo '--- 验证残留 ---' +if pgrep -fl 'Legged_sport|appTransit|keep_sport_alive' 2>/dev/null; then + echo ' ⚠️ 仍有残留' +else + echo ' ✅ 全部清除' +fi +EOF +} + +restart_sport() { + echo "=== 恢复 sportMode (Pi 端 watchdog 会拉起 Legged_sport) ===" + ssh ${PI_USER}@${PI_HOST} \ + "sudo bash -c 'nohup /home/pi/Unitree/autostart/sportMode/keep_sport_alive.sh > /dev/null 2>&1 &' 2>/dev/null || \ + sudo systemctl start sportMode 2>/dev/null || \ + echo '⚠️ 请手动重启或重启狗'" + sleep 2 + ssh ${PI_USER}@${PI_HOST} "pgrep -fl Legged_sport || echo '⚠️ Legged_sport 未运行'" +} + +case "$TEST" in + stop) + stop_competitors + ;; + start) + restart_sport + ;; + example) + stop_competitors + echo "" + echo "=== Mac 直连跑 example_position_pro.py ===" + conda run -n free_dog_sdk python tools/onboard/example_position_pro.py \ + --state data/lowlevel_captures/blowfish_state.bin \ + --max-steps 5000 + ;; + sin) + stop_competitors + echo "" + echo "=== Mac 直连跑 test_sin_leg.py ===" + conda run -n free_dog_sdk python tools/onboard/test_sin_leg.py \ + --amplitude 0.3 --freq 0.5 --duration 10 + ;; + monitor) + stop_competitors + echo "" + echo "=== Mac 直连只读监听 30 秒 ===" + conda run -n free_dog_sdk python tools/onboard/monitor_state.py \ + --state data/lowlevel_captures/blowfish_state.bin \ + --duration 30 --verbose + ;; + *) + echo "用法: bash run_on_mac.sh [example|sin|monitor|stop|start]" + exit 1 + ;; +esac + +echo "" +echo "============================================" +echo "测试结束. 恢复 sportMode:" +echo " bash tools/onboard/run_on_mac.sh start" +echo "============================================" diff --git a/tools/run_on_pi.sh b/tools/run_on_pi.sh new file mode 100644 index 0000000..a594b48 --- /dev/null +++ b/tools/run_on_pi.sh @@ -0,0 +1,104 @@ +#!/bin/bash +# 部署+执行测试 (在树莓派上, 源IP=192.168.123.161) +# +# 用法: +# bash tools/onboard/run_on_pi.sh sin # 单腿正弦 +# bash tools/onboard/run_on_pi.sh damping # 零力矩连通性 +# bash tools/onboard/run_on_pi.sh deploy # 仅部署不执行 +set -e + +PI_HOST="${PI_HOST:-192.168.123.161}" +PI_USER="${PI_USER:-pi}" +TEST="${1:-sin}" +REMOTE_DIR=/tmp/dog_test + +echo "=== 部署到 ${PI_USER}@${PI_HOST}:${REMOTE_DIR} ===" + +# 1. 创建远程目录结构 +ssh ${PI_USER}@${PI_HOST} "rm -rf ${REMOTE_DIR} && mkdir -p ${REMOTE_DIR}/ucl" + +# 2. 拷贝文件 (所有.py平铺在REMOTE_DIR, ucl/子目录放SDK文件) +echo "[1/2] 拷贝文件..." +scp tools/onboard/blowfish_codec.py \ + tools/onboard/build_pro_lowcmd.py \ + tools/onboard/test_sin_leg.py \ + tools/onboard/test_lowcmd.py \ + tools/onboard/example_position_pro.py \ + data/lowlevel_captures/blowfish_state.bin \ + ${PI_USER}@${PI_HOST}:${REMOTE_DIR}/ + +# 空 __init__.py 让 ucl 成为 package +ssh ${PI_USER}@${PI_HOST} "touch ${REMOTE_DIR}/ucl/__init__.py" +scp ucl/lowCmd.py ucl/lowState.py ucl/common.py ucl/complex.py ucl/enums.py \ + ${PI_USER}@${PI_HOST}:${REMOTE_DIR}/ucl/ + +# 提前杀掉 MCU:8007 的所有抢占者: +# 1. keep_sport_alive.sh - Legged_sport 看门狗 (每0.2秒重启Legged_sport, 必须先杀) +# 2. Legged_sport - 主控制循环 +# 3. appTransit - 也跟 MCU:8007 通信 +echo "" +echo "=== 停止 MCU:8007 抢占源 (顺序: 看门狗 → Legged_sport → appTransit) ===" +ssh ${PI_USER}@${PI_HOST} bash <<'EOF' || true +set +e +if sudo pkill -9 -f keep_sport_alive 2>/dev/null; then + echo ' ✅ keep_sport_alive.sh (看门狗)' +else + echo ' (无看门狗)' +fi +sleep 0.3 +if sudo pkill -9 -f Legged_sport 2>/dev/null; then + echo ' ✅ Legged_sport' +else + echo ' (无)' +fi +if sudo pkill -9 -f appTransit 2>/dev/null; then + echo ' ✅ appTransit' +else + echo ' (无)' +fi +sleep 0.5 +echo '--- 验证残留: ---' +if pgrep -fl 'Legged_sport|appTransit|keep_sport_alive' 2>/dev/null; then + echo ' ⚠️ 仍有残留, 可能看门狗在别处' +else + echo ' ✅ 全部清除' +fi +EOF +sleep 1 + +echo "" +echo "[2/2] 执行测试..." + +case "$TEST" in + sin) + echo "=== 单腿正弦测试 (Pi端) ===" + ssh -t ${PI_USER}@${PI_HOST} \ + "cd ${REMOTE_DIR} && python3 test_sin_leg.py --amplitude 0.3 --freq 0.5 --duration 10" + ;; + damping) + echo "=== 零力矩连通性测试 (Pi端) ===" + ssh -t ${PI_USER}@${PI_HOST} \ + "cd ${REMOTE_DIR} && python3 test_lowcmd.py --skip-stop --skip-mqtt" + ;; + example) + echo "=== example_position(lowlevel) PRO 版本 (Pi端) ===" + ssh -t ${PI_USER}@${PI_HOST} \ + "cd ${REMOTE_DIR} && python3 example_position_pro.py --max-steps 5000" + ;; + deploy) + echo "✅ 已部署到 ${PI_USER}@${PI_HOST}:${REMOTE_DIR}" + echo " ssh ${PI_USER}@${PI_HOST}" + echo " cd ${REMOTE_DIR} && python3 test_sin_leg.py ..." + ;; + *) + echo "用法: bash run_on_pi.sh [sin|damping|example|deploy]" + exit 1 + ;; +esac + +echo "" +echo "============================================" +echo "恢复 sportMode:" +echo " bash tools/onboard/stop_sportmode.sh start" +echo " 或 ssh ${PI_USER}@${PI_HOST} sudo systemctl start sportMode" +echo "============================================" diff --git a/tools/stop_sportmode.sh b/tools/stop_sportmode.sh new file mode 100755 index 0000000..308d8f6 --- /dev/null +++ b/tools/stop_sportmode.sh @@ -0,0 +1,121 @@ +#!/bin/bash +# 停止/启动 sportMode 服务 (从Mac SSH到狗执行) +# 用法: +# bash tools/onboard/stop_sportmode.sh stop # 停止sportMode,进入低层控制 +# bash tools/onboard/stop_sportmode.sh start # 重新启动sportMode +# bash tools/onboard/stop_sportmode.sh status # 检查状态 + +set -e + +PI_HOST="${PI_HOST:-192.168.123.161}" +PI_USER="${PI_USER:-pi}" + +# 等待用户按键继续的 pause 函数 +pause() { + echo "" + read -p "按 Enter 继续..." dummy +} + +cmd="${1:-status}" + +case "$cmd" in + stop) + echo "============================================" + echo "⚠️ 停止 sportMode — 进入低层控制" + echo "============================================" + echo "" + echo "这将会:" + echo " 1. 先发 damping 让电机失能" + echo " 2. 停止 Legged_sport 进程" + echo " 3. 停止后, :8007 将只接受你的 LowCmd" + echo "" + echo "⚠️ 停止后狗会软瘫, 确保:" + echo " - 狗趴在地上或脚架上" + echo " - 遥控器在手边 (L2+B 紧急停止)" + echo " - 10分钟内必须重启 (安全看门狗)" + echo "" + read -p "确认停止? (yes/no): " confirm + if [ "$confirm" != "yes" ]; then + echo "取消" + exit 0 + fi + + echo "" + echo "[1/3] 发送 damping ..." + ssh ${PI_USER}@${PI_HOST} \ + "mosquitto_pub -h localhost -t controller/action -m damping" 2>/dev/null || \ + echo " (MQTT可能不可用, 继续...)" + + sleep 2 + + echo "[2/3] 停止 Legged_sport 看门狗 + Legged_sport + appTransit ..." + # 必须按顺序: 先杀看门狗 keep_sport_alive.sh, 否则它每 0.2s 重拉 Legged_sport + # 同时杀 appTransit, 它也跟 MCU:8007 通信抢占 + ssh ${PI_USER}@${PI_HOST} \ + "sudo pkill -9 -f keep_sport_alive 2>/dev/null && echo ' ✅ keep_sport_alive (看门狗)' || echo ' (无看门狗)'; + sudo pkill -9 -f Legged_sport 2>/dev/null && echo ' ✅ Legged_sport' || echo ' (无)'; + sudo pkill -9 -f appTransit 2>/dev/null && echo ' ✅ appTransit' || echo ' (无)'; + sudo systemctl stop sportMode 2>/dev/null || true" + + sleep 1 + + echo "[3/3] 验证..." + if ssh ${PI_USER}@${PI_HOST} "! pgrep Legged_sport > /dev/null 2>&1"; then + echo " ✅ Legged_sport 已完全停止" + elif ssh ${PI_USER}@${PI_HOST} "pgrep Legged_sport > /dev/null 2>&1"; then + echo " ⚠️ Legged_sport 可能仍在运行(被STOP暂停)" + fi + + echo "" + echo "============================================" + echo "✅ sportMode 已停止, 现在可以做低层控制测试" + echo "" + echo "恢复: bash tools/onboard/stop_sportmode.sh start" + echo "============================================" + ;; + + start) + echo "============================================" + echo "🔄 恢复 sportMode" + echo "============================================" + echo "" + + echo "[1/2] 恢复 Legged_sport ..." + ssh ${PI_USER}@${PI_HOST} \ + "sudo systemctl restart sportMode 2>/dev/null || sudo systemctl restart legged_sport 2>/dev/null || (sudo killall -CONT Legged_sport 2>/dev/null) || echo '尝试手动启动'" && \ + echo " ✅ 重启命令已执行" || \ + echo " ⚠️ 重启失败, 尝试: sudo systemctl start sportMode 或重启狗" + + sleep 3 + + echo "[2/2] 验证..." + if ssh ${PI_USER}@${PI_HOST} "pgrep Legged_sport > /dev/null 2>&1"; then + echo " ✅ Legged_sport 已恢复运行" + else + echo " ⚠️ Legged_sport 未运行, 可能需要重启狗" + fi + + echo "" + echo "✅ sportMode 已恢复" + ;; + + status) + echo "检查 sportMode 状态..." + echo "" + echo "--- Legged_sport 进程 ---" + ssh ${PI_USER}@${PI_HOST} "ps aux | grep -v grep | grep Legged_sport || echo ' 未运行'" + echo "" + echo "--- systemd 服务 ---" + ssh ${PI_USER}@${PI_HOST} \ + "systemctl status sportMode 2>/dev/null || systemctl status Legged_sport 2>/dev/null || echo ' 服务名未知'" 2>/dev/null | head -10 + echo "" + echo "--- MQTT 状态 ---" + ssh ${PI_USER}@${PI_HOST} \ + "mosquitto_sub -h localhost -t programming/action -C 1 -W 2 2>/dev/null || echo ' (无消息/timeout)'" + ;; + + *) + echo "用法: bash stop_sportmode.sh [stop|start|status]" + exit 1 + ;; +esac