init pro_sdk
This commit is contained in:
cyy_mac
29
.gitignore
vendored
Normal file
29
.gitignore
vendored
Normal file
@@ -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
|
||||||
24
LICENSE
Normal file
24
LICENSE
Normal file
@@ -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.
|
||||||
129
README.md
Normal file
129
README.md
Normal file
@@ -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 一致)
|
||||||
142
docs/ARCHITECTURE.md
Normal file
142
docs/ARCHITECTURE.md
Normal file
@@ -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。
|
||||||
145
docs/PROTOCOL.md
Normal file
145
docs/PROTOCOL.md
Normal file
@@ -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 反复切换导致控制失败。
|
||||||
103
docs/REVERSE_ENGINEERING.md
Normal file
103
docs/REVERSE_ENGINEERING.md
Normal file
@@ -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 一致) |
|
||||||
66
docs/SAFETY.md
Normal file
66
docs/SAFETY.md
Normal file
@@ -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 (狗会软瘫倒地)
|
||||||
61
examples/README.md
Normal file
61
examples/README.md
Normal file
@@ -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`.
|
||||||
164
examples/example_position.py
Normal file
164
examples/example_position.py
Normal file
@@ -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)
|
||||||
100
examples/example_remote_control.py
Normal file
100
examples/example_remote_control.py
Normal file
@@ -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)
|
||||||
111
examples/example_sin_leg.py
Normal file
111
examples/example_sin_leg.py
Normal file
@@ -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)
|
||||||
78
examples/monitor_remote.py
Normal file
78
examples/monitor_remote.py
Normal file
@@ -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)
|
||||||
79
examples/monitor_state.py
Normal file
79
examples/monitor_state.py
Normal file
@@ -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)
|
||||||
68
go1_pro_sdk/__init__.py
Normal file
68
go1_pro_sdk/__init__.py
Normal file
@@ -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',
|
||||||
|
]
|
||||||
11
go1_pro_sdk/_data/README.md
Normal file
11
go1_pro_sdk/_data/README.md
Normal file
@@ -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 不一样 (固件升级后可能),
|
||||||
|
重新提取一份覆盖.
|
||||||
BIN
go1_pro_sdk/_data/blowfish_state.bin
Normal file
BIN
go1_pro_sdk/_data/blowfish_state.bin
Normal file
Binary file not shown.
10
go1_pro_sdk/codec/__init__.py
Normal file
10
go1_pro_sdk/codec/__init__.py
Normal file
@@ -0,0 +1,10 @@
|
|||||||
|
"""加解密 + LowCmd/LowState 序列化."""
|
||||||
|
from .blowfish import Blowfish
|
||||||
|
from .lowcmd_builder import build_low_cmd_plain, build_low_cmd_encrypted
|
||||||
|
from .lowstate_parser import parse_low_state
|
||||||
|
|
||||||
|
__all__ = [
|
||||||
|
'Blowfish',
|
||||||
|
'build_low_cmd_plain', 'build_low_cmd_encrypted',
|
||||||
|
'parse_low_state',
|
||||||
|
]
|
||||||
132
go1_pro_sdk/codec/blowfish.py
Normal file
132
go1_pro_sdk/codec/blowfish.py
Normal file
@@ -0,0 +1,132 @@
|
|||||||
|
"""Blowfish ECB 加解密 (使用从狗内存提取的 P-array + S-box).
|
||||||
|
|
||||||
|
跟标准 Blowfish 实现的区别:
|
||||||
|
- 不做 key schedule. 直接接受预先生成好的 4168 字节 state 文件 (P + S0..S3)
|
||||||
|
- 默认 LE 字节序 (Unitree ARM64 LDP 读取顺序), 也支持 BE 标准模式
|
||||||
|
- state 文件来源: tools/extract_blowfish_key.sh 从运行中 Legged_sport 内存 dump
|
||||||
|
|
||||||
|
完整 state 布局 (4168 B):
|
||||||
|
P[18] × 4B = 72B
|
||||||
|
S0[256] × 4B = 1024B
|
||||||
|
S1[256] × 4B = 1024B
|
||||||
|
S2[256] × 4B = 1024B
|
||||||
|
S3[256] × 4B = 1024B
|
||||||
|
合计 4168 B
|
||||||
|
|
||||||
|
已知明密文对 (供验证):
|
||||||
|
encrypt(0000000000000000) = 12aa0354236b66e3
|
||||||
|
encrypt(feefff0004020305) = 5d9874ad7eee5851
|
||||||
|
encrypt(0802010000010900) = c5ce0887ceb86f91
|
||||||
|
"""
|
||||||
|
import struct
|
||||||
|
from typing import Sequence
|
||||||
|
|
||||||
|
|
||||||
|
class Blowfish:
|
||||||
|
"""自定义 Blowfish: 接受预先初始化好的 P + S-box 直接加解密."""
|
||||||
|
|
||||||
|
def __init__(self, P: Sequence[int], S0: Sequence[int], S1: Sequence[int],
|
||||||
|
S2: Sequence[int], S3: Sequence[int], endian: str = 'little'):
|
||||||
|
"""
|
||||||
|
Args:
|
||||||
|
P: 18 个 uint32
|
||||||
|
S0..S3: 各 256 个 uint32
|
||||||
|
endian: 'little' (Unitree LE) 或 'big' (标准 Blowfish BE)
|
||||||
|
"""
|
||||||
|
assert len(P) == 18
|
||||||
|
assert len(S0) == 256 and len(S1) == 256 and len(S2) == 256 and len(S3) == 256
|
||||||
|
self.P = list(P)
|
||||||
|
self.S = [list(S0), list(S1), list(S2), list(S3)]
|
||||||
|
self.endian = endian
|
||||||
|
self._pack = '<II' if endian == 'little' else '>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
|
||||||
82
go1_pro_sdk/codec/lowcmd_builder.py
Normal file
82
go1_pro_sdk/codec/lowcmd_builder.py
Normal file
@@ -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
|
||||||
69
go1_pro_sdk/codec/lowstate_parser.py
Normal file
69
go1_pro_sdk/codec/lowstate_parser.py
Normal file
@@ -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
|
||||||
4
go1_pro_sdk/connection/__init__.py
Normal file
4
go1_pro_sdk/connection/__init__.py
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
"""高层 UDP 客户端 (隐藏 socket/Blowfish 细节)."""
|
||||||
|
from .mcu_client import MCUClient
|
||||||
|
|
||||||
|
__all__ = ['MCUClient']
|
||||||
145
go1_pro_sdk/connection/mcu_client.py
Normal file
145
go1_pro_sdk/connection/mcu_client.py
Normal file
@@ -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)
|
||||||
16
go1_pro_sdk/safety/__init__.py
Normal file
16
go1_pro_sdk/safety/__init__.py
Normal file
@@ -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',
|
||||||
|
]
|
||||||
115
go1_pro_sdk/safety/safety.py
Normal file
115
go1_pro_sdk/safety/safety.py
Normal file
@@ -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
|
||||||
14
go1_pro_sdk/types/__init__.py
Normal file
14
go1_pro_sdk/types/__init__.py
Normal file
@@ -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',
|
||||||
|
]
|
||||||
48
go1_pro_sdk/types/bms.py
Normal file
48
go1_pro_sdk/types/bms.py
Normal file
@@ -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('<i', data[4:8])[0]
|
||||||
|
bms.cycle = int.from_bytes(data[8:10], 'little')
|
||||||
|
bms.BQ_NTC = [data[10], data[11]]
|
||||||
|
bms.MCU_NTC = [data[12], data[13]]
|
||||||
|
# cell_vol 10 × uint16 LE
|
||||||
|
bms.cell_vol = [int.from_bytes(data[14 + i*2:14 + (i+1)*2], 'little') for i in range(10)]
|
||||||
|
return bms
|
||||||
31
go1_pro_sdk/types/imu.py
Normal file
31
go1_pro_sdk/types/imu.py
Normal file
@@ -0,0 +1,31 @@
|
|||||||
|
"""IMU 数据结构."""
|
||||||
|
import struct
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from typing import Tuple
|
||||||
|
from ..utils.common import hex_to_float
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class IMU:
|
||||||
|
"""惯性测量单元反馈."""
|
||||||
|
quaternion: Tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) # (w, x, y, z)
|
||||||
|
gyroscope: Tuple[float, float, float] = (0.0, 0.0, 0.0) # rad/s
|
||||||
|
accelerometer: Tuple[float, float, float] = (0.0, 0.0, 0.0) # m/s²
|
||||||
|
rpy: Tuple[float, float, float] = (0.0, 0.0, 0.0) # rad
|
||||||
|
temperature: int = 0
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_bytes(cls, data: bytes) -> '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
|
||||||
40
go1_pro_sdk/types/low_cmd.py
Normal file
40
go1_pro_sdk/types/low_cmd.py
Normal file
@@ -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
|
||||||
33
go1_pro_sdk/types/low_state.py
Normal file
33
go1_pro_sdk/types/low_state.py
Normal file
@@ -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)
|
||||||
83
go1_pro_sdk/types/motor.py
Normal file
83
go1_pro_sdk/types/motor.py
Normal file
@@ -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
|
||||||
54
go1_pro_sdk/types/remote.py
Normal file
54
go1_pro_sdk/types/remote.py
Normal file
@@ -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('<H', data40[2:4])[0]
|
||||||
|
rs.lx = struct.unpack('<f', data40[4:8])[0]
|
||||||
|
rs.rx = struct.unpack('<f', data40[8:12])[0]
|
||||||
|
rs.ry = struct.unpack('<f', data40[12:16])[0]
|
||||||
|
rs.L2 = struct.unpack('<f', data40[16:20])[0]
|
||||||
|
rs.ly = struct.unpack('<f', data40[20:24])[0]
|
||||||
|
rs.pressed = [name for mask, name in BUTTON_NAMES if rs.btn & mask]
|
||||||
|
return rs
|
||||||
30
go1_pro_sdk/utils/__init__.py
Normal file
30
go1_pro_sdk/utils/__init__.py
Normal file
@@ -0,0 +1,30 @@
|
|||||||
|
"""Utility functions and constants."""
|
||||||
|
from .common import (
|
||||||
|
gen_crc, float_to_hex, hex_to_float,
|
||||||
|
tau_to_hex, hex_to_tau,
|
||||||
|
kp_to_hex, hex_to_kp,
|
||||||
|
kd_to_hex, hex_to_kd,
|
||||||
|
decode_sn, decode_version,
|
||||||
|
)
|
||||||
|
from .constants import (
|
||||||
|
MCU_IP, MCU_PORT,
|
||||||
|
JOINT_NAMES, JOINT_TYPE, JOINT_ATTR,
|
||||||
|
JOINT_LIMITS, JOINT_LIMITS_MEASURED,
|
||||||
|
TAU_MAX, KT, CRITICAL_FACTOR,
|
||||||
|
DAMPING_POSE,
|
||||||
|
)
|
||||||
|
|
||||||
|
__all__ = [
|
||||||
|
# common
|
||||||
|
'gen_crc', 'float_to_hex', 'hex_to_float',
|
||||||
|
'tau_to_hex', 'hex_to_tau',
|
||||||
|
'kp_to_hex', 'hex_to_kp',
|
||||||
|
'kd_to_hex', 'hex_to_kd',
|
||||||
|
'decode_sn', 'decode_version',
|
||||||
|
# constants
|
||||||
|
'MCU_IP', 'MCU_PORT',
|
||||||
|
'JOINT_NAMES', 'JOINT_TYPE', 'JOINT_ATTR',
|
||||||
|
'JOINT_LIMITS', 'JOINT_LIMITS_MEASURED',
|
||||||
|
'TAU_MAX', 'KT', 'CRITICAL_FACTOR',
|
||||||
|
'DAMPING_POSE',
|
||||||
|
]
|
||||||
170
go1_pro_sdk/utils/common.py
Normal file
170
go1_pro_sdk/utils/common.py
Normal file
@@ -0,0 +1,170 @@
|
|||||||
|
"""通用编解码/工具函数 (基于 free-dog-sdk ucl/common.py 整理).
|
||||||
|
|
||||||
|
主要导出:
|
||||||
|
- CRC: gen_crc (PRO 用裸 CRC, 不 XOR)
|
||||||
|
- float ↔ Unitree 自定义 hex 字段: float_to_hex/hex_to_float
|
||||||
|
- 物理量 ↔ 字段: tau, Kp, Kd 各自的编码/解码
|
||||||
|
- SN/version 解码
|
||||||
|
|
||||||
|
来源: free-dog-sdk commit 0ececb5ed3b04c6bd88dbdc40c89db86bedc6543
|
||||||
|
作者: Bin4ry (Andreas Makris) — MIT License
|
||||||
|
"""
|
||||||
|
import struct
|
||||||
|
import binascii
|
||||||
|
|
||||||
|
|
||||||
|
# ===== CRC32 (用于 LowCmd 末尾 4 字节, PRO 不 XOR, EDU 才 XOR encryptCrc) =====
|
||||||
|
|
||||||
|
def gen_crc(data: bytes) -> 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('<I', crc)
|
||||||
|
|
||||||
|
|
||||||
|
# ===== float ↔ hex 字段 (Unitree 用 big-endian float 但小端整数读) =====
|
||||||
|
|
||||||
|
def float_to_hex(f: float) -> 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)
|
||||||
90
go1_pro_sdk/utils/constants.py
Normal file
90
go1_pro_sdk/utils/constants.py
Normal file
@@ -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, 小=自动丢弃积压
|
||||||
40
pyproject.toml
Normal file
40
pyproject.toml
Normal file
@@ -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"]
|
||||||
48
tests/test_blowfish.py
Normal file
48
tests/test_blowfish.py
Normal file
@@ -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)
|
||||||
94
tests/test_lowcmd_builder.py
Normal file
94
tests/test_lowcmd_builder.py
Normal file
@@ -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
|
||||||
82
tests/test_safety.py
Normal file
82
tests/test_safety.py
Normal file
@@ -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 # 在限位内, 不变
|
||||||
79
tools/README.md
Normal file
79
tools/README.md
Normal file
@@ -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
|
||||||
|
```
|
||||||
212
tools/_compare_lowcmd.py
Normal file
212
tools/_compare_lowcmd.py
Normal file
@@ -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('<I', gh[:4])[0]
|
||||||
|
endian = '<' if magic == 0xa1b2c3d4 else '>'
|
||||||
|
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())
|
||||||
121
tools/_raw_capture.py
Normal file
121
tools/_raw_capture.py
Normal file
@@ -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('<IHHIIII',
|
||||||
|
PCAP_MAGIC, 2, 4, 0, 0, 65535, LINKTYPE_ETHERNET))
|
||||||
|
|
||||||
|
|
||||||
|
def write_pcap_packet(f, ts, pkt):
|
||||||
|
ts_sec = int(ts)
|
||||||
|
ts_usec = int((ts - ts_sec) * 1e6)
|
||||||
|
f.write(struct.pack('<IIII', ts_sec, ts_usec, len(pkt), len(pkt)))
|
||||||
|
f.write(pkt)
|
||||||
|
|
||||||
|
|
||||||
|
def parse_packet(pkt):
|
||||||
|
"""返回 (src_ip, dst_ip, proto, src_port, dst_port) 或 None"""
|
||||||
|
if len(pkt) < 14: return None
|
||||||
|
eth_type = struct.unpack('>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()
|
||||||
310
tools/analyze_blowfish_dump.py
Normal file
310
tools/analyze_blowfish_dump.py
Normal file
@@ -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('<I', S0_INITIAL_FIRST))
|
||||||
|
s1_pos = find_pattern_locations(data, struct.pack('<I', S1_INITIAL_FIRST))
|
||||||
|
print(f" S0初始首字位置数: {len(s0_pos)}")
|
||||||
|
print(f" S1初始首字位置数: {len(s1_pos)}")
|
||||||
|
|
||||||
|
if s0_pos:
|
||||||
|
# 这些是静态初始值,在二进制中也有 (说明是从binary mmap过来的)
|
||||||
|
for p in s0_pos[:3]:
|
||||||
|
print(f" 初始S0@0x{p:x}")
|
||||||
|
|
||||||
|
# 策略2: 暴力搜索 - 假设 4168字节连续Blowfish state
|
||||||
|
# 每4字节对齐位置都试
|
||||||
|
print(f" 暴力搜索 Blowfish state (4字节对齐)...")
|
||||||
|
found = []
|
||||||
|
|
||||||
|
STEP = 4
|
||||||
|
count = 0
|
||||||
|
total = (len(data) - 4168) // STEP
|
||||||
|
|
||||||
|
for offset in range(0, len(data) - 4168, STEP):
|
||||||
|
count += 1
|
||||||
|
if count % 50000 == 0:
|
||||||
|
print(f" 进度: {count}/{total} ({100*count/total:.1f}%)")
|
||||||
|
|
||||||
|
# 快速过滤: 必须looks_like
|
||||||
|
if not looks_like_blowfish_buffer(data, offset):
|
||||||
|
continue
|
||||||
|
|
||||||
|
candidates_looking_like_bf.append(offset)
|
||||||
|
if save_candidates and len(candidates_looking_like_bf) <= 20:
|
||||||
|
cand_path = f'data/lowlevel_captures/candidate_{source_name}_{offset:08x}.bin'
|
||||||
|
os.makedirs('data/lowlevel_captures', exist_ok=True)
|
||||||
|
with open(cand_path, 'wb') as fp:
|
||||||
|
fp.write(data[offset:offset+4168])
|
||||||
|
print(f" 💾 保存候选 -> {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())
|
||||||
99
tools/calibrate_joints.py
Normal file
99
tools/calibrate_joints.py
Normal file
@@ -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)
|
||||||
73
tools/capture_lowcmd.sh
Normal file
73
tools/capture_lowcmd.sh
Normal file
@@ -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 <<EOF || true
|
||||||
|
set +e
|
||||||
|
sudo rm -f ${PCAP_REMOTE}
|
||||||
|
sudo timeout ${DURATION} /usr/sbin/tcpdump -i eth0 -w ${PCAP_REMOTE} -s 0 -n \\
|
||||||
|
'udp and host 192.168.123.10 and port 8007' 2>/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}
|
||||||
76
tools/diff_real_lowcmd.sh
Normal file
76
tools/diff_real_lowcmd.sh
Normal file
@@ -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_<ts>.pcap - 抓的原包
|
||||||
|
# data/lowlevel_captures/real_lowcmd_<ts>.bin - 一帧密文 (616B)
|
||||||
|
# data/lowlevel_captures/real_lowcmd_dec_<ts>.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 <<EOF
|
||||||
|
set +e
|
||||||
|
sudo rm -f ${PCAP_REMOTE}
|
||||||
|
sudo timeout ${DURATION} /usr/sbin/tcpdump -i any -w ${PCAP_REMOTE} -s 0 -n \
|
||||||
|
'udp and dst host 192.168.123.10 and dst port 8007' 2>/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 "============================================"
|
||||||
208
tools/extract_blowfish_key.sh
Normal file
208
tools/extract_blowfish_key.sh
Normal file
@@ -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/"
|
||||||
83
tools/run_on_mac.sh
Normal file
83
tools/run_on_mac.sh
Normal file
@@ -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 "============================================"
|
||||||
104
tools/run_on_pi.sh
Normal file
104
tools/run_on_pi.sh
Normal file
@@ -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 "============================================"
|
||||||
121
tools/stop_sportmode.sh
Executable file
121
tools/stop_sportmode.sh
Executable file
@@ -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
|
||||||
Reference in New Issue
Block a user