From dbfcb955663a1ebc18a6a1cb8c53702f1ee21f63 Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Wed, 29 Jul 2026 21:50:15 +0800 Subject: [PATCH] =?UTF-8?q?python=E9=83=A8=E5=88=86=E5=AE=98=E6=96=B9?= =?UTF-8?q?=E5=8C=96=E5=AF=B9=E9=BD=90=E6=8E=A5=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 23 ++ docs/OFFICIAL_API_COMPATIBILITY.md | 89 +++++++ go1_pro_sdk/__init__.py | 13 +- go1_pro_sdk/codec/lowcmd_builder.py | 45 +++- go1_pro_sdk/compat/__init__.py | 4 + go1_pro_sdk/compat/robot_interface.py | 328 ++++++++++++++++++++++++++ go1_pro_sdk/safety/safety.py | 6 +- go1_pro_sdk/types/__init__.py | 4 +- go1_pro_sdk/types/bms.py | 13 +- go1_pro_sdk/types/low_cmd.py | 3 + go1_pro_sdk/utils/__init__.py | 9 +- go1_pro_sdk/utils/constants.py | 9 + pyproject.toml | 2 + robot_interface.py | 4 + tests/test_official_compat.py | 140 +++++++++++ 15 files changed, 678 insertions(+), 14 deletions(-) create mode 100644 docs/OFFICIAL_API_COMPATIBILITY.md create mode 100644 go1_pro_sdk/compat/__init__.py create mode 100644 go1_pro_sdk/compat/robot_interface.py create mode 100644 robot_interface.py create mode 100644 tests/test_official_compat.py diff --git a/README.md b/README.md index edca214..4d98257 100644 --- a/README.md +++ b/README.md @@ -16,6 +16,29 @@ Unitree Go1 **PRO** 机型的低层电机控制 Python SDK。完整逆向 PRO ## 快速上手 +### 官方 Python SDK 兼容接口 + +需要复用官方 `unitree_legged_sdk` Python 示例时，可以继续使用原来的模块名和 +调用顺序： + +```python +import robot_interface as sdk + +udp = sdk.UDP(sdk.LOWLEVEL, 8080, "192.168.123.10", 8007) +safe = sdk.Safety(sdk.LeggedType.Go1) +cmd, state = sdk.LowCmd(), sdk.LowState() +udp.InitCmdData(cmd) + +udp.Recv() +udp.GetRecv(state) +safe.PowerProtect(cmd, state, 1) +udp.SetSend(cmd) +udp.Send() +``` + +兼容范围和协议差异见 [`docs/OFFICIAL_API_COMPATIBILITY.md`](docs/OFFICIAL_API_COMPATIBILITY.md)。 +官方 `HighCmd/HighState` UDP 通道和 C++ ABI 当前不在兼容范围内。 + ### 高层控制 (走/跳/姿态, 通过 sportMode 系统) ```python diff --git a/docs/OFFICIAL_API_COMPATIBILITY.md b/docs/OFFICIAL_API_COMPATIBILITY.md new file mode 100644 index 0000000..c6335c2 --- /dev/null +++ b/docs/OFFICIAL_API_COMPATIBILITY.md @@ -0,0 +1,89 @@ +# Unitree 官方 SDK 接口兼容说明 + +对比基准：Unitree [`unitree_legged_sdk`](https://github.com/unitreerobotics/unitree_legged_sdk) +`go1` 分支，README 版本 `v3.8.6`，提交 +`4539a6c10dfbc9781cea6fcb7d51bc6ddc6f71e1`（2023-07-11）。 + +## 结论 + +本项目和官方 SDK 使用相近的低层对象模型，但此前不是调用兼容：本项目入口是 +`MCUClient.send()/recv_state()`，官方 Python wrapper 入口是 +`robot_interface.UDP.SetSend()/Send()/Recv()/GetRecv()`。 + +当前项目已提供 `robot_interface` 兼容模块。官方低层 Python 示例可保留原来的 +导入和主要调用流程，同时底层会使用 Go1 PRO 所需的 Blowfish 加密和 616 字节 +私有 `LowCmd` 协议。 + +## 接口矩阵 + +| 官方接口 | 本项目原接口 | 当前兼容状态 | +|---|---|---| +| `robot_interface.LowCmd/LowState` | 同名类型 | 已对齐命令和常用状态字段名 | +| `MotorCmd/MotorState/IMU` | 同名类型 | 常用字段已对齐 | +| `BmsState` | `BMS` | 名称已对齐；PRO 状态区较短，见下文 | +| `BmsCmd`, `Cartesian`, `UDPState` | 原先缺失 | 已补齐 | +| `UDP(LOWLEVEL, ...)` | `MCUClient(...)` | 已适配标准四参数构造 | +| `InitCmdData` | 原先缺失 | 已实现官方 stop sentinel 初始化 | +| `Recv/GetRecv/SetSend/Send` | `recv_latest/send` | 已适配 | +| `Safety.PositionLimit` | `position_limit` | 已适配，跳过 `PosStopF` | +| `Safety.PowerProtect` | `power_protect` | 调用兼容，算法不是官方闭源实现 | +| `Safety.PositionProtect` | `position_protect` | 调用兼容，保护动作更保守 | +| `HighCmd/HighState` | MQTT `Go1/Go1MQTT` | 类型已提供，UDP 传输不支持 | +| `Loop/LoopFunc` | 无 | 官方 Python wrapper 本身也未导出 | +| C++ headers/static library | 无 | 不兼容 C++ 源码或 ABI | + +## 关键协议差异 + +官方 SDK 的公开结构不能直接描述本项目实测的 Go1 PRO 线协议。PRO 低层通道需要： + +- 616 字节 `LowCmd`，CRC 位于 612； +- Blowfish ECB 加密； +- `bandWidth` 使用实测字节序和值； +- 858 字节加密状态包中取 856 字节块解密，再解析 807 字节有效状态。 + +因此兼容层只对齐用户代码接口，不替换本项目现有 codec，也不承诺二进制包与官方 +EDU SDK 相同。 + +## 官方风格用法 + +```python +import robot_interface as sdk + +udp = sdk.UDP(sdk.LOWLEVEL, 8080, "192.168.123.10", 8007) +safe = sdk.Safety(sdk.LeggedType.Go1) +cmd = sdk.LowCmd() +state = sdk.LowState() +udp.InitCmdData(cmd) + +udp.Recv() +udp.GetRecv(state) + +cmd.motorCmd[sdk.FR_1].q = 1.2 +cmd.motorCmd[sdk.FR_1].dq = 0.0 +cmd.motorCmd[sdk.FR_1].Kp = 5.0 +cmd.motorCmd[sdk.FR_1].Kd = 1.0 +safe.PositionLimit(cmd) +safe.PowerProtect(cmd, state, 1) +udp.SetSend(cmd) +udp.Send() +``` + +`UDP(HIGHLEVEL, ..., "192.168.123.161", 8082)` 不会被静默映射到 MQTT。两种通道 +状态模型和时序不同，兼容层会明确抛出 `NotImplementedError`。高层控制继续使用 +`Go1` 或 `Go1MQTT`。 + +## 尚未完全对齐的风险 + +- 官方 `PowerProtect` 实现在静态库中，头文件只公开签名。本项目当前保护逻辑按 + 关节力矩限幅和实测过载执行，不等价于官方内部的累计功率算法。 +- 官方 `BmsState` 是 34 字节，本项目实测的 PRO 状态布局只给 BMS 分配 24 字节； + `cell_vol` 后半部分不能视为完整的官方数据。 +- `LowState.tick` 当前没有从 PRO 包中解析，仍为默认值；`footForce` 和 + `footForceEst` 的偏移来自逆向推断，尚未达到官方字段的同等可信度。 +- `head/SN/version/reserve/crc` 在本地原生类型中部分使用 `bytes`，官方 wrapper + 使用定长整数数组或整数。命令 builder 接受两种常见赋值形态，但读取后的 Python + 类型不保证完全相同。 +- 本项目 `LowState` 是 PRO 抓包格式的解释，不应按官方 `#pragma pack(1)` 结构大小 + 直接做内存映射。 +- 官方 C++ 用户需要单独的 C++ facade 和库链接方案；Python 兼容模块不能让现有 + C++ 程序直接重编译通过。 diff --git a/go1_pro_sdk/__init__.py b/go1_pro_sdk/__init__.py index 8ea9caa..1178ae5 100644 --- a/go1_pro_sdk/__init__.py +++ b/go1_pro_sdk/__init__.py @@ -30,7 +30,7 @@ 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, + IMU, BMS, BmsCmd, BmsState, RemoteState, parse_remote, BUTTON_NAMES, LowState, LowCmd, ) @@ -39,7 +39,10 @@ from .safety import ( PowerProtectViolation, ) from .utils import ( - MCU_IP, MCU_PORT, + MCU_IP, MCU_PORT, HIGHLEVEL, LOWLEVEL, TRIGERLEVEL, PosStopF, VelStopF, + FR_, FL_, RR_, RL_, + FR_0, FR_1, FR_2, FL_0, FL_1, FL_2, + RR_0, RR_1, RR_2, RL_0, RL_1, RL_2, JOINT_NAMES, JOINT_TYPE, JOINT_LIMITS, JOINT_LIMITS_MEASURED, TAU_MAX, KT, DAMPING_POSE, decode_sn, decode_version, @@ -54,7 +57,7 @@ __all__ = [ 'MCUClient', # 数据结构 'MotorCmd', 'MotorState', 'MotorMode', - 'IMU', 'BMS', + 'IMU', 'BMS', 'BmsCmd', 'BmsState', 'RemoteState', 'parse_remote', 'BUTTON_NAMES', 'LowState', 'LowCmd', # 编解码 @@ -65,6 +68,10 @@ __all__ = [ 'PowerProtectViolation', # 常量 'MCU_IP', 'MCU_PORT', + 'HIGHLEVEL', 'LOWLEVEL', 'TRIGERLEVEL', 'PosStopF', 'VelStopF', + 'FR_', 'FL_', 'RR_', 'RL_', + 'FR_0', 'FR_1', 'FR_2', 'FL_0', 'FL_1', 'FL_2', + 'RR_0', 'RR_1', 'RR_2', 'RL_0', 'RL_1', 'RL_2', 'JOINT_NAMES', 'JOINT_TYPE', 'JOINT_LIMITS', 'JOINT_LIMITS_MEASURED', 'TAU_MAX', 'KT', 'DAMPING_POSE', 'decode_sn', 'decode_version', diff --git a/go1_pro_sdk/codec/lowcmd_builder.py b/go1_pro_sdk/codec/lowcmd_builder.py index 9825fbe..d9dcc5d 100644 --- a/go1_pro_sdk/codec/lowcmd_builder.py +++ b/go1_pro_sdk/codec/lowcmd_builder.py @@ -33,34 +33,67 @@ from ..utils.common import gen_crc from .blowfish import Blowfish +def _pack_bytes(value, size: int, field_name: str) -> bytes: + packed = bytes(value) + if len(packed) != size: + raise ValueError(f'{field_name} 必须是 {size} 字节, got {len(packed)}') + return packed + + +def _pack_u32_pair(value, field_name: str) -> bytes: + if isinstance(value, (bytes, bytearray, memoryview)): + packed = bytes(value) + else: + try: + packed = struct.pack('<2I', *(int(item) for item in value)) + except (TypeError, ValueError, struct.error) as exc: + raise ValueError(f'{field_name} 必须是 8 字节或 2 个 uint32') from exc + if len(packed) != 8: + raise ValueError(f'{field_name} 必须是 8 字节, got {len(packed)}') + return packed + + +def _pack_u32(value, field_name: str) -> bytes: + if isinstance(value, int): + return struct.pack(' bytes: """构造 PRO 格式 616B 明文 LowCmd.""" cmd = bytearray(616) # 0..2 head - cmd[0:2] = lowcmd.head + cmd[0:2] = _pack_bytes(lowcmd.head, 2, 'head') # 2..3 levelFlag cmd[2] = lowcmd.levelFlag # 3..4 frameReserve cmd[3] = lowcmd.frameReserve # 4..12 SN - cmd[4:12] = lowcmd.SN + cmd[4:12] = _pack_u32_pair(lowcmd.SN, 'SN') # 12..20 version - cmd[12:20] = lowcmd.version + cmd[12:20] = _pack_u32_pair(lowcmd.version, 'version') # 20..22 bandWidth BE cmd[20:22] = struct.pack('>H', lowcmd.bandWidth) # 22..562 motorCmd[20] × 27B + if len(lowcmd.motorCmd) != 20: + raise ValueError(f'motorCmd 必须有 20 项, got {len(lowcmd.motorCmd)}') offset = 22 for motor in lowcmd.motorCmd: motor_bytes = motor.to_bytes() assert len(motor_bytes) == 27, f'motorCmd 序列化长度错: {len(motor_bytes)}' cmd[offset:offset + 27] = motor_bytes offset += 27 - # 562..566 bms (默认 0) + # 562..566 bms + cmd[562] = int(lowcmd.bms.off) & 0xff + cmd[563:566] = _pack_bytes(lowcmd.bms.reserve, 3, 'bms.reserve') # 566..606 wirelessRemote - cmd[566:606] = lowcmd.wirelessRemote + cmd[566:606] = _pack_bytes(lowcmd.wirelessRemote, 40, 'wirelessRemote') # 606..610 reserve - cmd[606:610] = lowcmd.reserve + cmd[606:610] = _pack_u32(lowcmd.reserve, 'reserve') # 610..612 固定填充 0x0000 # 612..616 CRC cmd[612:616] = gen_crc(memoryview(cmd)[:612]) diff --git a/go1_pro_sdk/compat/__init__.py b/go1_pro_sdk/compat/__init__.py new file mode 100644 index 0000000..10bda0f --- /dev/null +++ b/go1_pro_sdk/compat/__init__.py @@ -0,0 +1,4 @@ +"""Compatibility APIs for third-party Unitree SDK code.""" + +from .robot_interface import * +from .robot_interface import __all__ diff --git a/go1_pro_sdk/compat/robot_interface.py b/go1_pro_sdk/compat/robot_interface.py new file mode 100644 index 0000000..a2e0436 --- /dev/null +++ b/go1_pro_sdk/compat/robot_interface.py @@ -0,0 +1,328 @@ +"""Compatibility surface for Unitree's ``robot_interface`` Python module. + +Only Go1 PRO low-level transport is implemented. The public API mirrors the +official v3.8.6 Python wrapper while the wire codec remains the PRO-specific +encrypted protocol implemented by this project. +""" + +from copy import deepcopy +from dataclasses import dataclass, field, fields, is_dataclass +from enum import IntEnum +from typing import List, Optional + +from ..connection.mcu_client import MCUClient +from ..safety import position_limit, position_protect, power_protect +from ..safety import PowerProtectViolation +from ..types import BmsCmd, BmsState, IMU, LowCmd, LowState, MotorCmd, MotorState +from ..utils.constants import ( + HIGHLEVEL, LOWLEVEL, TRIGERLEVEL, PosStopF, VelStopF, + MCU_IP, MCU_PORT, + FR_, FL_, RR_, RL_, + FR_0, FR_1, FR_2, FL_0, FL_1, FL_2, + RR_0, RR_1, RR_2, RL_0, RL_1, RL_2, +) + + +class LeggedType(IntEnum): + Aliengo = 0 + A1 = 1 + Go1 = 2 + B1 = 3 + + +Aliengo = LeggedType.Aliengo +A1 = LeggedType.A1 +Go1 = LeggedType.Go1 +B1 = LeggedType.B1 + + +class RecvEnum(IntEnum): + nonBlock = 0x00 + block = 0x01 + blockTimeout = 0x02 + + +nonBlock = RecvEnum.nonBlock +block = RecvEnum.block +blockTimeout = RecvEnum.blockTimeout + + +@dataclass +class Cartesian: + x: float = 0.0 + y: float = 0.0 + z: float = 0.0 + + +@dataclass +class LED: + r: int = 0 + g: int = 0 + b: int = 0 + + +@dataclass +class HighState: + head: List[int] = field(default_factory=lambda: [0, 0]) + levelFlag: int = 0 + frameReserve: int = 0 + SN: List[int] = field(default_factory=lambda: [0, 0]) + version: List[int] = field(default_factory=lambda: [0, 0]) + bandWidth: int = 0 + imu: IMU = field(default_factory=IMU) + motorState: List[MotorState] = field(default_factory=lambda: [MotorState() for _ in range(20)]) + bms: BmsState = field(default_factory=BmsState) + footForce: List[int] = field(default_factory=lambda: [0] * 4) + footForceEst: List[int] = field(default_factory=lambda: [0] * 4) + mode: int = 0 + progress: float = 0.0 + gaitType: int = 0 + footRaiseHeight: float = 0.0 + position: List[float] = field(default_factory=lambda: [0.0] * 3) + bodyHeight: float = 0.0 + velocity: List[float] = field(default_factory=lambda: [0.0] * 3) + yawSpeed: float = 0.0 + rangeObstacle: List[float] = field(default_factory=lambda: [0.0] * 4) + footPosition2Body: List[Cartesian] = field(default_factory=lambda: [Cartesian() for _ in range(4)]) + footSpeed2Body: List[Cartesian] = field(default_factory=lambda: [Cartesian() for _ in range(4)]) + wirelessRemote: List[int] = field(default_factory=lambda: [0] * 40) + reserve: int = 0 + crc: int = 0 + + +@dataclass +class HighCmd: + head: List[int] = field(default_factory=lambda: [0, 0]) + levelFlag: int = 0 + frameReserve: int = 0 + SN: List[int] = field(default_factory=lambda: [0, 0]) + version: List[int] = field(default_factory=lambda: [0, 0]) + bandWidth: int = 0 + mode: int = 0 + gaitType: int = 0 + speedLevel: int = 0 + footRaiseHeight: float = 0.0 + bodyHeight: float = 0.0 + position: List[float] = field(default_factory=lambda: [0.0] * 2) + euler: List[float] = field(default_factory=lambda: [0.0] * 3) + velocity: List[float] = field(default_factory=lambda: [0.0] * 2) + yawSpeed: float = 0.0 + bms: BmsCmd = field(default_factory=BmsCmd) + led: List[LED] = field(default_factory=lambda: [LED() for _ in range(4)]) + wirelessRemote: List[int] = field(default_factory=lambda: [0] * 40) + reserve: int = 0 + crc: int = 0 + + +@dataclass +class UDPState: + TotalCount: int = 0 + SendCount: int = 0 + RecvCount: int = 0 + SendError: int = 0 + FlagError: int = 0 + RecvCRCError: int = 0 + RecvLoseError: int = 0 + + +def _copy_object(source, target) -> None: + if is_dataclass(target): + names = (item.name for item in fields(target)) + else: + names = vars(source).keys() + for name in names: + if hasattr(source, name): + setattr(target, name, deepcopy(getattr(source, name))) + + +class UDP: + """Official-style UDP facade backed by :class:`MCUClient`. + + The standard ``UDP(level, localPort, targetIP, targetPort)`` constructor is + supported for ``LOWLEVEL``. Custom packet-length constructors and the + official high-level UDP protocol are intentionally not emulated. + """ + + def __init__(self, *args): + if len(args) >= 4 and isinstance(args[2], str): + self.level = int(args[0]) + self.localPort = int(args[1]) + self.targetIP = args[2] + self.targetPort = int(args[3]) + self._recv_type = RecvEnum.nonBlock + self._custom_lengths = False + elif len(args) >= 5 and isinstance(args[1], str): + self.level = None + self.localPort = int(args[0]) + self.targetIP = args[1] + self.targetPort = int(args[2]) + self._recv_type = RecvEnum(args[6]) if len(args) > 6 else RecvEnum.nonBlock + self._custom_lengths = True + elif len(args) >= 3: + self.level = None + self.localPort = int(args[0]) + self.targetIP = MCU_IP + self.targetPort = MCU_PORT + self._recv_type = RecvEnum(args[4]) if len(args) > 4 else RecvEnum.nonBlock + self._custom_lengths = True + else: + raise TypeError("UDP expects one of the three official constructor signatures") + + self.localIP = "0.0.0.0" + self.accessible = False + self.udpState = UDPState() + self._client: Optional[MCUClient] = None + self._send_cmd: Optional[LowCmd] = None + self._recv_state: Optional[LowState] = None + self._recv_timeout_ms = 1000 + self._disconnect_time = None + self._accessible_time = None + + def __enter__(self): + return self + + def __exit__(self, *_): + self.close() + + def close(self): + if self._client is not None: + self._client.close() + self._client = None + + def _ensure_client(self) -> MCUClient: + if self.level == HIGHLEVEL: + raise NotImplementedError( + "HighCmd/HighState UDP (:8082) is not implemented by this " + "compatibility layer; use go1_pro_sdk.Go1 or Go1MQTT" + ) + if self.level not in (LOWLEVEL, None) or self._custom_lengths: + raise NotImplementedError("only the official LOWLEVEL constructor is supported") + if self._client is None: + self._client = MCUClient( + mcu_ip=self.targetIP, + mcu_port=self.targetPort, + local_port=self.localPort, + ) + self.localPort = self._client.local_port + return self._client + + def SetIpPort(self, targetIP, targetPort): + if self._client is not None: + raise RuntimeError("SetIpPort must be called before the first Send/Recv") + self.targetIP = str(targetIP) + self.targetPort = int(targetPort) + + def SetRecvTimeout(self, time): + self._recv_timeout_ms = max(0, int(time)) + + def SetDisconnectTime(self, callback_dt, disconnectTime): + self._disconnect_time = (float(callback_dt), float(disconnectTime)) + + def SetAccessibleTime(self, callback_dt, accessibleTime): + self._accessible_time = (float(callback_dt), float(accessibleTime)) + + def InitCmdData(self, cmd): + if isinstance(cmd, HighCmd): + _copy_object(HighCmd(levelFlag=HIGHLEVEL), cmd) + return None + if not isinstance(cmd, LowCmd): + raise TypeError("InitCmdData expects LowCmd or HighCmd") + + template = LowCmd(levelFlag=LOWLEVEL) + template.motorCmd = [ + MotorCmd(mode=0x0A, q=PosStopF, dq=VelStopF) + for _ in range(20) + ] + _copy_object(template, cmd) + return None + + def SetSend(self, cmd): + if isinstance(cmd, HighCmd): + self._send_cmd = deepcopy(cmd) + return 0 + if not isinstance(cmd, LowCmd): + raise TypeError("SetSend expects LowCmd or HighCmd") + self._send_cmd = deepcopy(cmd) + return 0 + + def Send(self): + if self._send_cmd is None: + raise RuntimeError("call InitCmdData and SetSend before Send") + if isinstance(self._send_cmd, HighCmd): + self._ensure_client() + self.udpState.TotalCount += 1 + try: + sent = self._ensure_client().send(self._send_cmd) + except Exception: + self.udpState.SendError += 1 + raise + self.udpState.SendCount += 1 + return sent + + def Recv(self): + client = self._ensure_client() + self.udpState.TotalCount += 1 + if self._recv_type == RecvEnum.nonBlock: + state = client.recv_latest() + else: + timeout = self._recv_timeout_ms / 1000.0 + state = client.recv_state(timeout=timeout) + if state is None: + return 0 + self._recv_state = state + self.accessible = True + self.udpState.RecvCount += 1 + return 858 + + def GetRecv(self, state): + if isinstance(state, HighState): + self._ensure_client() + if not isinstance(state, LowState): + raise TypeError("GetRecv expects LowState or HighState") + if self._recv_state is not None: + _copy_object(self._recv_state, state) + return None + + +class Safety: + """Official method names with conservative local safety behavior.""" + + def __init__(self, legged_type): + self.legged_type = LeggedType(legged_type) + if self.legged_type != LeggedType.Go1: + raise ValueError("go1_pro_sdk only supports LeggedType.Go1") + + def PositionLimit(self, lowcmd): + position_limit(lowcmd) + return None + + def PowerProtect(self, lowcmd, lowstate, factor): + try: + power_protect(lowcmd, lowstate, factor) + except PowerProtectViolation: + return -1 + return 0 + + def PositionProtect(self, lowcmd, lowstate, limit=0.087): + return -1 if position_protect(lowcmd, lowstate, limit) else 0 + + +def VersionSDK(): + return "go1_pro_sdk robot_interface compatibility (official API v3.8.6)" + + +def InitEnvironment(): + return 0 + + +__all__ = [ + "HIGHLEVEL", "LOWLEVEL", "TRIGERLEVEL", "PosStopF", "VelStopF", + "FR_", "FL_", "RR_", "RL_", + "FR_0", "FR_1", "FR_2", "FL_0", "FL_1", "FL_2", + "RR_0", "RR_1", "RR_2", "RL_0", "RL_1", "RL_2", + "LeggedType", "Aliengo", "A1", "Go1", "B1", + "RecvEnum", "nonBlock", "block", "blockTimeout", + "UDP", "Safety", "BmsCmd", "BmsState", "Cartesian", "IMU", "LED", + "MotorState", "MotorCmd", "LowState", "LowCmd", "HighState", "HighCmd", + "UDPState", "VersionSDK", "InitEnvironment", +] diff --git a/go1_pro_sdk/safety/safety.py b/go1_pro_sdk/safety/safety.py index a340295..5be025c 100644 --- a/go1_pro_sdk/safety/safety.py +++ b/go1_pro_sdk/safety/safety.py @@ -7,7 +7,7 @@ """ from typing import Optional, TYPE_CHECKING from ..utils.constants import ( - JOINT_LIMITS, JOINT_NAMES, JOINT_TYPE, TAU_MAX, CRITICAL_FACTOR, + JOINT_LIMITS, JOINT_NAMES, JOINT_TYPE, TAU_MAX, CRITICAL_FACTOR, PosStopF, ) if TYPE_CHECKING: @@ -26,6 +26,8 @@ def position_limit(lowcmd: 'LowCmd') -> int: jt = JOINT_TYPE[i] lo, hi = JOINT_LIMITS[jt] m = lowcmd.motorCmd[i] + if abs(m.q) >= PosStopF * 0.1: + continue if m.q < lo: m.q = lo; clamped += 1 elif m.q > hi: @@ -78,6 +80,8 @@ def position_protect(lowcmd: 'LowCmd', lstate: 'LowState', limit_rad: float) -> affected = 0 for i in range(12): m = lowcmd.motorCmd[i] + if abs(m.q) >= PosStopF * 0.1: + continue try: actual_q = lstate.motorState[i].q except (AttributeError, IndexError): diff --git a/go1_pro_sdk/types/__init__.py b/go1_pro_sdk/types/__init__.py index 9dab98c..8cceb1a 100644 --- a/go1_pro_sdk/types/__init__.py +++ b/go1_pro_sdk/types/__init__.py @@ -1,14 +1,14 @@ """数据结构 (MotorCmd, MotorState, LowState, LowCmd, IMU, BMS, Remote).""" from .motor import MotorCmd, MotorState, MotorMode from .imu import IMU -from .bms import BMS +from .bms import BMS, BmsCmd, BmsState from .remote import RemoteState, parse_remote, BUTTON_NAMES from .low_state import LowState from .low_cmd import LowCmd __all__ = [ 'MotorCmd', 'MotorState', 'MotorMode', - 'IMU', 'BMS', + 'IMU', 'BMS', 'BmsCmd', 'BmsState', 'RemoteState', 'parse_remote', 'BUTTON_NAMES', 'LowState', 'LowCmd', ] diff --git a/go1_pro_sdk/types/bms.py b/go1_pro_sdk/types/bms.py index 82dd0cc..255370e 100644 --- a/go1_pro_sdk/types/bms.py +++ b/go1_pro_sdk/types/bms.py @@ -1,9 +1,16 @@ -"""电池管理系统 (BMS) 数据.""" +"""电池管理系统 (BMS) 命令和状态数据.""" import struct from dataclasses import dataclass, field from typing import List +@dataclass +class BmsCmd: + """官方 SDK 同名的电池命令结构.""" + off: int = 0 + reserve: List[int] = field(default_factory=lambda: [0, 0, 0]) + + @dataclass class BMS: """BMS 反馈.""" @@ -46,3 +53,7 @@ class BMS: # 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 + + +# 官方 Python wrapper 使用 BmsState；保留 BMS 作为项目原有名称。 +BmsState = BMS diff --git a/go1_pro_sdk/types/low_cmd.py b/go1_pro_sdk/types/low_cmd.py index 4bcf19c..a2c73b1 100644 --- a/go1_pro_sdk/types/low_cmd.py +++ b/go1_pro_sdk/types/low_cmd.py @@ -1,6 +1,7 @@ """LowCmd — 主控发给 MCU 的控制命令 (PRO 格式 616B 明文 / Blowfish 加密后 616B).""" from dataclasses import dataclass, field from typing import List +from .bms import BmsCmd from .motor import MotorCmd @@ -17,8 +18,10 @@ class LowCmd: version: bytes = b'\x00' * 8 # PRO 不填, 全 0 bandWidth: int = 0x3ac0 # 真包实测值 motorCmd: List[MotorCmd] = field(default_factory=lambda: [MotorCmd() for _ in range(20)]) + bms: BmsCmd = field(default_factory=BmsCmd) wirelessRemote: bytes = b'\x00' * 40 reserve: bytes = b'\x00' * 4 + crc: int = 0 # 发送时由 builder 重新计算 def set_motor(self, index_or_name, cmd: MotorCmd): """设置某个电机命令. diff --git a/go1_pro_sdk/utils/__init__.py b/go1_pro_sdk/utils/__init__.py index af021b9..f4b0b02 100644 --- a/go1_pro_sdk/utils/__init__.py +++ b/go1_pro_sdk/utils/__init__.py @@ -7,7 +7,10 @@ from .common import ( decode_sn, decode_version, ) from .constants import ( - MCU_IP, MCU_PORT, + MCU_IP, MCU_PORT, HIGHLEVEL, LOWLEVEL, TRIGERLEVEL, PosStopF, VelStopF, + FR_, FL_, RR_, RL_, + FR_0, FR_1, FR_2, FL_0, FL_1, FL_2, + RR_0, RR_1, RR_2, RL_0, RL_1, RL_2, JOINT_NAMES, JOINT_TYPE, JOINT_ATTR, JOINT_LIMITS, JOINT_LIMITS_MEASURED, TAU_MAX, KT, CRITICAL_FACTOR, @@ -23,6 +26,10 @@ __all__ = [ 'decode_sn', 'decode_version', # constants 'MCU_IP', 'MCU_PORT', + 'HIGHLEVEL', 'LOWLEVEL', 'TRIGERLEVEL', 'PosStopF', 'VelStopF', + 'FR_', 'FL_', 'RR_', 'RL_', + 'FR_0', 'FR_1', 'FR_2', 'FL_0', 'FL_1', 'FL_2', + 'RR_0', 'RR_1', 'RR_2', 'RL_0', 'RL_1', 'RL_2', 'JOINT_NAMES', 'JOINT_TYPE', 'JOINT_ATTR', 'JOINT_LIMITS', 'JOINT_LIMITS_MEASURED', 'TAU_MAX', 'KT', 'CRITICAL_FACTOR', diff --git a/go1_pro_sdk/utils/constants.py b/go1_pro_sdk/utils/constants.py index be6adaf..ae32f7c 100644 --- a/go1_pro_sdk/utils/constants.py +++ b/go1_pro_sdk/utils/constants.py @@ -8,6 +8,12 @@ MCU_IP = '192.168.123.10' MCU_PORT = 8007 """MCU UDP 端口 (从这里发/收 LowCmd/LowState)""" +HIGHLEVEL = 0xEE +LOWLEVEL = 0xFF +TRIGERLEVEL = 0xF0 +PosStopF = 2.146e9 +VelStopF = 16000.0 + # ===== 关节命名/索引 ===== @@ -18,6 +24,9 @@ JOINT_NAMES = [ 'RL_0', 'RL_1', 'RL_2', # 9..11 左后 ] +FR_, FL_, RR_, RL_ = range(4) +FR_0, FR_1, FR_2, FL_0, FL_1, FL_2, RR_0, RR_1, RR_2, RL_0, RL_1, RL_2 = range(12) + JOINT_TYPE = {i: ('hip' if i % 3 == 0 else 'thigh' if i % 3 == 1 else 'knee') for i in range(12)} diff --git a/pyproject.toml b/pyproject.toml index 5e79c5e..4a98359 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -28,7 +28,9 @@ Homepage = "https://github.com/your-username/go1_pro_sdk" Documentation = "https://github.com/your-username/go1_pro_sdk/blob/main/docs/" [tool.setuptools] +py-modules = ["robot_interface"] packages = ["go1_pro_sdk", + "go1_pro_sdk.compat", "go1_pro_sdk.codec", "go1_pro_sdk.connection", "go1_pro_sdk.highlevel", diff --git a/robot_interface.py b/robot_interface.py new file mode 100644 index 0000000..823b965 --- /dev/null +++ b/robot_interface.py @@ -0,0 +1,4 @@ +"""Drop-in import name for Unitree's official Python wrapper.""" + +from go1_pro_sdk.compat.robot_interface import * +from go1_pro_sdk.compat.robot_interface import __all__ diff --git a/tests/test_official_compat.py b/tests/test_official_compat.py new file mode 100644 index 0000000..75d15ab --- /dev/null +++ b/tests/test_official_compat.py @@ -0,0 +1,140 @@ +"""Contract tests for the official ``robot_interface`` compatibility API.""" + +from copy import deepcopy + +import pytest + +import robot_interface as sdk +from go1_pro_sdk import build_low_cmd_plain + + +class _FakeMCUClient: + instances = [] + + def __init__(self, mcu_ip, mcu_port, local_port): + self.mcu_ip = mcu_ip + self.mcu_port = mcu_port + self.local_port = local_port or 49152 + self.next_state = sdk.LowState() + self.next_state.motorState[sdk.FR_1].q = 1.23 + self.sent = [] + self.closed = False + self.__class__.instances.append(self) + + def send(self, cmd): + self.sent.append(deepcopy(cmd)) + return 616 + + def recv_latest(self): + return deepcopy(self.next_state) + + def recv_state(self, timeout): + return deepcopy(self.next_state) + + def close(self): + self.closed = True + + +@pytest.fixture +def fake_client(monkeypatch): + _FakeMCUClient.instances.clear() + monkeypatch.setattr( + "go1_pro_sdk.compat.robot_interface.MCUClient", + _FakeMCUClient, + ) + return _FakeMCUClient + + +def test_official_names_and_data_types_are_available(): + assert sdk.LOWLEVEL == 0xff + assert sdk.LeggedType.Go1 == 2 + assert sdk.Go1 is sdk.LeggedType.Go1 + assert sdk.FR_0 == 0 + assert sdk.RL_2 == 11 + assert sdk.BmsState().SOC == 0 + assert sdk.HighCmd().velocity == [0.0, 0.0] + + +def test_init_cmd_data_uses_official_stop_sentinels(): + udp = sdk.UDP(sdk.LOWLEVEL, 0, "192.168.123.10", 8007) + cmd = sdk.LowCmd() + + assert udp.InitCmdData(cmd) is None + assert cmd.levelFlag == sdk.LOWLEVEL + assert len(cmd.motorCmd) == 20 + assert all(m.mode == 0x0A for m in cmd.motorCmd) + assert all(m.q == sdk.PosStopF for m in cmd.motorCmd) + assert all(m.dq == sdk.VelStopF for m in cmd.motorCmd) + + +def test_official_lowlevel_send_receive_sequence(fake_client): + udp = sdk.UDP(sdk.LOWLEVEL, 0, "192.168.123.10", 8007) + cmd = sdk.LowCmd() + state = sdk.LowState() + udp.InitCmdData(cmd) + + assert udp.Recv() == 858 + assert udp.GetRecv(state) is None + assert state.motorState[sdk.FR_1].q == pytest.approx(1.23) + + cmd.motorCmd[sdk.FR_1].q = 1.2 + cmd.motorCmd[sdk.FR_1].dq = 0.0 + cmd.motorCmd[sdk.FR_1].Kp = 5.0 + cmd.motorCmd[sdk.FR_1].Kd = 1.0 + assert udp.SetSend(cmd) == 0 + assert udp.Send() == 616 + + client = fake_client.instances[0] + assert client.sent[0].motorCmd[sdk.FR_1].q == pytest.approx(1.2) + assert udp.udpState.SendCount == 1 + assert udp.udpState.RecvCount == 1 + + +def test_position_limit_skips_stop_sentinel(): + cmd = sdk.LowCmd() + sdk.UDP(sdk.LOWLEVEL, 0, "192.168.123.10", 8007).InitCmdData(cmd) + cmd.motorCmd[sdk.FR_1].q = 99.0 + + safe = sdk.Safety(sdk.LeggedType.Go1) + assert safe.PositionLimit(cmd) is None + + assert cmd.motorCmd[sdk.FR_0].q == sdk.PosStopF + assert cmd.motorCmd[sdk.FR_1].q == pytest.approx(3.5) + + +def test_bms_command_is_serialized(): + cmd = sdk.LowCmd() + cmd.bms.off = 0xA5 + cmd.bms.reserve = [1, 2, 3] + + plain = build_low_cmd_plain(cmd) + assert plain[562:566] == b"\xa5\x01\x02\x03" + + +def test_official_reserved_field_shapes_are_serialized(): + cmd = sdk.LowCmd() + cmd.SN = [0x04030201, 0x08070605] + cmd.version = [0x01020304, 0x05060708] + cmd.reserve = 0x44332211 + + plain = build_low_cmd_plain(cmd) + assert plain[4:12] == bytes.fromhex("0102030405060708") + assert plain[12:20] == bytes.fromhex("0403020108070605") + assert plain[606:610] == bytes.fromhex("11223344") + + +def test_fixed_size_official_arrays_are_validated(): + cmd = sdk.LowCmd() + cmd.wirelessRemote = [0] * 39 + with pytest.raises(ValueError, match="wirelessRemote"): + build_low_cmd_plain(cmd) + + +def test_highlevel_udp_reports_the_unsupported_boundary(): + udp = sdk.UDP(sdk.HIGHLEVEL, 8080, "192.168.123.161", 8082) + cmd = sdk.HighCmd() + udp.InitCmdData(cmd) + udp.SetSend(cmd) + + with pytest.raises(NotImplementedError, match="HighCmd/HighState UDP"): + udp.Send()