From f8b849397dcb800acbe8ef1eb2b0b5205a6f1bfd Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Thu, 30 Jul 2026 15:25:48 +0800 Subject: [PATCH] =?UTF-8?q?cpp=E5=AF=B9=E9=BD=90=E5=AE=98=E6=96=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 2 + CMakeLists.txt | 115 ++++++ README.md | 38 +- cmake/unitree_legged_sdkConfig.cmake.in | 7 + docs/OFFICIAL_API_COMPATIBILITY.md | 98 ++++- .../example_official_compatible_position.cpp | 60 +++ .../example_official_compatible_position.py | 62 +++ fast_lowcmd_cpp/go1_fast_lowcmd.cpp | 2 +- go1_pro_sdk/codec/lowcmd_builder.py | 2 +- go1_pro_sdk/compat/robot_interface.py | 363 ++++++++++++---- go1_pro_sdk/types/motor.py | 3 +- include/unitree_legged_sdk/a1_const.h | 11 + include/unitree_legged_sdk/aliengo_const.h | 11 + include/unitree_legged_sdk/b1_const.h | 11 + include/unitree_legged_sdk/comm.h | 179 ++++++++ include/unitree_legged_sdk/go1_const.h | 11 + include/unitree_legged_sdk/joystick.h | 27 ++ include/unitree_legged_sdk/loop.h | 48 +++ include/unitree_legged_sdk/quadruped.h | 33 ++ include/unitree_legged_sdk/safety.h | 30 ++ include/unitree_legged_sdk/udp.h | 62 +++ .../unitree_legged_sdk/unitree_legged_sdk.h | 14 + robot_interface.py | 1 - src/loop.cpp | 50 +++ src/pro_codec.cpp | 391 ++++++++++++++++++ src/pro_codec.h | 48 +++ src/quadruped.cpp | 25 ++ src/safety.cpp | 77 ++++ src/udp.cpp | 310 ++++++++++++++ tests/cpp/package_consumer/CMakeLists.txt | 7 + tests/cpp/package_consumer/main.cpp | 9 + tests/cpp/test_compat.cpp | 127 ++++++ tests/test_capture_fixtures.py | 108 +++++ tests/test_lowcmd_builder.py | 4 +- tests/test_mcu_client.py | 97 +++++ tests/test_official_compat.py | 240 +++++++++-- 36 files changed, 2561 insertions(+), 122 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 cmake/unitree_legged_sdkConfig.cmake.in create mode 100644 examples/cpp/example_official_compatible_position.cpp create mode 100644 examples/example_official_compatible_position.py create mode 100644 include/unitree_legged_sdk/a1_const.h create mode 100644 include/unitree_legged_sdk/aliengo_const.h create mode 100644 include/unitree_legged_sdk/b1_const.h create mode 100644 include/unitree_legged_sdk/comm.h create mode 100644 include/unitree_legged_sdk/go1_const.h create mode 100644 include/unitree_legged_sdk/joystick.h create mode 100644 include/unitree_legged_sdk/loop.h create mode 100644 include/unitree_legged_sdk/quadruped.h create mode 100644 include/unitree_legged_sdk/safety.h create mode 100644 include/unitree_legged_sdk/udp.h create mode 100644 include/unitree_legged_sdk/unitree_legged_sdk.h create mode 100644 src/loop.cpp create mode 100644 src/pro_codec.cpp create mode 100644 src/pro_codec.h create mode 100644 src/quadruped.cpp create mode 100644 src/safety.cpp create mode 100644 src/udp.cpp create mode 100644 tests/cpp/package_consumer/CMakeLists.txt create mode 100644 tests/cpp/package_consumer/main.cpp create mode 100644 tests/cpp/test_compat.cpp create mode 100644 tests/test_capture_fixtures.py create mode 100644 tests/test_mcu_client.py diff --git a/.gitignore b/.gitignore index 24b7d00..01aed92 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,8 @@ __pycache__/ *.so .Python build/ +build-cpp*/ +CMakeFiles/ dist/ *.egg-info/ .installed.cfg diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..a771cd2 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,115 @@ +cmake_minimum_required(VERSION 3.16) +project(unitree_legged_sdk VERSION 3.8.6 LANGUAGES CXX) + +if(POLICY CMP0167) + cmake_policy(SET CMP0167 NEW) +endif() + +include(GNUInstallDirs) +include(CMakePackageConfigHelpers) +find_package(Threads REQUIRED) +find_package(Boost REQUIRED) + +option(GO1_PRO_BUILD_EXAMPLES "Build portable official-API C++ examples" ON) +option(GO1_PRO_BUILD_TESTS "Build C++ compatibility tests" ON) + +add_library(unitree_legged_sdk + src/loop.cpp + src/pro_codec.cpp + src/quadruped.cpp + src/safety.cpp + src/udp.cpp +) +add_library(unitree_legged_sdk::unitree_legged_sdk ALIAS unitree_legged_sdk) +target_compile_features(unitree_legged_sdk PUBLIC cxx_std_14) +target_include_directories(unitree_legged_sdk + PUBLIC + $ + $ + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src +) +target_compile_definitions(unitree_legged_sdk PRIVATE + GO1_PRO_SOURCE_STATE_FILE="${CMAKE_CURRENT_SOURCE_DIR}/go1_pro_sdk/_data/blowfish_state.bin" + GO1_PRO_INSTALL_STATE_FILE="${CMAKE_INSTALL_FULL_DATADIR}/go1_pro_sdk/blowfish_state.bin" +) +if(TARGET Boost::headers) + set(GO1_PRO_BOOST_TARGET Boost::headers) +else() + set(GO1_PRO_BOOST_TARGET Boost::boost) +endif() +target_link_libraries(unitree_legged_sdk PUBLIC Threads::Threads ${GO1_PRO_BOOST_TARGET}) + +if(GO1_PRO_BUILD_EXAMPLES) + add_executable(example_official_compatible_position + examples/cpp/example_official_compatible_position.cpp) + target_link_libraries(example_official_compatible_position PRIVATE unitree_legged_sdk) +endif() + +if(GO1_PRO_BUILD_TESTS) + enable_testing() + add_executable(test_cpp_compat tests/cpp/test_compat.cpp) + target_include_directories(test_cpp_compat PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src) + set(GO1_PRO_CAPTURE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/data/captures" CACHE PATH + "Optional private real-robot capture fixture directory") + if(EXISTS "${GO1_PRO_CAPTURE_DIR}/mcu_response_new.bin" + AND EXISTS "${GO1_PRO_CAPTURE_DIR}/mcu_response.bin" + AND EXISTS "${GO1_PRO_CAPTURE_DIR}/real_lowcmd.bin" + AND EXISTS "${GO1_PRO_CAPTURE_DIR}/real_lowcmd_decrypted.bin") + target_compile_definitions(test_cpp_compat PRIVATE + GO1_PRO_HAVE_CAPTURE_FIXTURES=1 + GO1_PRO_TEST_CAPTURE="${GO1_PRO_CAPTURE_DIR}/mcu_response_new.bin" + GO1_PRO_TEST_CAPTURE_OLD="${GO1_PRO_CAPTURE_DIR}/mcu_response.bin" + GO1_PRO_TEST_LOWCMD="${GO1_PRO_CAPTURE_DIR}/real_lowcmd.bin" + GO1_PRO_TEST_LOWCMD_PLAIN="${GO1_PRO_CAPTURE_DIR}/real_lowcmd_decrypted.bin") + else() + message(STATUS "Private data/captures fixtures not found; C++ capture checks disabled") + endif() + target_link_libraries(test_cpp_compat PRIVATE unitree_legged_sdk) + add_test(NAME cpp_official_compat COMMAND test_cpp_compat) + + set(GO1_PRO_TEST_INSTALL_DIR "${CMAKE_CURRENT_BINARY_DIR}/test-install") + set(GO1_PRO_TEST_CONSUMER_BUILD "${CMAKE_CURRENT_BINARY_DIR}/package-consumer") + add_test(NAME cpp_package_install + COMMAND ${CMAKE_COMMAND} --install ${CMAKE_CURRENT_BINARY_DIR} + --prefix ${GO1_PRO_TEST_INSTALL_DIR}) + add_test(NAME cpp_package_configure + COMMAND ${CMAKE_COMMAND} + -S ${CMAKE_CURRENT_SOURCE_DIR}/tests/cpp/package_consumer + -B ${GO1_PRO_TEST_CONSUMER_BUILD} + -DCMAKE_PREFIX_PATH=${GO1_PRO_TEST_INSTALL_DIR} + "-DCMAKE_CXX_FLAGS=${CMAKE_CXX_FLAGS}" + "-DCMAKE_EXE_LINKER_FLAGS=${CMAKE_EXE_LINKER_FLAGS}") + set_tests_properties(cpp_package_configure PROPERTIES DEPENDS cpp_package_install) + add_test(NAME cpp_package_build + COMMAND ${CMAKE_COMMAND} --build ${GO1_PRO_TEST_CONSUMER_BUILD}) + set_tests_properties(cpp_package_build PROPERTIES DEPENDS cpp_package_configure) + add_test(NAME cpp_package_run + COMMAND ${GO1_PRO_TEST_CONSUMER_BUILD}/package_consumer) + set_tests_properties(cpp_package_run PROPERTIES DEPENDS cpp_package_build) +endif() + +install(TARGETS unitree_legged_sdk EXPORT unitree_legged_sdkTargets + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) +install(DIRECTORY include/unitree_legged_sdk DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) +install(FILES go1_pro_sdk/_data/blowfish_state.bin DESTINATION ${CMAKE_INSTALL_DATADIR}/go1_pro_sdk) +install(EXPORT unitree_legged_sdkTargets + FILE unitree_legged_sdkTargets.cmake + NAMESPACE unitree_legged_sdk:: + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/unitree_legged_sdk) + +configure_package_config_file( + cmake/unitree_legged_sdkConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/unitree_legged_sdkConfig.cmake + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/unitree_legged_sdk) +write_basic_package_version_file( + ${CMAKE_CURRENT_BINARY_DIR}/unitree_legged_sdkConfigVersion.cmake + VERSION ${PROJECT_VERSION} + COMPATIBILITY SameMajorVersion) +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/unitree_legged_sdkConfig.cmake + ${CMAKE_CURRENT_BINARY_DIR}/unitree_legged_sdkConfigVersion.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/unitree_legged_sdk) diff --git a/README.md b/README.md index 4d98257..c739bea 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Go1 PRO SDK -Unitree Go1 **PRO** 机型的低层电机控制 Python SDK。完整逆向 PRO 版的私有协议（Blowfish 加密 + 私有 LowCmd 格式），不依赖官方 C++ SDK，Mac 直连可达 480Hz 控制频率。 +Unitree Go1 **PRO** 机型的低层电机控制 Python/C++ SDK。完整逆向 PRO 版的私有协议（Blowfish 加密 + 私有 LowCmd 格式），不依赖官方二进制库，Mac 直连可达 480Hz 控制频率。 > 这是为 PRO 准备的 SDK。如果你有 **EDU** 版机器狗，请用原版 [free-dog-sdk](https://github.com/Bin4ry/free-dog-sdk) — PRO 跟 EDU 协议有显著差异，互不兼容。 @@ -24,7 +24,9 @@ Unitree Go1 **PRO** 机型的低层电机控制 Python SDK。完整逆向 PRO ```python import robot_interface as sdk -udp = sdk.UDP(sdk.LOWLEVEL, 8080, "192.168.123.10", 8007) +LOWLEVEL = 0xff # 官方 wrapper 不导出这个常量，官方示例也是在应用中定义 + +udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007) safe = sdk.Safety(sdk.LeggedType.Go1) cmd, state = sdk.LowCmd(), sdk.LowState() udp.InitCmdData(cmd) @@ -37,7 +39,33 @@ udp.Send() ``` 兼容范围和协议差异见 [`docs/OFFICIAL_API_COMPATIBILITY.md`](docs/OFFICIAL_API_COMPATIBILITY.md)。 -官方 `HighCmd/HighState` UDP 通道和 C++ ABI 当前不在兼容范围内。 +官方 `HighCmd/HighState` UDP 通道和二进制 ABI 当前不在兼容范围内。 +需要同一份代码同时运行在本项目和官方 SDK 上时，只使用官方实际导出的接口；参考 +[`examples/example_official_compatible_position.py`](examples/example_official_compatible_position.py)。 + +### 官方 C++ SDK 兼容接口 + +同一份低层 C++ 源码可分别链接本项目或官方 `unitree_legged_sdk`： + +```cpp +#include "unitree_legged_sdk/unitree_legged_sdk.h" +using namespace UNITREE_LEGGED_SDK; + +UDP udp(LOWLEVEL, 8090, "192.168.123.10", 8007); +Safety safe(LeggedType::Go1); +LowCmd cmd{}; +udp.InitCmdData(cmd); +``` + +```bash +cmake -S . -B build-cpp -DCMAKE_BUILD_TYPE=Release +cmake --build build-cpp +ctest --test-dir build-cpp --output-on-failure +``` + +PRO 环境链接本项目生成的 `libunitree_legged_sdk`；官方机器人环境链接官方同名库。 +参考 [`examples/cpp/example_official_compatible_position.cpp`](examples/cpp/example_official_compatible_position.cpp)。 +Blowfish state 默认随库安装，也可用 `GO1_PRO_BLOWFISH_STATE` 指定。 ### 高层控制 (走/跳/姿态, 通过 sportMode 系统) @@ -151,6 +179,10 @@ go1_pro_sdk/ ├── safety/ # PositionLimit / PowerProtect / PositionProtect ├── utils/ # CRC, 浮点编解码, 关节常量 └── _data/ # blowfish_state.bin + +include/unitree_legged_sdk/ # 官方 C++ 头文件兼容层 +src/ # PRO UDP/Blowfish/Safety 实现 +tests/cpp/ # C++ 契约、抓包和安装消费测试 ``` ## 文档 diff --git a/cmake/unitree_legged_sdkConfig.cmake.in b/cmake/unitree_legged_sdkConfig.cmake.in new file mode 100644 index 0000000..4bf240b --- /dev/null +++ b/cmake/unitree_legged_sdkConfig.cmake.in @@ -0,0 +1,7 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(Threads) +find_dependency(Boost) +include("${CMAKE_CURRENT_LIST_DIR}/unitree_legged_sdkTargets.cmake") +check_required_components(unitree_legged_sdk) diff --git a/docs/OFFICIAL_API_COMPATIBILITY.md b/docs/OFFICIAL_API_COMPATIBILITY.md index c6335c2..3a91db3 100644 --- a/docs/OFFICIAL_API_COMPATIBILITY.md +++ b/docs/OFFICIAL_API_COMPATIBILITY.md @@ -14,6 +14,10 @@ 导入和主要调用流程，同时底层会使用 Go1 PRO 所需的 Blowfish 加密和 616 字节 私有 `LowCmd` 协议。 +反过来，要让本项目编写的程序直接在官方 SDK 上复现，应用必须导入 +`robot_interface`，并限制在官方 wrapper 真正导出的公共子集内。应用不应直接导入 +`go1_pro_sdk`。 + ## 接口矩阵 | 官方接口 | 本项目原接口 | 当前兼容状态 | @@ -29,8 +33,8 @@ | `Safety.PowerProtect` | `power_protect` | 调用兼容，算法不是官方闭源实现 | | `Safety.PositionProtect` | `position_protect` | 调用兼容，保护动作更保守 | | `HighCmd/HighState` | MQTT `Go1/Go1MQTT` | 类型已提供，UDP 传输不支持 | -| `Loop/LoopFunc` | 无 | 官方 Python wrapper 本身也未导出 | -| C++ headers/static library | 无 | 不兼容 C++ 源码或 ABI | +| `Loop/LoopFunc` | 无 | C++ 已提供；官方 Python wrapper 本身未导出 | +| C++ headers/static library | 无 | 低层 C++ 源码兼容；不承诺二进制 ABI | ## 关键协议差异 @@ -49,7 +53,10 @@ EDU SDK 相同。 ```python import robot_interface as sdk -udp = sdk.UDP(sdk.LOWLEVEL, 8080, "192.168.123.10", 8007) +LOWLEVEL = 0xff +FR_1 = 1 + +udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007) safe = sdk.Safety(sdk.LeggedType.Go1) cmd = sdk.LowCmd() state = sdk.LowState() @@ -58,16 +65,84 @@ 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 +cmd.motorCmd[FR_1].q = 1.2 +cmd.motorCmd[FR_1].dq = 0.0 +cmd.motorCmd[FR_1].Kp = 5.0 +cmd.motorCmd[FR_1].Kd = 1.0 safe.PositionLimit(cmd) safe.PowerProtect(cmd, state, 1) udp.SetSend(cmd) udp.Send() ``` +这里故意不使用 `sdk.LOWLEVEL`、`sdk.FR_1`、上下文管理器或 `close()`：这些是本项目 +提供的便利能力，但不属于官方 Python wrapper 的导出契约。 + +## C++ 同源码用法 + +C++ 公共入口、命名空间、结构字段和低层方法签名与官方 v3.8.6 对齐： + +```cpp +#include "unitree_legged_sdk/unitree_legged_sdk.h" +using namespace UNITREE_LEGGED_SDK; + +UDP udp(LOWLEVEL, 8090, "192.168.123.10", 8007); +Safety safe(LeggedType::Go1); +LowCmd cmd{}; +LowState state{}; +udp.InitCmdData(cmd); +``` + +业务源码和 `#include` 不变，只切换链接库。PRO 端用本项目 CMake 目标 +`unitree_legged_sdk`，官方 Go1 端用官方仓库的 `libunitree_legged_sdk.a`。本项目已经用 +官方头文件反向编译可移植示例，并用本项目头文件编译官方仓库的五个 C++ 示例。 + +本项目构建方法： + +```bash +cmake -S . -B build-cpp -DCMAKE_BUILD_TYPE=Release +cmake --build build-cpp +ctest --test-dir build-cpp --output-on-failure +``` + +标准四参数 `UDP(LOWLEVEL, ...)` 会在库内部完成 PRO 编解码。默认从安装数据目录读取 +Blowfish state；需要覆盖时设置 `GO1_PRO_BLOWFISH_STATE=/path/to/blowfish_state.bin`。 + +## 可移植程序规则 + +要求同一份源码直接运行时，主控制流程只使用： + +- `import robot_interface as sdk`； +- `sdk.UDP`、`sdk.Safety`、`sdk.LeggedType`； +- `sdk.LowCmd/LowState`、`MotorCmd/MotorState`、`IMU/BmsState`； +- `InitCmdData/Recv/GetRecv/SetSend/Send`； +- 官方结构中公开的字段，例如 `motorCmd[i].q` 和 `motorState[i].q`。 + +所有官方结构对象都应先无参构造，再逐字段赋值。例如使用 +`motor = sdk.MotorCmd(); motor.q = 1.0`，不要写 `sdk.MotorCmd(q=1.0)`。后者是 Python +dataclass 常见写法，但官方 pybind wrapper 不接受构造参数。 + +枚举也按官方对象使用，例如 `sdk.Safety(sdk.LeggedType.Go1)`。不要把枚举当作整数比较 +或直接写 `sdk.Safety(2)`；本项目的严格兼容入口会和官方一样拒绝后一种写法。 + +以下接口属于 PRO 扩展，使用后程序不再能在只安装官方 SDK 的环境中直接运行： + +- `from go1_pro_sdk import ...`； +- `MCUClient`、`Go1`、`Go1MQTT`； +- `LowCmd.set_motor()`、`all_damping()`； +- `LowState.remote`、`apply_safety()`； +- `wake_mcu()`、`safe_stop()`、Blowfish 和快速 C++ builder。 + +推荐把程序分为“官方公共控制核心”和“可选 PRO 增强”两个模块。公共核心不能导入 +增强模块；增强模块可以包装公共核心，但必须允许在官方环境中完全不加载。 + +`Send()`、`Recv()` 和 `SetSend()` 的具体正整数来自线协议和底层 socket，不属于跨 +SDK 稳定契约。初始化时可以用 `Recv() > 0` 判断是否收到包，但不应比较具体字节数。 + +首次进入 PRO 低层通道前，应先反复发送 `InitCmdData()` 产生的 stop sentinel，并在 +收到第一帧 `LowState` 后再写入有效位置目标。可移植示例已采用这一时序，避免在状态 +仍为全零默认值时直接驱动关节。 + `UDP(HIGHLEVEL, ..., "192.168.123.161", 8082)` 不会被静默映射到 MQTT。两种通道 状态模型和时序不同，兼容层会明确抛出 `NotImplementedError`。高层控制继续使用 `Go1` 或 `Go1MQTT`。 @@ -85,5 +160,10 @@ udp.Send() 类型不保证完全相同。 - 本项目 `LowState` 是 PRO 抓包格式的解释，不应按官方 `#pragma pack(1)` 结构大小 直接做内存映射。 -- 官方 C++ 用户需要单独的 C++ facade 和库链接方案；Python 兼容模块不能让现有 - C++ 程序直接重编译通过。 +- C++ 类型布局和公开符号用于源码重编译兼容，不保证本项目库与官方预编译对象之间 + 的 ABI 互换；切换实现时应重新编译应用。 +- 两个自定义包长 C++ `UDP` 构造函数和 `SetSend(char*)` 提供原始 UDP 透传，但不会 + 自动把任意自定义结构转换为 PRO 私有协议；主动断连时间和 accessible 时间参数 + 仅保留调用形状。Python 兼容入口仍只保证标准四参数 `LOWLEVEL` 通道。 +- 官方 SDK 和本项目都提供顶层 `robot_interface`，同一个 Python 环境不要同时安装 + 两种实现。应在 PRO 环境安装本项目，在官方机器人环境安装官方 SDK。 diff --git a/examples/cpp/example_official_compatible_position.cpp b/examples/cpp/example_official_compatible_position.cpp new file mode 100644 index 0000000..fb45081 --- /dev/null +++ b/examples/cpp/example_official_compatible_position.cpp @@ -0,0 +1,60 @@ +#include "unitree_legged_sdk/unitree_legged_sdk.h" + +#include +#include +#include +#include + +using namespace UNITREE_LEGGED_SDK; + +int main() { + constexpr float dt = 0.002f; + UDP udp(LOWLEVEL, 8090, UDP_SERVER_IP_BASIC, UDP_SERVER_PORT); + Safety safety(LeggedType::Go1); + LowCmd cmd{}; + LowState state{}; + udp.InitCmdData(cmd); + + bool have_state = false; + for (int attempt = 0; attempt < 1000 && !have_state; ++attempt) { + udp.SetSend(cmd); + udp.Send(); + if (udp.Recv() > 0) { + udp.GetRecv(state); + have_state = state.head[0] == 0xfe && state.head[1] == 0xef; + } + std::this_thread::sleep_for(std::chrono::duration(dt)); + } + if (!have_state) { + std::cerr << "No LowState received; active position command was not enabled.\n"; + return 1; + } + + const float initial = state.motorState[FR_1].q; + for (int step = 0; step < 2500; ++step) { + udp.Recv(); + udp.GetRecv(state); + const float phase = static_cast(step) * dt; + cmd.motorCmd[FR_1].q = initial + 0.15f * std::sin(phase * 2.0f); + cmd.motorCmd[FR_1].dq = 0.0f; + cmd.motorCmd[FR_1].Kp = 5.0f; + cmd.motorCmd[FR_1].Kd = 1.0f; + cmd.motorCmd[FR_1].tau = 0.0f; + safety.PositionLimit(cmd); + if (safety.PowerProtect(cmd, state, 1) < 0) { + udp.InitCmdData(cmd); + udp.SetSend(cmd); + udp.Send(); + return 2; + } + safety.PositionProtect(cmd, state, 0.5); + udp.SetSend(cmd); + udp.Send(); + std::this_thread::sleep_for(std::chrono::duration(dt)); + } + + udp.InitCmdData(cmd); + udp.SetSend(cmd); + udp.Send(); + return 0; +} diff --git a/examples/example_official_compatible_position.py b/examples/example_official_compatible_position.py new file mode 100644 index 0000000..0c9022e --- /dev/null +++ b/examples/example_official_compatible_position.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +"""Low-level example that runs with either SDK's ``robot_interface`` module.""" + +import time + +import robot_interface as sdk + + +# Unitree's official Python wrapper does not export these C++ constants. +LOWLEVEL = 0xff +FR_1 = 1 + + +def control_step(udp, safe, cmd, state, target_q=1.2): + """Execute one control iteration using only the official Python API.""" + udp.Recv() + udp.GetRecv(state) + + motor = cmd.motorCmd[FR_1] + motor.q = target_q + motor.dq = 0.0 + motor.Kp = 5.0 + motor.Kd = 1.0 + motor.tau = 0.0 + + safe.PositionLimit(cmd) + if safe.PowerProtect(cmd, state, 1) < 0: + raise RuntimeError("PowerProtect rejected the command") + + udp.SetSend(cmd) + udp.Send() + + +def initialize_transport(udp, cmd, state, frames=50, dt=0.01): + """Send only stop sentinels until the PRO MCU can return initial state.""" + udp.SetSend(cmd) + received = False + for _ in range(frames): + udp.Send() + time.sleep(dt) + if udp.Recv() > 0: + received = True + udp.GetRecv(state) + if not received: + raise TimeoutError("No LowState received during initialization") + + +def main(): + udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007) + safe = sdk.Safety(sdk.LeggedType.Go1) + cmd = sdk.LowCmd() + state = sdk.LowState() + udp.InitCmdData(cmd) + initialize_transport(udp, cmd, state) + + while True: + control_step(udp, safe, cmd, state) + time.sleep(0.002) + + +if __name__ == "__main__": + main() diff --git a/fast_lowcmd_cpp/go1_fast_lowcmd.cpp b/fast_lowcmd_cpp/go1_fast_lowcmd.cpp index e62ec5d..64b1084 100644 --- a/fast_lowcmd_cpp/go1_fast_lowcmd.cpp +++ b/fast_lowcmd_cpp/go1_fast_lowcmd.cpp @@ -913,7 +913,7 @@ PyObject* FastLowCmdBuilder_decrypt_lowstate(FastLowCmdBuilder* self, PyObject* q_raw[i] = static_cast(get_f32_le(m + 13)); dq_raw[i] = static_cast(get_f32_le(m + 17)); ddq_raw[i] = static_cast(get_i16_le(m + 21)); - temperature[i] = static_cast(m[24]); + temperature[i] = static_cast(m[23]); reserve0[i] = static_cast(get_u32_le(m + 24)); reserve1[i] = static_cast(get_u32_le(m + 28)); } diff --git a/go1_pro_sdk/codec/lowcmd_builder.py b/go1_pro_sdk/codec/lowcmd_builder.py index d9dcc5d..a60593e 100644 --- a/go1_pro_sdk/codec/lowcmd_builder.py +++ b/go1_pro_sdk/codec/lowcmd_builder.py @@ -83,7 +83,7 @@ def build_low_cmd_plain(lowcmd: LowCmd) -> bytes: raise ValueError(f'motorCmd 必须有 20 项, got {len(lowcmd.motorCmd)}') offset = 22 for motor in lowcmd.motorCmd: - motor_bytes = motor.to_bytes() + motor_bytes = MotorCmd.to_bytes(motor) assert len(motor_bytes) == 27, f'motorCmd 序列化长度错: {len(motor_bytes)}' cmd[offset:offset + 27] = motor_bytes offset += 27 diff --git a/go1_pro_sdk/compat/robot_interface.py b/go1_pro_sdk/compat/robot_interface.py index a2e0436..92830a2 100644 --- a/go1_pro_sdk/compat/robot_interface.py +++ b/go1_pro_sdk/compat/robot_interface.py @@ -7,23 +7,37 @@ encrypted protocol implemented by this project. from copy import deepcopy from dataclasses import dataclass, field, fields, is_dataclass -from enum import IntEnum +from enum import Enum +import struct 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 ..types import ( + BmsCmd as _NativeBmsCmd, + BmsState as _NativeBmsState, + IMU as _NativeIMU, + LowCmd as _NativeLowCmd, + LowState as _NativeLowState, + MotorCmd as _NativeMotorCmd, + MotorState as _NativeMotorState, +) from ..utils.constants import ( - HIGHLEVEL, LOWLEVEL, TRIGERLEVEL, PosStopF, VelStopF, + HIGHLEVEL, LOWLEVEL, 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): +class _OfficialEnum(Enum): + def __int__(self): + return self.value + + def __index__(self): + return self.value + + +class LeggedType(_OfficialEnum): Aliengo = 0 A1 = 1 Go1 = 2 @@ -36,7 +50,7 @@ Go1 = LeggedType.Go1 B1 = LeggedType.B1 -class RecvEnum(IntEnum): +class RecvEnum(_OfficialEnum): nonBlock = 0x00 block = 0x01 blockTimeout = 0x02 @@ -47,21 +61,115 @@ block = RecvEnum.block blockTimeout = RecvEnum.blockTimeout -@dataclass +class _HiddenAttribute: + def __get__(self, instance, owner=None): + raise AttributeError("not part of Unitree's official Python API") + + +class BmsCmd(_NativeBmsCmd): + def __init__(self): + super().__init__() + + +class BmsState(_NativeBmsState): + from_bytes = _HiddenAttribute() + voltage_mv = _HiddenAttribute() + voltage_v = _HiddenAttribute() + current_a = _HiddenAttribute() + + def __init__(self): + super().__init__() + + +class IMU(_NativeIMU): + from_bytes = _HiddenAttribute() + + def __init__(self): + super().__init__() + self.quaternion = [0.0] * 4 + self.gyroscope = [0.0] * 3 + self.accelerometer = [0.0] * 3 + self.rpy = [0.0] * 3 + + +class MotorCmd(_NativeMotorCmd): + to_bytes = _HiddenAttribute() + + def __init__(self): + super().__init__() + self.mode = 0 + + +class MotorState(_NativeMotorState): + from_bytes = _HiddenAttribute() + estimated_current = _HiddenAttribute() + + def __init__(self): + super().__init__() + + +class LowCmd(_NativeLowCmd): + set_motor = _HiddenAttribute() + all_damping = _HiddenAttribute() + + def __init__(self): + super().__init__() + self.head = [0, 0] + self.levelFlag = 0 + self.frameReserve = 0 + self.SN = [0, 0] + self.version = [0, 0] + self.bandWidth = 0 + self.motorCmd = [MotorCmd() for _ in range(20)] + self.bms = BmsCmd() + self.wirelessRemote = [0] * 40 + self.reserve = 0 + self.crc = 0 + + +class LowState(_NativeLowState): + remote = _HiddenAttribute() + + def __init__(self): + super().__init__() + self.head = [0, 0] + self.SN = [0, 0] + self.version = [0, 0] + self.imu = IMU() + self.motorState = [MotorState() for _ in range(20)] + self.footForce = [0] * 4 + self.footForceEst = [0] * 4 + self.bms = BmsState() + self.wirelessRemote = [0] * 40 + self.reserve = 0 + self.crc = 0 + + +@dataclass(init=False) class Cartesian: x: float = 0.0 y: float = 0.0 z: float = 0.0 + def __init__(self): + self.x = 0.0 + self.y = 0.0 + self.z = 0.0 -@dataclass + +@dataclass(init=False) class LED: r: int = 0 g: int = 0 b: int = 0 + def __init__(self): + self.r = 0 + self.g = 0 + self.b = 0 -@dataclass + +@dataclass(init=False) class HighState: head: List[int] = field(default_factory=lambda: [0, 0]) levelFlag: int = 0 @@ -89,8 +197,35 @@ class HighState: reserve: int = 0 crc: int = 0 + def __init__(self): + self.head = [0, 0] + self.levelFlag = 0 + self.frameReserve = 0 + self.SN = [0, 0] + self.version = [0, 0] + self.bandWidth = 0 + self.imu = IMU() + self.motorState = [MotorState() for _ in range(20)] + self.bms = BmsState() + self.footForce = [0] * 4 + self.footForceEst = [0] * 4 + self.mode = 0 + self.progress = 0.0 + self.gaitType = 0 + self.footRaiseHeight = 0.0 + self.position = [0.0] * 3 + self.bodyHeight = 0.0 + self.velocity = [0.0] * 3 + self.yawSpeed = 0.0 + self.rangeObstacle = [0.0] * 4 + self.footPosition2Body = [Cartesian() for _ in range(4)] + self.footSpeed2Body = [Cartesian() for _ in range(4)] + self.wirelessRemote = [0] * 40 + self.reserve = 0 + self.crc = 0 -@dataclass + +@dataclass(init=False) class HighCmd: head: List[int] = field(default_factory=lambda: [0, 0]) levelFlag: int = 0 @@ -113,8 +248,30 @@ class HighCmd: reserve: int = 0 crc: int = 0 + def __init__(self): + self.head = [0, 0] + self.levelFlag = 0 + self.frameReserve = 0 + self.SN = [0, 0] + self.version = [0, 0] + self.bandWidth = 0 + self.mode = 0 + self.gaitType = 0 + self.speedLevel = 0 + self.footRaiseHeight = 0.0 + self.bodyHeight = 0.0 + self.position = [0.0] * 2 + self.euler = [0.0] * 3 + self.velocity = [0.0] * 2 + self.yawSpeed = 0.0 + self.bms = BmsCmd() + self.led = [LED() for _ in range(4)] + self.wirelessRemote = [0] * 40 + self.reserve = 0 + self.crc = 0 -@dataclass + +@dataclass(init=False) class UDPState: TotalCount: int = 0 SendCount: int = 0 @@ -124,6 +281,15 @@ class UDPState: RecvCRCError: int = 0 RecvLoseError: int = 0 + def __init__(self): + self.TotalCount = 0 + self.SendCount = 0 + self.RecvCount = 0 + self.SendError = 0 + self.FlagError = 0 + self.RecvCRCError = 0 + self.RecvLoseError = 0 + def _copy_object(source, target) -> None: if is_dataclass(target): @@ -135,6 +301,54 @@ def _copy_object(source, target) -> None: setattr(target, name, deepcopy(getattr(source, name))) +def _u32_pair(value) -> List[int]: + if isinstance(value, (bytes, bytearray, memoryview)): + return list(struct.unpack('<2I', bytes(value))) + return [int(item) for item in value] + + +def _u32(value) -> int: + if isinstance(value, (bytes, bytearray, memoryview)): + return int.from_bytes(value, 'little') + return int(value) + + +def _copy_low_state(source, target: LowState) -> None: + if isinstance(source.head, int): + target.head = [source.head & 0xff, (source.head >> 8) & 0xff] + else: + target.head = list(source.head) + target.levelFlag = int(source.levelFlag) + target.frameReserve = int(source.frameReserve) + target.SN = _u32_pair(source.SN) + target.version = _u32_pair(source.version) + target.bandWidth = int(source.bandWidth) + + target.imu = IMU() + for name in ('quaternion', 'gyroscope', 'accelerometer', 'rpy'): + setattr(target.imu, name, list(getattr(source.imu, name))) + target.imu.temperature = int(source.imu.temperature) + + target.motorState = [] + for source_motor in source.motorState: + motor = MotorState() + _copy_object(source_motor, motor) + motor.reserve = list(source_motor.reserve) + target.motorState.append(motor) + + target.bms = BmsState() + _copy_object(source.bms, target.bms) + target.bms.BQ_NTC = list(source.bms.BQ_NTC) + target.bms.MCU_NTC = list(source.bms.MCU_NTC) + target.bms.cell_vol = list(source.bms.cell_vol) + target.footForce = list(source.footForce) + target.footForceEst = list(source.footForceEst) + target.tick = int(source.tick) + target.wirelessRemote = list(source.wirelessRemote) + target.reserve = _u32(source.reserve) + target.crc = _u32(source.crc) + + class UDP: """Official-style UDP facade backed by :class:`MCUClient`. @@ -144,33 +358,32 @@ class UDP: """ 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]) + if len(args) == 4 and isinstance(args[2], str): + self._level = int(args[0]) + self._local_port = int(args[1]) + self._target_ip = args[2] + self._target_port = 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 + elif len(args) == 7 and isinstance(args[1], str): + self._level = None + self._local_port = int(args[0]) + self._target_ip = args[1] + self._target_port = int(args[2]) + self._recv_type = RecvEnum(args[6]) 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 + elif len(args) == 6: + self._level = None + self._local_port = int(args[0]) + self._target_ip = MCU_IP + self._target_port = MCU_PORT + self._recv_type = RecvEnum(args[4]) 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._accessible = False + self._udp_state = UDPState() self._client: Optional[MCUClient] = None self._send_cmd: Optional[LowCmd] = None self._recv_state: Optional[LowState] = None @@ -178,39 +391,34 @@ class UDP: 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() + def __del__(self): + client = getattr(self, '_client', None) + if client is not None: + client.close() self._client = None def _ensure_client(self) -> MCUClient: - if self.level == HIGHLEVEL: + 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: + 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, + mcu_ip=self._target_ip, + mcu_port=self._target_port, + local_port=self._local_port, ) - self.localPort = self._client.local_port + self._local_port = 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) + self._target_ip = str(targetIP) + self._target_port = int(targetPort) def SetRecvTimeout(self, time): self._recv_timeout_ms = max(0, int(time)) @@ -223,16 +431,22 @@ class UDP: def InitCmdData(self, cmd): if isinstance(cmd, HighCmd): - _copy_object(HighCmd(levelFlag=HIGHLEVEL), cmd) + template = HighCmd() + template.head = [0xfe, 0xef] + template.levelFlag = HIGHLEVEL + _copy_object(template, 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) - ] + template = LowCmd() + template.head = [0xfe, 0xef] + template.levelFlag = LOWLEVEL + template.bandWidth = 0x3ac0 + for motor in template.motorCmd: + motor.mode = 0x0A + motor.q = PosStopF + motor.dq = VelStopF _copy_object(template, cmd) return None @@ -250,18 +464,18 @@ class UDP: raise RuntimeError("call InitCmdData and SetSend before Send") if isinstance(self._send_cmd, HighCmd): self._ensure_client() - self.udpState.TotalCount += 1 + self._udp_state.TotalCount += 1 try: sent = self._ensure_client().send(self._send_cmd) except Exception: - self.udpState.SendError += 1 + self._udp_state.SendError += 1 raise - self.udpState.SendCount += 1 + self._udp_state.SendCount += 1 return sent def Recv(self): client = self._ensure_client() - self.udpState.TotalCount += 1 + self._udp_state.TotalCount += 1 if self._recv_type == RecvEnum.nonBlock: state = client.recv_latest() else: @@ -270,8 +484,8 @@ class UDP: if state is None: return 0 self._recv_state = state - self.accessible = True - self.udpState.RecvCount += 1 + self._accessible = True + self._udp_state.RecvCount += 1 return 858 def GetRecv(self, state): @@ -280,7 +494,7 @@ class UDP: 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) + _copy_low_state(self._recv_state, state) return None @@ -288,8 +502,10 @@ 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: + if not isinstance(legged_type, LeggedType): + raise TypeError("Safety expects a LeggedType value") + self._legged_type = legged_type + if self._legged_type != LeggedType.Go1: raise ValueError("go1_pro_sdk only supports LeggedType.Go1") def PositionLimit(self, lowcmd): @@ -303,26 +519,13 @@ class Safety: return -1 return 0 - def PositionProtect(self, lowcmd, lowstate, limit=0.087): + def PositionProtect(self, lowcmd, lowstate, limit): 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", + "UDPState", ] diff --git a/go1_pro_sdk/types/motor.py b/go1_pro_sdk/types/motor.py index 4c28398..1f83bc1 100644 --- a/go1_pro_sdk/types/motor.py +++ b/go1_pro_sdk/types/motor.py @@ -71,7 +71,8 @@ class MotorState: 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] + # PRO wire layout stores the signed temperature before the two reserve words. + ms.temperature = data[23] ms.reserve = [ int.from_bytes(data[24:28], 'little'), int.from_bytes(data[28:32], 'little'), diff --git a/include/unitree_legged_sdk/a1_const.h b/include/unitree_legged_sdk/a1_const.h new file mode 100644 index 0000000..ee0f0a6 --- /dev/null +++ b/include/unitree_legged_sdk/a1_const.h @@ -0,0 +1,11 @@ +#ifndef GO1_PRO_UNITREE_LEGGED_A1_CONST_H_ +#define GO1_PRO_UNITREE_LEGGED_A1_CONST_H_ +namespace UNITREE_LEGGED_SDK { +constexpr double a1_Hip_max = 0.802; +constexpr double a1_Hip_min = -0.802; +constexpr double a1_Thigh_max = 4.19; +constexpr double a1_Thigh_min = -1.05; +constexpr double a1_Calf_max = -0.916; +constexpr double a1_Calf_min = -2.7; +} // namespace UNITREE_LEGGED_SDK +#endif diff --git a/include/unitree_legged_sdk/aliengo_const.h b/include/unitree_legged_sdk/aliengo_const.h new file mode 100644 index 0000000..bac50a7 --- /dev/null +++ b/include/unitree_legged_sdk/aliengo_const.h @@ -0,0 +1,11 @@ +#ifndef GO1_PRO_UNITREE_LEGGED_ALIENGO_CONST_H_ +#define GO1_PRO_UNITREE_LEGGED_ALIENGO_CONST_H_ +namespace UNITREE_LEGGED_SDK { +constexpr double aliengo_Hip_max = 1.047; +constexpr double aliengo_Hip_min = -0.873; +constexpr double aliengo_Thigh_max = 3.927; +constexpr double aliengo_Thigh_min = -0.524; +constexpr double aliengo_Calf_max = -0.611; +constexpr double aliengo_Calf_min = -2.775; +} // namespace UNITREE_LEGGED_SDK +#endif diff --git a/include/unitree_legged_sdk/b1_const.h b/include/unitree_legged_sdk/b1_const.h new file mode 100644 index 0000000..9289d82 --- /dev/null +++ b/include/unitree_legged_sdk/b1_const.h @@ -0,0 +1,11 @@ +#ifndef GO1_PRO_UNITREE_LEGGED_B1_CONST_H_ +#define GO1_PRO_UNITREE_LEGGED_B1_CONST_H_ +namespace UNITREE_LEGGED_SDK { +constexpr double b1_Hip_max = 0.75; +constexpr double b1_Hip_min = -0.75; +constexpr double b1_Thigh_max = 3.5; +constexpr double b1_Thigh_min = -1.0; +constexpr double b1_Calf_max = -0.6; +constexpr double b1_Calf_min = -2.6; +} // namespace UNITREE_LEGGED_SDK +#endif diff --git a/include/unitree_legged_sdk/comm.h b/include/unitree_legged_sdk/comm.h new file mode 100644 index 0000000..43e0c33 --- /dev/null +++ b/include/unitree_legged_sdk/comm.h @@ -0,0 +1,179 @@ +#ifndef GO1_PRO_UNITREE_LEGGED_COMM_H_ +#define GO1_PRO_UNITREE_LEGGED_COMM_H_ + +#include +#include + +namespace UNITREE_LEGGED_SDK { + +constexpr int HIGHLEVEL = 0xee; +constexpr int LOWLEVEL = 0xff; +constexpr int TRIGERLEVEL = 0xf0; +constexpr double PosStopF = 2.146E+9f; +constexpr double VelStopF = 16000.0f; + +extern const int HIGH_CMD_LENGTH; +extern const int HIGH_STATE_LENGTH; +extern const int LOW_CMD_LENGTH; +extern const int LOW_STATE_LENGTH; + +#pragma pack(push, 1) + +struct BmsCmd { + uint8_t off; + std::array reserve; +}; + +struct BmsState { + uint8_t version_h; + uint8_t version_l; + uint8_t bms_status; + uint8_t SOC; + int32_t current; + uint16_t cycle; + std::array BQ_NTC; + std::array MCU_NTC; + std::array cell_vol; +}; + +struct Cartesian { + float x; + float y; + float z; +}; + +struct IMU { + std::array quaternion; + std::array gyroscope; + std::array accelerometer; + std::array rpy; + int8_t temperature; +}; + +struct LED { + uint8_t r; + uint8_t g; + uint8_t b; +}; + +struct MotorState { + uint8_t mode; + float q; + float dq; + float ddq; + float tauEst; + float q_raw; + float dq_raw; + float ddq_raw; + int8_t temperature; + std::array reserve; +}; + +struct MotorCmd { + uint8_t mode; + float q; + float dq; + float tau; + float Kp; + float Kd; + std::array reserve; +}; + +struct LowState { + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + std::array SN; + std::array version; + uint16_t bandWidth; + IMU imu; + std::array motorState; + BmsState bms; + std::array footForce; + std::array footForceEst; + uint32_t tick; + std::array wirelessRemote; + uint32_t reserve; + uint32_t crc; +}; + +struct LowCmd { + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + std::array SN; + std::array version; + uint16_t bandWidth; + std::array motorCmd; + BmsCmd bms; + std::array wirelessRemote; + uint32_t reserve; + uint32_t crc; +}; + +struct HighState { + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + std::array SN; + std::array version; + uint16_t bandWidth; + IMU imu; + std::array motorState; + BmsState bms; + std::array footForce; + std::array footForceEst; + uint8_t mode; + float progress; + uint8_t gaitType; + float footRaiseHeight; + std::array position; + float bodyHeight; + std::array velocity; + float yawSpeed; + std::array rangeObstacle; + std::array footPosition2Body; + std::array footSpeed2Body; + std::array wirelessRemote; + uint32_t reserve; + uint32_t crc; +}; + +struct HighCmd { + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + std::array SN; + std::array version; + uint16_t bandWidth; + uint8_t mode; + uint8_t gaitType; + uint8_t speedLevel; + float footRaiseHeight; + float bodyHeight; + std::array position; + std::array euler; + std::array velocity; + float yawSpeed; + BmsCmd bms; + std::array led; + std::array wirelessRemote; + uint32_t reserve; + uint32_t crc; +}; + +#pragma pack(pop) + +struct UDPState { + unsigned long long TotalCount; + unsigned long long SendCount; + unsigned long long RecvCount; + unsigned long long SendError; + unsigned long long FlagError; + unsigned long long RecvCRCError; + unsigned long long RecvLoseError; +}; + +} // namespace UNITREE_LEGGED_SDK + +#endif diff --git a/include/unitree_legged_sdk/go1_const.h b/include/unitree_legged_sdk/go1_const.h new file mode 100644 index 0000000..37e4339 --- /dev/null +++ b/include/unitree_legged_sdk/go1_const.h @@ -0,0 +1,11 @@ +#ifndef GO1_PRO_UNITREE_LEGGED_GO1_CONST_H_ +#define GO1_PRO_UNITREE_LEGGED_GO1_CONST_H_ +namespace UNITREE_LEGGED_SDK { +constexpr double go1_Hip_max = 1.047; +constexpr double go1_Hip_min = -1.047; +constexpr double go1_Thigh_max = 2.966; +constexpr double go1_Thigh_min = -0.663; +constexpr double go1_Calf_max = -0.837; +constexpr double go1_Calf_min = -2.721; +} // namespace UNITREE_LEGGED_SDK +#endif diff --git a/include/unitree_legged_sdk/joystick.h b/include/unitree_legged_sdk/joystick.h new file mode 100644 index 0000000..27ab573 --- /dev/null +++ b/include/unitree_legged_sdk/joystick.h @@ -0,0 +1,27 @@ +#ifndef GO1_PRO_UNITREE_LEGGED_JOYSTICK_H_ +#define GO1_PRO_UNITREE_LEGGED_JOYSTICK_H_ + +#include + +typedef union { + struct { + uint8_t R1 : 1; uint8_t L1 : 1; uint8_t start : 1; uint8_t select : 1; + uint8_t R2 : 1; uint8_t L2 : 1; uint8_t F1 : 1; uint8_t F2 : 1; + uint8_t A : 1; uint8_t B : 1; uint8_t X : 1; uint8_t Y : 1; + uint8_t up : 1; uint8_t right : 1; uint8_t down : 1; uint8_t left : 1; + } components; + uint16_t value; +} xKeySwitchUnion; + +typedef struct { + uint8_t head[2]; + xKeySwitchUnion btn; + float lx; + float rx; + float ry; + float L2; + float ly; + uint8_t idle[16]; +} xRockerBtnDataStruct; + +#endif diff --git a/include/unitree_legged_sdk/loop.h b/include/unitree_legged_sdk/loop.h new file mode 100644 index 0000000..c2e2bdf --- /dev/null +++ b/include/unitree_legged_sdk/loop.h @@ -0,0 +1,48 @@ +#ifndef GO1_PRO_UNITREE_LEGGED_LOOP_H_ +#define GO1_PRO_UNITREE_LEGGED_LOOP_H_ + +#include +#include +#include +#include + +namespace UNITREE_LEGGED_SDK { + +constexpr int THREAD_PRIORITY = 95; +typedef boost::function Callback; + +class Loop { + public: + Loop(std::string name, float period, int bindCPU = -1) + : _name(name), _period(period), _bindCPU(bindCPU) {} + virtual ~Loop(); + void start(); + void shutdown(); + virtual void functionCB() = 0; + + private: + void entryFunc(); + std::string _name; + float _period; + int _bindCPU; + bool _bind_cpu_flag = false; + bool _isrunning = false; + std::atomic _running_atomic{false}; + std::thread _thread; +}; + +class LoopFunc : public Loop { + public: + LoopFunc(std::string name, float period, const Callback& cb) + : Loop(name, period), _fp(cb) {} + LoopFunc(std::string name, float period, int bindCPU, const Callback& cb) + : Loop(name, period, bindCPU), _fp(cb) {} + void functionCB() override { _fp(); } + + private: + boost::function _fp; +}; + +} // namespace UNITREE_LEGGED_SDK + +#endif diff --git a/include/unitree_legged_sdk/quadruped.h b/include/unitree_legged_sdk/quadruped.h new file mode 100644 index 0000000..18941e8 --- /dev/null +++ b/include/unitree_legged_sdk/quadruped.h @@ -0,0 +1,33 @@ +#ifndef GO1_PRO_UNITREE_LEGGED_QUADRUPED_H_ +#define GO1_PRO_UNITREE_LEGGED_QUADRUPED_H_ + +#include + +namespace UNITREE_LEGGED_SDK { + +enum class LeggedType { Aliengo, A1, Go1, B1 }; + +std::string VersionSDK(); +int InitEnvironment(); + +constexpr int FR_ = 0; +constexpr int FL_ = 1; +constexpr int RR_ = 2; +constexpr int RL_ = 3; + +constexpr int FR_0 = 0; +constexpr int FR_1 = 1; +constexpr int FR_2 = 2; +constexpr int FL_0 = 3; +constexpr int FL_1 = 4; +constexpr int FL_2 = 5; +constexpr int RR_0 = 6; +constexpr int RR_1 = 7; +constexpr int RR_2 = 8; +constexpr int RL_0 = 9; +constexpr int RL_1 = 10; +constexpr int RL_2 = 11; + +} // namespace UNITREE_LEGGED_SDK + +#endif diff --git a/include/unitree_legged_sdk/safety.h b/include/unitree_legged_sdk/safety.h new file mode 100644 index 0000000..4d8b959 --- /dev/null +++ b/include/unitree_legged_sdk/safety.h @@ -0,0 +1,30 @@ +#ifndef GO1_PRO_UNITREE_LEGGED_SAFETY_H_ +#define GO1_PRO_UNITREE_LEGGED_SAFETY_H_ + +#include "comm.h" +#include "quadruped.h" + +namespace UNITREE_LEGGED_SDK { + +class Safety { + public: + Safety(LeggedType type); + ~Safety(); + void PositionLimit(LowCmd& cmd); + int PowerProtect(LowCmd& cmd, LowState& state, int factor); + int PositionProtect(LowCmd& cmd, LowState& state, double limit = 0.087); + + private: + int WattLimit; + int Wcount; + double Hip_max; + double Hip_min; + double Thigh_max; + double Thigh_min; + double Calf_max; + double Calf_min; +}; + +} // namespace UNITREE_LEGGED_SDK + +#endif diff --git a/include/unitree_legged_sdk/udp.h b/include/unitree_legged_sdk/udp.h new file mode 100644 index 0000000..b88e07d --- /dev/null +++ b/include/unitree_legged_sdk/udp.h @@ -0,0 +1,62 @@ +#ifndef GO1_PRO_UNITREE_LEGGED_UDP_H_ +#define GO1_PRO_UNITREE_LEGGED_UDP_H_ + +#include "comm.h" +#include "unitree_legged_sdk/quadruped.h" +#include + +namespace UNITREE_LEGGED_SDK { + +constexpr int UDP_CLIENT_PORT = 8080; +constexpr int UDP_SERVER_PORT = 8007; +constexpr char UDP_SERVER_IP_BASIC[] = "192.168.123.10"; +constexpr char UDP_SERVER_IP_SPORT[] = "192.168.123.161"; + +enum RecvEnum { nonBlock = 0x00, block = 0x01, blockTimeout = 0x02 }; + +class UDP { + public: + UDP(uint8_t level, uint16_t localPort, const char* targetIP, uint16_t targetPort); + UDP(uint16_t localPort, const char* targetIP, uint16_t targetPort, + int sendLength, int recvLength, bool initiativeDisconnect = false, + RecvEnum recvType = RecvEnum::nonBlock); + UDP(uint16_t localPort, int sendLength, int recvLength, + bool initiativeDisconnect = false, RecvEnum recvType = RecvEnum::nonBlock, + bool setIpPort = false); + ~UDP(); + + UDP(const UDP&) = delete; + UDP& operator=(const UDP&) = delete; + + void SetIpPort(const char* targetIP, uint16_t targetPort); + void SetRecvTimeout(int time); + void SetDisconnectTime(float callback_dt, float disconnectTime); + void SetAccessibleTime(float callback_dt, float accessibleTime); + + int Send(); + int Recv(); + + void InitCmdData(HighCmd& cmd); + void InitCmdData(LowCmd& cmd); + int SetSend(char* data); + int SetSend(HighCmd& cmd); + int SetSend(LowCmd& cmd); + void GetRecv(char* data); + void GetRecv(HighState& state); + void GetRecv(LowState& state); + + UDPState udpState; + char* targetIP; + uint16_t targetPort; + char* localIP; + uint16_t localPort; + bool accessible = false; + + private: + struct Impl; + Impl* impl_; +}; + +} // namespace UNITREE_LEGGED_SDK + +#endif diff --git a/include/unitree_legged_sdk/unitree_legged_sdk.h b/include/unitree_legged_sdk/unitree_legged_sdk.h new file mode 100644 index 0000000..b9c34b5 --- /dev/null +++ b/include/unitree_legged_sdk/unitree_legged_sdk.h @@ -0,0 +1,14 @@ +#ifndef GO1_PRO_UNITREE_LEGGED_SDK_H_ +#define GO1_PRO_UNITREE_LEGGED_SDK_H_ + +#include "comm.h" +#include "joystick.h" +#include "loop.h" +#include "quadruped.h" +#include "safety.h" +#include "udp.h" +#include + +#define UT UNITREE_LEGGED_SDK + +#endif diff --git a/robot_interface.py b/robot_interface.py index 823b965..bea29fc 100644 --- a/robot_interface.py +++ b/robot_interface.py @@ -1,4 +1,3 @@ """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/src/loop.cpp b/src/loop.cpp new file mode 100644 index 0000000..974eba8 --- /dev/null +++ b/src/loop.cpp @@ -0,0 +1,50 @@ +#include "unitree_legged_sdk/loop.h" + +#include + +#if defined(__linux__) +#include +#include +#endif + +namespace UNITREE_LEGGED_SDK { + +Loop::~Loop() { shutdown(); } + +void Loop::start() { + if (_running_atomic.exchange(true)) return; + _isrunning = true; + _thread = std::thread(&Loop::entryFunc, this); +} + +void Loop::shutdown() { + _running_atomic.store(false); + _isrunning = false; + if (_thread.joinable()) _thread.join(); +} + +void Loop::entryFunc() { +#if defined(__linux__) + if (_bindCPU >= 0) { + cpu_set_t cpu_set; + CPU_ZERO(&cpu_set); + CPU_SET(_bindCPU, &cpu_set); + _bind_cpu_flag = pthread_setaffinity_np( + pthread_self(), sizeof(cpu_set), &cpu_set) == 0; + } +#else + (void)_bindCPU; + _bind_cpu_flag = false; +#endif + using Clock = std::chrono::steady_clock; + const auto period = std::chrono::duration_cast( + std::chrono::duration(_period)); + auto next = Clock::now(); + while (_running_atomic.load()) { + next += period; + functionCB(); + std::this_thread::sleep_until(next); + } +} + +} // namespace UNITREE_LEGGED_SDK diff --git a/src/pro_codec.cpp b/src/pro_codec.cpp new file mode 100644 index 0000000..6f2d5f3 --- /dev/null +++ b/src/pro_codec.cpp @@ -0,0 +1,391 @@ +#include "pro_codec.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#if defined(__APPLE__) +#include +#elif defined(__linux__) +#include +#endif + +#ifndef GO1_PRO_SOURCE_STATE_FILE +#define GO1_PRO_SOURCE_STATE_FILE "" +#endif + +#ifndef GO1_PRO_INSTALL_STATE_FILE +#define GO1_PRO_INSTALL_STATE_FILE "" +#endif + +namespace go1_pro_internal { +namespace { + +constexpr std::size_t kStateSize = 4168; +constexpr std::size_t kMotorOffset = 22; +constexpr std::size_t kMotorWireSize = 27; +constexpr std::size_t kCrcOffset = 612; +constexpr uint32_t kCrcPoly = 0x04c11db7u; + +uint16_t GetU16Le(const uint8_t* p) { + return static_cast(p[0]) | + static_cast(static_cast(p[1]) << 8); +} + +int16_t GetI16Le(const uint8_t* p) { + return static_cast(GetU16Le(p)); +} + +uint32_t GetU32Le(const uint8_t* p) { + return static_cast(p[0]) | + (static_cast(p[1]) << 8) | + (static_cast(p[2]) << 16) | + (static_cast(p[3]) << 24); +} + +int32_t GetI32Le(const uint8_t* p) { + return static_cast(GetU32Le(p)); +} + +float GetF32Le(const uint8_t* p) { + const uint32_t raw = GetU32Le(p); + float result; + std::memcpy(&result, &raw, sizeof(result)); + return result; +} + +void PutU16Le(uint8_t* p, uint16_t value) { + p[0] = static_cast(value); + p[1] = static_cast(value >> 8); +} + +void PutU32Le(uint8_t* p, uint32_t value) { + p[0] = static_cast(value); + p[1] = static_cast(value >> 8); + p[2] = static_cast(value >> 16); + p[3] = static_cast(value >> 24); +} + +void PutF32Le(uint8_t* p, float value) { + uint32_t raw; + static_assert(sizeof(raw) == sizeof(value), "float must be 32-bit"); + std::memcpy(&raw, &value, sizeof(raw)); + PutU32Le(p, raw); +} + +double RoundHalfEven(double value, double scale) { + const double scaled = value * scale; + const double floor_value = std::floor(scaled); + const double fraction = scaled - floor_value; + constexpr double kEpsilon = 1e-12; + double rounded = floor_value; + if (fraction > 0.5 + kEpsilon) { + rounded += 1.0; + } else if (fraction >= 0.5 - kEpsilon && std::fmod(floor_value, 2.0) != 0.0) { + rounded += 1.0; + } + return rounded / scale; +} + +void EncodeTau(uint8_t* p, double tau) { + tau = RoundHalfEven(tau, 100.0); + int integer = static_cast(tau); + const double fraction = tau - static_cast(integer); + bool negative = tau < 0.0; + if (negative) integer = 255 + integer; + if (fraction == 0.0) negative = false; + int encoded_fraction = static_cast(fraction * 256.0); + if (negative) encoded_fraction = 256 + encoded_fraction; + p[0] = static_cast(encoded_fraction); + p[1] = static_cast(integer); +} + +void EncodeKp(uint8_t* p, double kp) { + const double base_double = std::floor(kp); + const int base = static_cast(base_double); + const int fraction = static_cast(RoundHalfEven(kp - base_double, 10.0) * 10.0); + const int value = fraction < 5 ? base * 32 + fraction * 3 + : base * 32 + (fraction - 1) * 3 + 4; + PutU16Le(p, static_cast(value)); +} + +void EncodeKd(uint8_t* p, double kd) { + static constexpr uint8_t kFraction[10] = {0x0, 0x1, 0x3, 0x4, 0x6, + 0x8, 0x9, 0xb, 0xc, 0xe}; + const int integer = static_cast(kd); + const int fraction = static_cast(RoundHalfEven(kd - integer, 10.0) * 10.0); + const uint8_t nibble = fraction >= 0 && fraction < 10 ? kFraction[fraction] : 0; + PutU16Le(p, static_cast((integer << 4) | nibble)); +} + +void EncodeMotor(uint8_t* p, const UNITREE_LEGGED_SDK::MotorCmd& motor) { + p[0] = motor.mode; + PutF32Le(p + 1, motor.q); + PutF32Le(p + 5, motor.dq); + EncodeTau(p + 9, motor.tau); + EncodeKp(p + 11, motor.Kp); + EncodeKd(p + 13, motor.Kd); + for (std::size_t i = 0; i < motor.reserve.size(); ++i) { + PutU32Le(p + 15 + i * 4, motor.reserve[i]); + } +} + +bool ReadableFile(const std::string& path) { + if (path.empty()) return false; + std::ifstream stream(path, std::ios::binary); + return stream.good(); +} + +std::string ExecutableDirectory() { +#if defined(__APPLE__) + uint32_t size = 0; + _NSGetExecutablePath(nullptr, &size); + std::string path(size, '\0'); + if (_NSGetExecutablePath(&path[0], &size) != 0) return {}; + path.resize(std::strlen(path.c_str())); +#elif defined(__linux__) + char buffer[PATH_MAX] = {}; + const ssize_t size = readlink("/proc/self/exe", buffer, sizeof(buffer) - 1); + if (size <= 0) return {}; + buffer[size] = '\0'; + std::string path(buffer); +#else + return {}; +#endif + const std::size_t slash = path.find_last_of("/"); + return slash == std::string::npos ? std::string{} : path.substr(0, slash); +} + +} // namespace + +void InitLowCmd(UNITREE_LEGGED_SDK::LowCmd& cmd) { + std::memset(&cmd, 0, sizeof(cmd)); + cmd.head = {{0xfe, 0xef}}; + cmd.levelFlag = UNITREE_LEGGED_SDK::LOWLEVEL; + cmd.bandWidth = 0x3ac0; + for (auto& motor : cmd.motorCmd) { + motor.mode = 0x0a; + motor.q = static_cast(UNITREE_LEGGED_SDK::PosStopF); + motor.dq = static_cast(UNITREE_LEGGED_SDK::VelStopF); + } +} + +void InitHighCmd(UNITREE_LEGGED_SDK::HighCmd& cmd) { + std::memset(&cmd, 0, sizeof(cmd)); + cmd.head = {{0xfe, 0xef}}; + cmd.levelFlag = UNITREE_LEGGED_SDK::HIGHLEVEL; +} + +ProCodec::ProCodec(const std::string& state_path) { + std::ifstream stream(state_path, std::ios::binary); + if (!stream) { + throw std::runtime_error("cannot open Go1 PRO Blowfish state file: " + state_path); + } + std::array data{}; + stream.read(reinterpret_cast(data.data()), data.size()); + if (stream.gcount() != static_cast(data.size())) { + throw std::runtime_error("Go1 PRO Blowfish state file must contain at least 4168 bytes: " + state_path); + } + for (std::size_t i = 0; i < p_.size(); ++i) p_[i] = GetU32Le(data.data() + i * 4); + const uint8_t* boxes = data.data() + p_.size() * 4; + for (std::size_t box = 0; box < s_.size(); ++box) { + for (std::size_t i = 0; i < s_[box].size(); ++i) { + s_[box][i] = GetU32Le(boxes + (box * 256 + i) * 4); + } + } +} + +std::string ProCodec::FindStateFile() { + const char* environment = std::getenv("GO1_PRO_BLOWFISH_STATE"); + const std::string executable_dir = ExecutableDirectory(); + const std::string candidates[] = { + environment ? environment : "", + GO1_PRO_SOURCE_STATE_FILE, + GO1_PRO_INSTALL_STATE_FILE, + executable_dir.empty() ? "" : executable_dir + "/../share/go1_pro_sdk/blowfish_state.bin", + executable_dir.empty() ? "" : executable_dir + "/share/go1_pro_sdk/blowfish_state.bin", + "go1_pro_sdk/_data/blowfish_state.bin", + "../share/go1_pro_sdk/blowfish_state.bin", + "/usr/local/share/go1_pro_sdk/blowfish_state.bin", + }; + for (const auto& candidate : candidates) { + if (ReadableFile(candidate)) return candidate; + } + throw std::runtime_error( + "Go1 PRO Blowfish state not found; set GO1_PRO_BLOWFISH_STATE to blowfish_state.bin"); +} + +uint32_t ProCodec::Crc32(const uint8_t* data, std::size_t size) { + static const std::array table = [] { + std::array result{}; + for (uint32_t byte = 0; byte < 256; ++byte) { + uint32_t crc = byte << 24; + for (int bit = 0; bit < 8; ++bit) { + crc = (crc & 0x80000000u) ? (crc << 1) ^ kCrcPoly : crc << 1; + } + result[byte] = crc; + } + return result; + }(); + uint32_t crc = 0xffffffffu; + for (std::size_t offset = 0; offset + 4 <= size; offset += 4) { + const uint32_t word = GetU32Le(data + offset); + for (int shift = 24; shift >= 0; shift -= 8) { + crc = (crc << 8) ^ table[((crc >> 24) ^ (word >> shift)) & 0xffu]; + } + } + return crc; +} + +uint32_t ProCodec::F(uint32_t value) const { + return ((s_[0][(value >> 24) & 0xffu] + s_[1][(value >> 16) & 0xffu]) ^ + s_[2][(value >> 8) & 0xffu]) + s_[3][value & 0xffu]; +} + +void ProCodec::EncryptBlock(const uint8_t* input, uint8_t* output) const { + uint32_t left = GetU32Le(input); + uint32_t right = GetU32Le(input + 4); + for (int i = 0; i < 16; ++i) { + left ^= p_[i]; + right ^= F(left); + const uint32_t temporary = left; left = right; right = temporary; + } + const uint32_t temporary = left; left = right; right = temporary; + right ^= p_[16]; + left ^= p_[17]; + PutU32Le(output, left); + PutU32Le(output + 4, right); +} + +void ProCodec::DecryptBlock(const uint8_t* input, uint8_t* output) const { + uint32_t left = GetU32Le(input); + uint32_t right = GetU32Le(input + 4); + for (int i = 17; i > 1; --i) { + left ^= p_[i]; + right ^= F(left); + const uint32_t temporary = left; left = right; right = temporary; + } + const uint32_t temporary = left; left = right; right = temporary; + right ^= p_[1]; + left ^= p_[0]; + PutU32Le(output, left); + PutU32Le(output + 4, right); +} + +void ProCodec::Encrypt(const uint8_t* input, uint8_t* output, std::size_t size) const { + if (size % 8 != 0) throw std::invalid_argument("Blowfish input must be 8-byte aligned"); + for (std::size_t offset = 0; offset < size; offset += 8) { + EncryptBlock(input + offset, output + offset); + } +} + +void ProCodec::Decrypt(const uint8_t* input, uint8_t* output, std::size_t size) const { + if (size % 8 != 0) throw std::invalid_argument("Blowfish input must be 8-byte aligned"); + for (std::size_t offset = 0; offset < size; offset += 8) { + DecryptBlock(input + offset, output + offset); + } +} + +std::array ProCodec::EncodeLowCmd( + const UNITREE_LEGGED_SDK::LowCmd& cmd) const { + std::array plain{}; + plain[0] = cmd.head[0]; + plain[1] = cmd.head[1]; + plain[2] = cmd.levelFlag; + plain[3] = cmd.frameReserve; + for (std::size_t i = 0; i < 2; ++i) { + PutU32Le(plain.data() + 4 + i * 4, cmd.SN[i]); + PutU32Le(plain.data() + 12 + i * 4, cmd.version[i]); + } + plain[20] = static_cast(cmd.bandWidth >> 8); + plain[21] = static_cast(cmd.bandWidth); + for (std::size_t i = 0; i < cmd.motorCmd.size(); ++i) { + EncodeMotor(plain.data() + kMotorOffset + i * kMotorWireSize, cmd.motorCmd[i]); + } + plain[562] = cmd.bms.off; + std::copy(cmd.bms.reserve.begin(), cmd.bms.reserve.end(), plain.begin() + 563); + std::copy(cmd.wirelessRemote.begin(), cmd.wirelessRemote.end(), plain.begin() + 566); + PutU32Le(plain.data() + 606, cmd.reserve); + PutU32Le(plain.data() + kCrcOffset, Crc32(plain.data(), kCrcOffset)); + + std::array encrypted{}; + Encrypt(plain.data(), encrypted.data(), encrypted.size()); + return encrypted; +} + +bool ProCodec::DecodeLowState(const uint8_t* encrypted, std::size_t size, + UNITREE_LEGGED_SDK::LowState& state) const { + if (size < kLowStateParsedSize) return false; + const std::size_t aligned = (size / 8) * 8; + if (aligned < kLowStateParsedSize) return false; + std::vector data(aligned); + Decrypt(encrypted, data.data(), aligned); + if (data[0] != 0xfe || data[1] != 0xef || + data[2] != UNITREE_LEGGED_SDK::LOWLEVEL || data[3] != 0) { + return false; + } + + std::memset(&state, 0, sizeof(state)); + state.head = {{data[0], data[1]}}; + state.levelFlag = data[2]; + state.frameReserve = data[3]; + for (std::size_t i = 0; i < 2; ++i) { + state.SN[i] = GetU32Le(data.data() + 4 + i * 4); + state.version[i] = GetU32Le(data.data() + 12 + i * 4); + } + state.bandWidth = GetU16Le(data.data() + 20); + for (std::size_t i = 0; i < 4; ++i) state.imu.quaternion[i] = GetF32Le(data.data() + 22 + i * 4); + for (std::size_t i = 0; i < 3; ++i) { + state.imu.gyroscope[i] = GetF32Le(data.data() + 38 + i * 4); + state.imu.accelerometer[i] = GetF32Le(data.data() + 50 + i * 4); + state.imu.rpy[i] = GetF32Le(data.data() + 62 + i * 4); + } + state.imu.temperature = static_cast(data[74]); + + for (std::size_t i = 0; i < state.motorState.size(); ++i) { + const uint8_t* motor = data.data() + 75 + i * 32; + auto& output = state.motorState[i]; + output.mode = motor[0]; + output.q = GetF32Le(motor + 1); + output.dq = GetF32Le(motor + 5); + output.ddq = static_cast(GetI16Le(motor + 9)); + output.tauEst = static_cast(GetI16Le(motor + 11)) * 0.00390625f; + output.q_raw = GetF32Le(motor + 13); + output.dq_raw = GetF32Le(motor + 17); + output.ddq_raw = static_cast(GetI16Le(motor + 21)); + output.temperature = static_cast(motor[23]); + output.reserve[0] = GetU32Le(motor + 24); + output.reserve[1] = GetU32Le(motor + 28); + } + + const uint8_t* bms = data.data() + 715; + state.bms.version_h = bms[0]; + state.bms.version_l = bms[1]; + state.bms.bms_status = bms[2]; + state.bms.SOC = bms[3]; + state.bms.current = GetI32Le(bms + 4); + state.bms.cycle = GetU16Le(bms + 8); + state.bms.BQ_NTC = {{static_cast(bms[10]), static_cast(bms[11])}}; + state.bms.MCU_NTC = {{static_cast(bms[12]), static_cast(bms[13])}}; + for (std::size_t i = 0; i < 5; ++i) state.bms.cell_vol[i] = GetU16Le(bms + 14 + i * 2); + + // The PRO state packs these fields differently from the public EDU struct. + // Keep the reverse-engineered mapping used by the Python implementation. + const std::size_t force_offsets[4] = {739, 751, 753, 755}; + const std::size_t force_est_offsets[4] = {747, 759, 761, 763}; + for (std::size_t i = 0; i < 4; ++i) { + state.footForce[i] = GetI16Le(data.data() + force_offsets[i]); + state.footForceEst[i] = GetI16Le(data.data() + force_est_offsets[i]); + } + std::copy(data.begin() + 759, data.begin() + 799, state.wirelessRemote.begin()); + state.reserve = GetU32Le(data.data() + 799); + state.crc = GetU32Le(data.data() + 803); + return true; +} + +} // namespace go1_pro_internal diff --git a/src/pro_codec.h b/src/pro_codec.h new file mode 100644 index 0000000..a8272c0 --- /dev/null +++ b/src/pro_codec.h @@ -0,0 +1,48 @@ +#ifndef GO1_PRO_SDK_PRO_CODEC_H_ +#define GO1_PRO_SDK_PRO_CODEC_H_ + +#include "unitree_legged_sdk/comm.h" + +#include +#include +#include +#include +#include + +namespace go1_pro_internal { + +constexpr std::size_t kLowCmdWireSize = 616; +constexpr std::size_t kLowStateDatagramSize = 858; +constexpr std::size_t kLowStateEncryptedSize = 856; +constexpr std::size_t kLowStateParsedSize = 807; + +void InitLowCmd(UNITREE_LEGGED_SDK::LowCmd& cmd); +void InitHighCmd(UNITREE_LEGGED_SDK::HighCmd& cmd); + +class ProCodec { + public: + explicit ProCodec(const std::string& state_path); + + static std::string FindStateFile(); + static uint32_t Crc32(const uint8_t* data, std::size_t size); + + std::array EncodeLowCmd( + const UNITREE_LEGGED_SDK::LowCmd& cmd) const; + bool DecodeLowState(const uint8_t* encrypted, std::size_t size, + UNITREE_LEGGED_SDK::LowState& state) const; + + void Encrypt(const uint8_t* input, uint8_t* output, std::size_t size) const; + void Decrypt(const uint8_t* input, uint8_t* output, std::size_t size) const; + + private: + uint32_t F(uint32_t value) const; + void EncryptBlock(const uint8_t* input, uint8_t* output) const; + void DecryptBlock(const uint8_t* input, uint8_t* output) const; + + std::array p_{}; + std::array, 4> s_{}; +}; + +} // namespace go1_pro_internal + +#endif diff --git a/src/quadruped.cpp b/src/quadruped.cpp new file mode 100644 index 0000000..d84a036 --- /dev/null +++ b/src/quadruped.cpp @@ -0,0 +1,25 @@ +#include "unitree_legged_sdk/quadruped.h" +#include "unitree_legged_sdk/comm.h" + +#if defined(__unix__) || defined(__APPLE__) +#include +#endif + +namespace UNITREE_LEGGED_SDK { + +const int HIGH_CMD_LENGTH = sizeof(HighCmd); +const int HIGH_STATE_LENGTH = sizeof(HighState); +const int LOW_CMD_LENGTH = 616; +const int LOW_STATE_LENGTH = 858; + +std::string VersionSDK() { return "go1-pro-compat-3.8.6"; } + +int InitEnvironment() { +#if defined(__unix__) || defined(__APPLE__) + return mlockall(MCL_CURRENT | MCL_FUTURE); +#else + return 0; +#endif +} + +} // namespace UNITREE_LEGGED_SDK diff --git a/src/safety.cpp b/src/safety.cpp new file mode 100644 index 0000000..86aa91a --- /dev/null +++ b/src/safety.cpp @@ -0,0 +1,77 @@ +#include "unitree_legged_sdk/safety.h" + +#include +#include + +namespace UNITREE_LEGGED_SDK { +namespace { + +constexpr double kTorqueLimit[12] = { + 23.7, 23.7, 35.55, 23.7, 23.7, 35.55, + 23.7, 23.7, 35.55, 23.7, 23.7, 35.55, +}; + +bool IsPositionStop(float value) { + return std::fabs(static_cast(value)) >= PosStopF * 0.1; +} + +} // namespace + +Safety::Safety(LeggedType type) + : WattLimit(0), Wcount(0), + Hip_max(0.78), Hip_min(-0.78), + Thigh_max(3.50), Thigh_min(-0.60), + Calf_max(-0.95), Calf_min(-2.70) { + (void)type; +} + +Safety::~Safety() = default; + +void Safety::PositionLimit(LowCmd& cmd) { + for (std::size_t i = 0; i < 12; ++i) { + auto& motor = cmd.motorCmd[i]; + if (IsPositionStop(motor.q)) continue; + const std::size_t joint = i % 3; + const double minimum = joint == 0 ? Hip_min : (joint == 1 ? Thigh_min : Calf_min); + const double maximum = joint == 0 ? Hip_max : (joint == 1 ? Thigh_max : Calf_max); + motor.q = static_cast(std::max(minimum, std::min(maximum, static_cast(motor.q)))); + } +} + +int Safety::PowerProtect(LowCmd& cmd, LowState& state, int factor) { + if (factor < 1 || factor > 10) return -1; + WattLimit = factor; + int limited = 0; + for (std::size_t i = 0; i < 12; ++i) { + const double maximum = kTorqueLimit[i]; + if (std::fabs(static_cast(cmd.motorCmd[i].tau)) > maximum * 5.0 || + std::fabs(static_cast(state.motorState[i].tauEst)) > maximum) { + return -1; + } + const double limit = maximum * static_cast(factor) / 10.0; + const float bounded = static_cast( + std::max(-limit, std::min(limit, static_cast(cmd.motorCmd[i].tau)))); + if (bounded != cmd.motorCmd[i].tau) { + cmd.motorCmd[i].tau = bounded; + ++limited; + } + } + Wcount += limited; + return limited; +} + +int Safety::PositionProtect(LowCmd& cmd, LowState& state, double limit) { + int protected_count = 0; + for (std::size_t i = 0; i < 12; ++i) { + auto& motor = cmd.motorCmd[i]; + if (IsPositionStop(motor.q)) continue; + if (std::fabs(static_cast(motor.q - state.motorState[i].q)) > limit) { + motor.Kp = 0.0f; + motor.Kd = 0.0f; + ++protected_count; + } + } + return protected_count; +} + +} // namespace UNITREE_LEGGED_SDK diff --git a/src/udp.cpp b/src/udp.cpp new file mode 100644 index 0000000..ccead1e --- /dev/null +++ b/src/udp.cpp @@ -0,0 +1,310 @@ +#include "unitree_legged_sdk/udp.h" + +#include "pro_codec.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace UNITREE_LEGGED_SDK { +namespace { + +char* Duplicate(const char* value) { + if (!value) return nullptr; + const std::size_t size = std::strlen(value) + 1; + char* result = new char[size]; + std::memcpy(result, value, size); + return result; +} + +std::runtime_error SocketError(const char* operation) { + return std::runtime_error(std::string(operation) + ": " + std::strerror(errno)); +} + +} // namespace + +struct UDP::Impl { + int socket_fd = -1; + int send_length = 0; + int recv_length = 0; + RecvEnum recv_type = RecvEnum::nonBlock; + bool pro_low_level = false; + bool target_configured = false; + std::mutex send_mutex; + std::mutex recv_mutex; + std::mutex socket_recv_mutex; + std::mutex state_mutex; + std::unique_ptr codec; + std::vector send_buffer; + std::vector recv_buffer; + std::vector wire_recv_buffer; + std::size_t received_size = 0; + LowState low_state{}; + bool have_low_state = false; +}; + +namespace { + +template +void ConfigureBlocking(ImplType& impl) { + const int flags = fcntl(impl.socket_fd, F_GETFL, 0); + if (flags < 0) throw SocketError("fcntl(F_GETFL)"); + const int next = impl.recv_type == RecvEnum::nonBlock ? flags | O_NONBLOCK : flags & ~O_NONBLOCK; + if (fcntl(impl.socket_fd, F_SETFL, next) < 0) throw SocketError("fcntl(F_SETFL)"); +} + +template +void BindSocket(UDP& udp, ImplType& impl, uint16_t port) { + impl.socket_fd = ::socket(AF_INET, SOCK_DGRAM, 0); + if (impl.socket_fd < 0) throw SocketError("socket"); + const int reuse = 1; + setsockopt(impl.socket_fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + sockaddr_in local{}; + local.sin_family = AF_INET; + local.sin_port = htons(port); + local.sin_addr.s_addr = htonl(INADDR_ANY); + if (::bind(impl.socket_fd, reinterpret_cast(&local), sizeof(local)) < 0) { + throw SocketError("bind"); + } + udp.localPort = port; + udp.localIP = Duplicate("0.0.0.0"); + ConfigureBlocking(impl); +} + +} // namespace + +UDP::UDP(uint8_t level, uint16_t port, const char* ip, uint16_t target_port) + : udpState{}, targetIP(nullptr), targetPort(0), localIP(nullptr), localPort(0), + impl_(new Impl) { + try { + if (level != LOWLEVEL) { + throw std::invalid_argument("Go1 PRO compatibility library supports LOWLEVEL UDP only"); + } + impl_->pro_low_level = true; + impl_->send_length = static_cast(go1_pro_internal::kLowCmdWireSize); + impl_->recv_length = static_cast(go1_pro_internal::kLowStateDatagramSize); + impl_->send_buffer.resize(impl_->send_length); + impl_->recv_buffer.resize(2048); + impl_->wire_recv_buffer.resize(2048); + impl_->codec.reset(new go1_pro_internal::ProCodec( + go1_pro_internal::ProCodec::FindStateFile())); + BindSocket(*this, *impl_, port); + SetIpPort(ip, target_port); + } catch (...) { + if (impl_->socket_fd >= 0) ::close(impl_->socket_fd); + delete[] localIP; + delete impl_; + throw; + } +} + +UDP::UDP(uint16_t port, const char* ip, uint16_t target_port, + int send_length, int recv_length, bool initiative_disconnect, RecvEnum recv_type) + : udpState{}, targetIP(nullptr), targetPort(0), localIP(nullptr), localPort(0), + impl_(new Impl) { + (void)initiative_disconnect; + try { + impl_->send_length = send_length; + impl_->recv_length = recv_length; + impl_->recv_type = recv_type; + impl_->send_buffer.resize(send_length); + impl_->recv_buffer.resize(recv_length); + impl_->wire_recv_buffer.resize(recv_length); + BindSocket(*this, *impl_, port); + SetIpPort(ip, target_port); + } catch (...) { + if (impl_->socket_fd >= 0) ::close(impl_->socket_fd); + delete[] localIP; + delete impl_; + throw; + } +} + +UDP::UDP(uint16_t port, int send_length, int recv_length, + bool initiative_disconnect, RecvEnum recv_type, bool set_ip_port) + : udpState{}, targetIP(nullptr), targetPort(0), localIP(nullptr), localPort(0), + impl_(new Impl) { + (void)initiative_disconnect; + (void)set_ip_port; + try { + impl_->send_length = send_length; + impl_->recv_length = recv_length; + impl_->recv_type = recv_type; + impl_->send_buffer.resize(send_length); + impl_->recv_buffer.resize(recv_length); + impl_->wire_recv_buffer.resize(recv_length); + BindSocket(*this, *impl_, port); + } catch (...) { + if (impl_->socket_fd >= 0) ::close(impl_->socket_fd); + delete[] localIP; + delete impl_; + throw; + } +} + +UDP::~UDP() { + if (impl_) { + if (impl_->socket_fd >= 0) ::close(impl_->socket_fd); + delete impl_; + } + delete[] targetIP; + delete[] localIP; +} + +void UDP::SetIpPort(const char* ip, uint16_t port) { + if (!ip) throw std::invalid_argument("targetIP must not be null"); + sockaddr_in target{}; + target.sin_family = AF_INET; + target.sin_port = htons(port); + if (inet_pton(AF_INET, ip, &target.sin_addr) != 1) { + throw std::invalid_argument(std::string("invalid IPv4 target: ") + ip); + } + if (::connect(impl_->socket_fd, reinterpret_cast(&target), sizeof(target)) < 0) { + throw SocketError("connect"); + } + delete[] targetIP; + targetIP = Duplicate(ip); + targetPort = port; + impl_->target_configured = true; +} + +void UDP::SetRecvTimeout(int time) { + timeval timeout{}; + timeout.tv_sec = time / 1000; + timeout.tv_usec = (time % 1000) * 1000; + if (setsockopt(impl_->socket_fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) < 0) { + throw SocketError("setsockopt(SO_RCVTIMEO)"); + } + impl_->recv_type = RecvEnum::blockTimeout; + ConfigureBlocking(*impl_); +} + +void UDP::SetDisconnectTime(float callback_dt, float disconnectTime) { + (void)callback_dt; + (void)disconnectTime; +} + +void UDP::SetAccessibleTime(float callback_dt, float accessibleTime) { + (void)callback_dt; + (void)accessibleTime; +} + +int UDP::Send() { + { + std::lock_guard state_lock(impl_->state_mutex); + ++udpState.TotalCount; + } + std::lock_guard lock(impl_->send_mutex); + if (!impl_->target_configured || impl_->send_buffer.empty()) { + std::lock_guard state_lock(impl_->state_mutex); + ++udpState.SendError; + return -1; + } + const ssize_t sent = ::send(impl_->socket_fd, impl_->send_buffer.data(), + impl_->send_buffer.size(), 0); + if (sent < 0) { + std::lock_guard state_lock(impl_->state_mutex); + ++udpState.SendError; + return -1; + } + { + std::lock_guard state_lock(impl_->state_mutex); + ++udpState.SendCount; + } + return static_cast(sent); +} + +int UDP::Recv() { + { + std::lock_guard state_lock(impl_->state_mutex); + ++udpState.TotalCount; + } + std::lock_guard socket_lock(impl_->socket_recv_mutex); + const ssize_t received = ::recv(impl_->socket_fd, impl_->wire_recv_buffer.data(), + impl_->wire_recv_buffer.size(), 0); + if (received < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) return -1; + return -1; + } + LowState decoded{}; + if (impl_->pro_low_level) { + if (!impl_->codec->DecodeLowState(impl_->wire_recv_buffer.data(), + static_cast(received), decoded)) { + std::lock_guard state_lock(impl_->state_mutex); + ++udpState.FlagError; + return -1; + } + } + { + std::lock_guard lock(impl_->recv_mutex); + ++udpState.TotalCount; + impl_->received_size = static_cast(received); + impl_->recv_buffer.assign(impl_->wire_recv_buffer.begin(), + impl_->wire_recv_buffer.begin() + received); + if (impl_->pro_low_level) { + impl_->low_state = decoded; + impl_->have_low_state = true; + } + accessible = true; + } + { + std::lock_guard state_lock(impl_->state_mutex); + ++udpState.RecvCount; + } + return static_cast(received); +} + +void UDP::InitCmdData(LowCmd& cmd) { + go1_pro_internal::InitLowCmd(cmd); +} + +void UDP::InitCmdData(HighCmd& cmd) { + go1_pro_internal::InitHighCmd(cmd); +} + +int UDP::SetSend(char* data) { + if (!data) return -1; + std::lock_guard lock(impl_->send_mutex); + std::memcpy(impl_->send_buffer.data(), data, impl_->send_buffer.size()); + return 0; +} + +int UDP::SetSend(LowCmd& cmd) { + if (!impl_->pro_low_level || !impl_->codec) return -1; + const auto encoded = impl_->codec->EncodeLowCmd(cmd); + std::lock_guard lock(impl_->send_mutex); + impl_->send_buffer.assign(encoded.begin(), encoded.end()); + return 0; +} + +int UDP::SetSend(HighCmd& cmd) { + (void)cmd; + return -1; +} + +void UDP::GetRecv(char* data) { + std::lock_guard lock(impl_->recv_mutex); + if (!data || impl_->received_size == 0) return; + std::memcpy(data, impl_->recv_buffer.data(), + std::min(impl_->received_size, static_cast(impl_->recv_length))); +} + +void UDP::GetRecv(LowState& state) { + std::lock_guard lock(impl_->recv_mutex); + if (impl_->have_low_state) state = impl_->low_state; +} + +void UDP::GetRecv(HighState& state) { (void)state; } + +} // namespace UNITREE_LEGGED_SDK diff --git a/tests/cpp/package_consumer/CMakeLists.txt b/tests/cpp/package_consumer/CMakeLists.txt new file mode 100644 index 0000000..6ec0ec8 --- /dev/null +++ b/tests/cpp/package_consumer/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.16) +project(unitree_legged_sdk_package_consumer LANGUAGES CXX) + +find_package(unitree_legged_sdk 3.8 CONFIG REQUIRED) + +add_executable(package_consumer main.cpp) +target_link_libraries(package_consumer PRIVATE unitree_legged_sdk::unitree_legged_sdk) diff --git a/tests/cpp/package_consumer/main.cpp b/tests/cpp/package_consumer/main.cpp new file mode 100644 index 0000000..41f5439 --- /dev/null +++ b/tests/cpp/package_consumer/main.cpp @@ -0,0 +1,9 @@ +#include "unitree_legged_sdk/unitree_legged_sdk.h" + +#include + +int main() { + UNITREE_LEGGED_SDK::LowCmd command{}; + command.motorCmd[UNITREE_LEGGED_SDK::FR_0].q = 0.0f; + return UNITREE_LEGGED_SDK::VersionSDK().empty() ? 1 : 0; +} diff --git a/tests/cpp/test_compat.cpp b/tests/cpp/test_compat.cpp new file mode 100644 index 0000000..e9ddec3 --- /dev/null +++ b/tests/cpp/test_compat.cpp @@ -0,0 +1,127 @@ +#include "pro_codec.h" +#include "unitree_legged_sdk/a1_const.h" +#include "unitree_legged_sdk/aliengo_const.h" +#include "unitree_legged_sdk/b1_const.h" +#include "unitree_legged_sdk/go1_const.h" +#include "unitree_legged_sdk/unitree_legged_sdk.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace UNITREE_LEGGED_SDK; + +namespace { + +#define CHECK(expression) do { if (!(expression)) return __LINE__; } while (false) + +uint64_t Fnv1a(const uint8_t* data, std::size_t size) { + uint64_t result = 1469598103934665603ull; + for (std::size_t i = 0; i < size; ++i) { + result ^= data[i]; + result *= 1099511628211ull; + } + return result; +} + +std::vector ReadFile(const std::string& path) { + std::ifstream stream(path, std::ios::binary); + return std::vector( + (std::istreambuf_iterator(stream)), std::istreambuf_iterator()); +} + +} // namespace + +int main() { + static_assert(std::is_same>::value, + "official LowCmd field shape changed"); + static_assert(sizeof(IMU) == 53, "IMU must match official packed declaration"); + static_assert(sizeof(MotorCmd) == 33, "MotorCmd must match official packed declaration"); + static_assert(sizeof(MotorState) == 38, "MotorState must match official packed declaration"); + static_assert(sizeof(BmsState) == 34, "BmsState must match official packed declaration"); + static_assert(sizeof(LowCmd) == 734, "LowCmd must match official packed declaration"); + static_assert(sizeof(LowState) == 937, "LowState must match official packed declaration"); + static_assert(sizeof(HighCmd) == 129, "HighCmd must match official packed declaration"); + static_assert(sizeof(HighState) == 1087, "HighState must match official packed declaration"); + static_assert(go1_Hip_max > 1.0 && a1_Hip_max < 1.0 && + aliengo_Hip_max > 1.0 && b1_Hip_max < 1.0, + "official robot constant headers must be available"); + + go1_pro_internal::ProCodec codec(go1_pro_internal::ProCodec::FindStateFile()); + LowCmd initialized{}; + go1_pro_internal::InitLowCmd(initialized); + CHECK(initialized.head[0] == 0xfe && initialized.head[1] == 0xef); + CHECK(initialized.levelFlag == LOWLEVEL && initialized.bandWidth == 0x3ac0); + for (const auto& motor : initialized.motorCmd) { + CHECK(motor.mode == 0x0a); + CHECK(motor.q == static_cast(PosStopF)); + CHECK(motor.dq == static_cast(VelStopF)); + CHECK(motor.Kp == 0.0f && motor.Kd == 0.0f && motor.tau == 0.0f); + } + std::array zero{}; + std::array encrypted{}; + codec.Encrypt(zero.data(), encrypted.data(), encrypted.size()); + const std::array known = {{0x12, 0xaa, 0x03, 0x54, 0x23, 0x6b, 0x66, 0xe3}}; + CHECK(encrypted == known); + + LowCmd cmd{}; + cmd.head = {{0xfe, 0xef}}; + cmd.levelFlag = LOWLEVEL; + cmd.bandWidth = 0x3ac0; + const auto packet = codec.EncodeLowCmd(cmd); + CHECK(Fnv1a(packet.data(), packet.size()) == 0x63c5ef1a61977e18ull); + +#if GO1_PRO_HAVE_CAPTURE_FIXTURES + const auto real_lowcmd = ReadFile(GO1_PRO_TEST_LOWCMD); + const auto real_lowcmd_plain = ReadFile(GO1_PRO_TEST_LOWCMD_PLAIN); + CHECK(real_lowcmd.size() == go1_pro_internal::kLowCmdWireSize); + CHECK(real_lowcmd_plain.size() == go1_pro_internal::kLowCmdWireSize); + std::vector decrypted_lowcmd(real_lowcmd.size()); + codec.Decrypt(real_lowcmd.data(), decrypted_lowcmd.data(), real_lowcmd.size()); + CHECK(decrypted_lowcmd == real_lowcmd_plain); + std::vector encrypted_lowcmd(real_lowcmd_plain.size()); + codec.Encrypt(real_lowcmd_plain.data(), encrypted_lowcmd.data(), real_lowcmd_plain.size()); + CHECK(encrypted_lowcmd == real_lowcmd); + + const auto state_packet = ReadFile(GO1_PRO_TEST_CAPTURE); + CHECK(state_packet.size() == go1_pro_internal::kLowStateDatagramSize); + LowState state{}; + CHECK(codec.DecodeLowState(state_packet.data(), state_packet.size(), state)); + CHECK(state.head[0] == 0xfe && state.head[1] == 0xef); + CHECK(state.levelFlag == LOWLEVEL); + CHECK(state.motorState[FR_1].temperature == 50); + CHECK(std::isfinite(state.motorState[FR_1].q)); + CHECK(state.wirelessRemote[0] == 0x55 && state.wirelessRemote[1] == 0x51); + + const auto old_state_packet = ReadFile(GO1_PRO_TEST_CAPTURE_OLD); + CHECK(old_state_packet.size() == go1_pro_internal::kLowStateDatagramSize); + LowState old_state{}; + CHECK(codec.DecodeLowState(old_state_packet.data(), old_state_packet.size(), old_state)); + CHECK(old_state.bms.SOC == 76); + CHECK(old_state.motorState[FR_0].temperature == 52); +#endif + + Safety safety(LeggedType::Go1); + cmd.motorCmd[FR_0].q = 4.0f; + safety.PositionLimit(cmd); + CHECK(cmd.motorCmd[FR_0].q == 0.78f); + cmd.motorCmd[FR_1].q = static_cast(PosStopF); + safety.PositionLimit(cmd); + CHECK(cmd.motorCmd[FR_1].q == static_cast(PosStopF)); + + std::atomic callbacks{0}; + LoopFunc loop("compat-test", 0.001f, Callback([&callbacks] { ++callbacks; })); + loop.start(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + loop.shutdown(); + CHECK(callbacks.load() > 0); + return 0; +} diff --git a/tests/test_capture_fixtures.py b/tests/test_capture_fixtures.py new file mode 100644 index 0000000..bde6b48 --- /dev/null +++ b/tests/test_capture_fixtures.py @@ -0,0 +1,108 @@ +import socket +import struct +import tarfile +from pathlib import Path + +import pytest + +from go1_pro_sdk import Blowfish, parse_low_state + + +ROOT = Path(__file__).resolve().parents[1] +CAPTURES = ROOT / "data/captures" +STATE_FILE = ROOT / "go1_pro_sdk/_data/blowfish_state.bin" + +if not CAPTURES.is_dir(): + pytest.skip("private data/captures fixtures are not available", allow_module_level=True) + + +@pytest.fixture(scope="module") +def blowfish(): + return Blowfish.from_state_file(str(STATE_FILE)) + + +def _pcap_udp_packets(path): + packets = [] + with path.open("rb") as stream: + header = stream.read(24) + assert len(header) == 24 + magic = header[:4] + assert magic in (b"\xd4\xc3\xb2\xa1", b"\xa1\xb2\xc3\xd4") + endian = "<" if magic == b"\xd4\xc3\xb2\xa1" else ">" + linktype = struct.unpack(endian + "I", header[20:24])[0] + assert linktype == 113 # Linux cooked capture v1 + + while True: + packet_header = stream.read(16) + if not packet_header: + break + assert len(packet_header) == 16 + _, _, captured_length, _ = struct.unpack(endian + "IIII", packet_header) + frame = stream.read(captured_length) + assert len(frame) == captured_length + assert struct.unpack(">H", frame[14:16])[0] == 0x0800 + ip_offset = 16 + ip_header_length = (frame[ip_offset] & 0x0F) * 4 + assert frame[ip_offset + 9] == 17 + udp_offset = ip_offset + ip_header_length + source_port, target_port, udp_length, _ = struct.unpack( + ">HHHH", frame[udp_offset:udp_offset + 8] + ) + payload = frame[udp_offset + 8:udp_offset + udp_length] + packets.append(( + socket.inet_ntoa(frame[ip_offset + 12:ip_offset + 16]), + socket.inet_ntoa(frame[ip_offset + 16:ip_offset + 20]), + source_port, + target_port, + payload, + )) + return packets + + +def test_real_lowcmd_full_cipher_plain_pair(blowfish): + cipher = (CAPTURES / "real_lowcmd.bin").read_bytes() + plain = (CAPTURES / "real_lowcmd_decrypted.bin").read_bytes() + assert len(cipher) == len(plain) == 616 + assert blowfish.decrypt_ecb(cipher) == plain + assert blowfish.encrypt_ecb(plain) == cipher + assert plain[:22] == bytes.fromhex( + "feefff00000000000000000000000000000000003ac0" + ) + assert plain[22] == 0x0A # This fixture contains active servo commands. + + +def test_real_lowcmd_pcap_matches_extracted_first_frame(): + packets = _pcap_udp_packets(CAPTURES / "real_lowcmd.pcap") + assert len(packets) == 1406 + assert {(src, dst, sport, dport) for src, dst, sport, dport, _ in packets} == { + ("192.168.123.161", "192.168.123.10", 8008, 8007) + } + assert {len(payload) for *_, payload in packets} == {616} + assert packets[0][-1] == (CAPTURES / "real_lowcmd.bin").read_bytes() + + +@pytest.mark.parametrize( + "filename,soc,temperatures", + [ + ("mcu_response.bin", 76, [52, 42, 40]), + ("mcu_response_new.bin", 19, [79, 50, 51]), + ], +) +def test_real_lowstate_samples(blowfish, filename, soc, temperatures): + cipher = (CAPTURES / filename).read_bytes() + assert len(cipher) == 858 + plain = blowfish.decrypt_ecb(cipher[:856]) + assert plain[:4] == b"\xfe\xef\xff\x00" + state = parse_low_state(plain) + assert state.bms.SOC == soc + assert [motor.temperature for motor in state.motorState[:3]] == temperatures + assert state.wirelessRemote[:2] == b"\x55\x51" + + +def test_memory_dump_reproduces_packaged_blowfish_state(): + with tarfile.open(CAPTURES / "blowfish_dump.tar.gz", "r:gz") as archive: + member = archive.extractfile("./rw_7f86a1f000.bin") + assert member is not None + memory = member.read() + extracted = memory[0x232190:0x232190 + 4168] + assert extracted == STATE_FILE.read_bytes() diff --git a/tests/test_lowcmd_builder.py b/tests/test_lowcmd_builder.py index 43657b1..49b1006 100644 --- a/tests/test_lowcmd_builder.py +++ b/tests/test_lowcmd_builder.py @@ -61,8 +61,8 @@ def test_plain_crc_at_612(): assert plain[612:616] == expected_crc -def test_encrypted_damping_matches_real(bf): - """全 damping 加密后, 前 16B 应跟真实 Legged_sport 抓包一致.""" +def test_encrypted_default_header_matches_real_capture(bf): + """默认命令与活动实机命令的公共头部加密结果必须一致.""" cmd = LowCmd() enc = build_low_cmd_encrypted(cmd, bf) assert len(enc) == 616 diff --git a/tests/test_mcu_client.py b/tests/test_mcu_client.py new file mode 100644 index 0000000..f1d17a3 --- /dev/null +++ b/tests/test_mcu_client.py @@ -0,0 +1,97 @@ +from pathlib import Path +from hashlib import sha256 + +import pytest + +from go1_pro_sdk import LowCmd, MCUClient +from go1_pro_sdk.connection import mcu_client as mcu_module + + +ROOT = Path(__file__).resolve().parents[1] +CAPTURE_FILE = ROOT / "data/captures/mcu_response_new.bin" +if not CAPTURE_FILE.is_file(): + pytest.skip("private LowState capture is not available", allow_module_level=True) +CAPTURE = CAPTURE_FILE.read_bytes() +DAMPING_SHA256 = "ca911b04a09d8e8b069c96ac31e410b683fcf1bff625dd203f9efde215888400" + + +class FakeSocket: + def __init__(self): + self.sent = [] + self.recv_batches = [] + self.closed = False + self.bound = None + self.blocking = None + + def setblocking(self, value): + self.blocking = value + + def setsockopt(self, *args): + pass + + def bind(self, address): + self.bound = address + + def getsockname(self): + return ("0.0.0.0", 45678) + + def sendto(self, data, address): + self.sent.append((bytes(data), address)) + return len(data) + + def recvfrom(self, size): + if not self.recv_batches: + raise BlockingIOError + batch = self.recv_batches[0] + if not batch: + self.recv_batches.pop(0) + raise BlockingIOError + return batch.pop(0)[:size], ("192.168.123.10", 8007) + + def close(self): + self.closed = True + + +@pytest.fixture +def fake_socket(monkeypatch): + sock = FakeSocket() + monkeypatch.setattr(mcu_module.socket, "socket", lambda *args, **kwargs: sock) + monkeypatch.setattr(mcu_module.time, "sleep", lambda duration: None) + return sock + + +def test_native_client_send_receive_and_close(fake_socket): + with MCUClient() as client: + assert client.local_port == 45678 + assert client.send(LowCmd().all_damping()) == 616 + packet, address = fake_socket.sent[-1] + assert address == ("192.168.123.10", 8007) + assert sha256(packet).hexdigest() == DAMPING_SHA256 + + fake_socket.recv_batches = [[CAPTURE]] + state = client.recv_latest() + assert state is client.last_state + assert [motor.temperature for motor in state.motorState[:3]] == [79, 50, 51] + assert state.wirelessRemote[:2] == b"\x55\x51" + + assert fake_socket.closed + assert client.sock is None + + +def test_native_client_ignores_invalid_state(fake_socket): + client = MCUClient() + fake_socket.recv_batches = [[b"\x00" * 858]] + assert client.recv_latest() is None + assert client.last_state is None + + +def test_native_wake_and_safe_stop(fake_socket): + client = MCUClient() + fake_socket.recv_batches = [[CAPTURE], [CAPTURE], [CAPTURE]] + assert client.wake_mcu(n_frames=3, dt=0) == 3 + assert len(fake_socket.sent) == 3 + assert all(sha256(packet).hexdigest() == DAMPING_SHA256 for packet, _ in fake_socket.sent) + + client.safe_stop(n_frames=2, dt=0) + assert len(fake_socket.sent) == 5 + assert all(sha256(packet).hexdigest() == DAMPING_SHA256 for packet, _ in fake_socket.sent) diff --git a/tests/test_official_compat.py b/tests/test_official_compat.py index 75d15ab..5e897fb 100644 --- a/tests/test_official_compat.py +++ b/tests/test_official_compat.py @@ -1,11 +1,23 @@ """Contract tests for the official ``robot_interface`` compatibility API.""" +import ast from copy import deepcopy +import inspect +from pathlib import Path import pytest import robot_interface as sdk +from go1_pro_sdk import LowState as NativeLowState from go1_pro_sdk import build_low_cmd_plain +from examples.example_official_compatible_position import initialize_transport + + +HIGHLEVEL = 0xee +LOWLEVEL = 0xff +POS_STOP_F = 2.146e9 +FR_0 = 0 +FR_1 = 1 class _FakeMCUClient: @@ -15,8 +27,8 @@ class _FakeMCUClient: 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.next_state = NativeLowState() + self.next_state.motorState[FR_1].q = 1.23 self.sent = [] self.closed = False self.__class__.instances.append(self) @@ -46,60 +58,117 @@ def fake_client(monkeypatch): def test_official_names_and_data_types_are_available(): - assert sdk.LOWLEVEL == 0xff - assert sdk.LeggedType.Go1 == 2 + assert sdk.LeggedType.Go1 != 2 + assert int(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] + assert not hasattr(sdk, "LOWLEVEL") + assert not hasattr(sdk, "FR_0") + assert not hasattr(sdk, "__all__") + assert {name for name in dir(sdk) if not name.startswith("_")} == { + "LeggedType", "Aliengo", "A1", "Go1", "B1", + "RecvEnum", "nonBlock", "block", "blockTimeout", + "UDP", "Safety", "BmsCmd", "BmsState", "Cartesian", "IMU", + "LED", "MotorState", "MotorCmd", "LowState", "LowCmd", + "HighState", "HighCmd", "UDPState", + } + with pytest.raises(TypeError): + sdk.Safety(2) + + +@pytest.mark.parametrize( + ("factory", "kwargs"), + [ + (sdk.MotorCmd, {"q": 1.0}), + (sdk.LowCmd, {"levelFlag": LOWLEVEL}), + (sdk.Cartesian, {"x": 1.0}), + (sdk.HighCmd, {"mode": 2}), + ], +) +def test_official_struct_constructors_are_no_arg_only(factory, kwargs): + with pytest.raises(TypeError): + factory(**kwargs) def test_init_cmd_data_uses_official_stop_sentinels(): - udp = sdk.UDP(sdk.LOWLEVEL, 0, "192.168.123.10", 8007) + udp = sdk.UDP(LOWLEVEL, 0, "192.168.123.10", 8007) cmd = sdk.LowCmd() assert udp.InitCmdData(cmd) is None - assert cmd.levelFlag == sdk.LOWLEVEL + assert cmd.levelFlag == LOWLEVEL assert len(cmd.motorCmd) == 20 + assert cmd.head == [0xfe, 0xef] + assert cmd.SN == [0, 0] + assert cmd.version == [0, 0] + assert isinstance(cmd.motorCmd[0], sdk.MotorCmd) + assert isinstance(cmd.motorCmd[0].mode, int) + assert isinstance(cmd.bms, sdk.BmsCmd) + assert isinstance(cmd.wirelessRemote, list) + assert isinstance(cmd.reserve, int) + assert isinstance(cmd.crc, int) + assert not hasattr(cmd, "set_motor") + assert not hasattr(cmd, "all_damping") + assert not hasattr(cmd.motorCmd[0], "to_bytes") 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) + assert all(m.q == POS_STOP_F for m in cmd.motorCmd) + assert all(m.dq == 16000.0 for m in cmd.motorCmd) + plain = build_low_cmd_plain(cmd) + assert len(plain) == 616 + assert plain[:4] == b"\xfe\xef\xff\x00" def test_official_lowlevel_send_receive_sequence(fake_client): - udp = sdk.UDP(sdk.LOWLEVEL, 0, "192.168.123.10", 8007) + udp = sdk.UDP(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) + assert state.motorState[FR_1].q == pytest.approx(1.23) + assert isinstance(state.head, list) + assert isinstance(state.SN, list) + assert isinstance(state.imu.rpy, list) + assert isinstance(state.imu, sdk.IMU) + assert isinstance(state.motorState[0], sdk.MotorState) + assert isinstance(state.bms, sdk.BmsState) + assert not hasattr(state, "remote") + assert not hasattr(state.motorState[0], "estimated_current") + assert not hasattr(state.bms, "voltage_v") + assert isinstance(state.reserve, int) + assert isinstance(state.crc, int) - 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 + cmd.motorCmd[FR_1].q = 1.2 + cmd.motorCmd[FR_1].dq = 0.0 + cmd.motorCmd[FR_1].Kp = 5.0 + cmd.motorCmd[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 + assert client.sent[0].motorCmd[FR_1].q == pytest.approx(1.2) + assert not hasattr(udp, "udpState") + assert not hasattr(udp, "accessible") + assert not hasattr(udp, "close") 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 + sdk.UDP(LOWLEVEL, 0, "192.168.123.10", 8007).InitCmdData(cmd) + cmd.motorCmd[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) + assert cmd.motorCmd[FR_0].q == POS_STOP_F + assert cmd.motorCmd[FR_1].q == pytest.approx(3.5) + + +def test_position_protect_requires_official_explicit_limit(): + safe = sdk.Safety(sdk.LeggedType.Go1) + with pytest.raises(TypeError): + safe.PositionProtect(sdk.LowCmd(), sdk.LowState()) def test_bms_command_is_serialized(): @@ -131,10 +200,133 @@ def test_fixed_size_official_arrays_are_validated(): def test_highlevel_udp_reports_the_unsupported_boundary(): - udp = sdk.UDP(sdk.HIGHLEVEL, 8080, "192.168.123.161", 8082) + udp = sdk.UDP(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() + + +def test_portable_initialization_sends_only_stop_sentinels_before_control(): + class RecordingUDP: + def __init__(self): + self.cmd = None + self.sent_q = [] + self.recv_count = 0 + + def SetSend(self, cmd): + self.cmd = cmd + return 0 + + def Send(self): + self.sent_q.append([motor.q for motor in self.cmd.motorCmd]) + return 616 + + def Recv(self): + self.recv_count += 1 + return 858 if self.recv_count >= 2 else 0 + + def GetRecv(self, state): + return None + + udp = RecordingUDP() + cmd = sdk.LowCmd() + state = sdk.LowState() + sdk.UDP(LOWLEVEL, 0, "192.168.123.10", 8007).InitCmdData(cmd) + + initialize_transport(udp, cmd, state, frames=2, dt=0) + + assert len(udp.sent_q) == 2 + assert all(all(q == POS_STOP_F for q in frame) for frame in udp.sent_q) + + +def test_portable_example_only_uses_official_module_exports(): + source_path = ( + Path(__file__).parents[1] + / "examples" + / "example_official_compatible_position.py" + ) + tree = ast.parse(source_path.read_text()) + sdk_attributes = { + node.attr + for node in ast.walk(tree) + if isinstance(node, ast.Attribute) + and isinstance(node.value, ast.Name) + and node.value.id == "sdk" + } + assert sdk_attributes <= { + "UDP", "Safety", "LeggedType", "LowCmd", "LowState", + } + called_attributes = { + node.func.attr + for node in ast.walk(tree) + if isinstance(node, ast.Call) + and isinstance(node.func, ast.Attribute) + } + assert called_attributes.isdisjoint({ + "set_motor", "all_damping", "wake_mcu", "safe_stop", "close", + }) + + +def test_official_struct_field_snapshot(): + expected = { + "BmsCmd": {"off", "reserve"}, + "BmsState": { + "version_h", "version_l", "bms_status", "SOC", "current", + "cycle", "BQ_NTC", "MCU_NTC", "cell_vol", + }, + "Cartesian": {"x", "y", "z"}, + "IMU": {"quaternion", "gyroscope", "accelerometer", "rpy", "temperature"}, + "LED": {"r", "g", "b"}, + "MotorState": { + "mode", "q", "dq", "ddq", "tauEst", "q_raw", "dq_raw", + "ddq_raw", "temperature", "reserve", + }, + "MotorCmd": {"mode", "q", "dq", "tau", "Kp", "Kd", "reserve"}, + "LowState": { + "head", "levelFlag", "frameReserve", "SN", "version", + "bandWidth", "imu", "motorState", "bms", "footForce", + "footForceEst", "tick", "wirelessRemote", "reserve", "crc", + }, + "LowCmd": { + "head", "levelFlag", "frameReserve", "SN", "version", + "bandWidth", "motorCmd", "bms", "wirelessRemote", "reserve", "crc", + }, + "HighState": { + "head", "levelFlag", "frameReserve", "SN", "version", + "bandWidth", "imu", "motorState", "bms", "footForce", + "footForceEst", "mode", "progress", "gaitType", + "footRaiseHeight", "position", "bodyHeight", "velocity", + "yawSpeed", "rangeObstacle", "footPosition2Body", + "footSpeed2Body", "wirelessRemote", "reserve", "crc", + }, + "HighCmd": { + "head", "levelFlag", "frameReserve", "SN", "version", + "bandWidth", "mode", "gaitType", "speedLevel", + "footRaiseHeight", "bodyHeight", "position", "euler", + "velocity", "yawSpeed", "bms", "led", "wirelessRemote", + "reserve", "crc", + }, + "UDPState": { + "TotalCount", "SendCount", "RecvCount", "SendError", + "FlagError", "RecvCRCError", "RecvLoseError", + }, + } + for class_name, field_names in expected.items(): + factory = getattr(sdk, class_name) + assert str(inspect.signature(factory)) == "()" + instance = factory() + assert not (field_names - set(dir(instance))) + + +def test_official_method_snapshot(): + assert { + "SetIpPort", "SetRecvTimeout", "SetDisconnectTime", + "SetAccessibleTime", "Send", "Recv", "InitCmdData", "SetSend", + "GetRecv", + } <= set(dir(sdk.UDP)) + assert str(inspect.signature(sdk.Safety.PositionLimit)) == "(self, lowcmd)" + assert str(inspect.signature(sdk.Safety.PowerProtect)) == "(self, lowcmd, lowstate, factor)" + assert str(inspect.signature(sdk.Safety.PositionProtect)) == "(self, lowcmd, lowstate, limit)"