diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 042b309..2de8372 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,11 +1,12 @@ -# Test gate for a repository that currently ships documentation only. +# Test gate for the LiteGrip C++ SDK. # # Tests live in their own workflow: they run on pull requests, while the release # workflow runs on pushes to the default branch. Mixing the two means a broken # release blocks a pull request, or the reverse. # -# Replace the test step with the project's real toolchain and test command as soon -# as source code lands; see the repository standards in AGENTS.md. +# The suite runs without hardware: test_transport is deliberately +# non-transmitting and every other test covers behaviour that must hold while +# disconnected. No CAN interface and no root are required. # # AGENTS.md is deliberately not linted: it is canonical content synced from # repo-template and must not be edited here. @@ -31,6 +32,15 @@ jobs: - name: Checkout Code uses: actions/checkout@v4 + - name: Configure + run: cmake -S . -B build -DCMAKE_BUILD_TYPE=Release + + - name: Build + run: cmake --build build -j + + - name: Run Tests + run: ctest --test-dir build --output-on-failure + - name: Setup Node uses: actions/setup-node@v4 with: diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..b2f5e85 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,127 @@ +cmake_minimum_required(VERSION 3.16) +project(litegrip_cpp VERSION 0.1.0 LANGUAGES CXX) + +# litegrip_cpp — ROS-agnostic C++ SDK for the LiteGrip adaptive two-finger gripper. +# +# Design constraints (see ../PLAN-litegrip-cpp.md): +# * C++17, zero third-party dependencies (stdlib + Linux SocketCAN only). +# * MUST NOT depend on ROS / ament / ros2_control — this library is the bottom +# layer that ros2_control links, and is usable from any plain C++ program. +# * Installable both into a ROS workspace (colcon, plain-cmake package.xml) +# and into a normal prefix (find_package + pkg-config). + +if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR) + set(LITEGRIP_CPP_IS_TOP_LEVEL ON) +else() + set(LITEGRIP_CPP_IS_TOP_LEVEL OFF) +endif() + +option(LITEGRIP_CPP_BUILD_TESTS "Build litegrip_cpp unit tests" ${LITEGRIP_CPP_IS_TOP_LEVEL}) +option(LITEGRIP_CPP_BUILD_EXAMPLES "Build litegrip_cpp examples" ${LITEGRIP_CPP_IS_TOP_LEVEL}) + +# Must come before any $ use: +# the variable is only defined once GNUInstallDirs is included, and an empty +# value silently drops the include directory from the exported target. +include(GNUInstallDirs) + +find_package(Threads REQUIRED) + +# ── library ─────────────────────────────────────────────────────────────── +# T1 froze the API surface (headers). T2 adds the can/* layer; bus/LiteGrip +# land in T3 and safety/ControlLoop in T4. +add_library(litegrip_cpp + src/version.cpp + src/exceptions.cpp + src/constants.cpp + src/can/protocol.cpp + src/can/motor.cpp + src/can/transport.cpp + src/can/controller.cpp + src/json.cpp + src/calibration.cpp + src/hold_policy.cpp + src/bus.cpp + src/safety.cpp + src/gripper.cpp + src/control_loop.cpp +) +add_library(litegrip_cpp::litegrip_cpp ALIAS litegrip_cpp) + +target_compile_features(litegrip_cpp PUBLIC cxx_std_17) +target_link_libraries(litegrip_cpp PUBLIC Threads::Threads) + +target_include_directories(litegrip_cpp + PUBLIC + $ + $ +) + +set_target_properties(litegrip_cpp PROPERTIES + VERSION ${PROJECT_VERSION} + SOVERSION ${PROJECT_VERSION_MAJOR} + POSITION_INDEPENDENT_CODE ON +) + +if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") + target_compile_options(litegrip_cpp PRIVATE -Wall -Wextra -Wpedantic) +endif() + +# ── install ─────────────────────────────────────────────────────────────── +include(CMakePackageConfigHelpers) + +install(TARGETS litegrip_cpp + EXPORT litegrip_cppTargets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} +) + +install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) + +# Calibration / safety data (factory_calibration.json, safety_limits_*.json). +# These are *data*: a deployment may override them, but the factory defaults +# must ship so load_calibration()'s read-only fallback works out of the box. +# The path is mirrored by litegrip_cpp_DATA_DIR in the CMake package config. +install(DIRECTORY calibration/ + DESTINATION ${CMAKE_INSTALL_DATADIR}/litegrip_cpp/calibration +) + +install(EXPORT litegrip_cppTargets + FILE litegrip_cppTargets.cmake + NAMESPACE litegrip_cpp:: + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/litegrip_cpp +) + +configure_package_config_file( + cmake/litegrip_cpp-config.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/litegrip_cpp-config.cmake + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/litegrip_cpp +) +write_basic_package_version_file( + ${CMAKE_CURRENT_BINARY_DIR}/litegrip_cpp-config-version.cmake + VERSION ${PROJECT_VERSION} + COMPATIBILITY SameMajorVersion +) + +install(FILES + ${CMAKE_CURRENT_BINARY_DIR}/litegrip_cpp-config.cmake + ${CMAKE_CURRENT_BINARY_DIR}/litegrip_cpp-config-version.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/litegrip_cpp +) + +# ── pkg-config ──────────────────────────────────────────────────────────── +configure_file(cmake/litegrip_cpp.pc.in + ${CMAKE_CURRENT_BINARY_DIR}/litegrip_cpp.pc @ONLY) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/litegrip_cpp.pc + DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig +) + +# ── tests / examples (no third-party test framework: plain assert-based) ── +if(LITEGRIP_CPP_BUILD_TESTS) + enable_testing() + add_subdirectory(test) +endif() + +if(LITEGRIP_CPP_BUILD_EXAMPLES) + add_subdirectory(examples) +endif() diff --git a/README.md b/README.md index 1c7d8d4..d9e6e42 100644 --- a/README.md +++ b/README.md @@ -1,34 +1,143 @@ -# litegrip-cpp +# litegrip_cpp -C++ SDK for the **LiteGrip lightweight robotic gripper series**. +ROS-agnostic **C++ SDK** for the LiteGrip adaptive two-finger gripper. -> **Status:** repository initialized. Source code, packaging and documentation -> have not landed yet. +**English** · [简体中文](README_zh.md) -## Scope +This is layer 1 of the litegrip stack — `litegrip_cpp` (SDK) → `litegrip_ros2_control` +(hardware interface) → `litegrip_moveit_config` (MoveIt). It speaks SocketCAN and +the Damiao DM4310 MIT protocol directly and depends on **nothing** but the C++17 +standard library, `pthread`, and the Linux SocketCAN headers: no ROS, no ament, +no third-party libraries. Non-ROS implementations can link it as-is. -| | | -| --- | --- | -| Product | LiteGrip lightweight robotic gripper series | -| Repository role | C++ SDK | -| Status | Initializing — no source code yet | +> Status: **layer 1 is complete** — `can/*`, `GripperBus`, `LiteGrip`, +> `json`/calibration, `SafetyGuard` and `ControlLoop` are all implemented and +> tested. -## Related repositories +## What is **not** in this version -| Repository | Role | -| --- | --- | -| [litegrip-python](https://github.com/nexform-tech/litegrip-python) | Python SDK | -| [litegrip-docs](https://github.com/nexform-tech/litegrip-docs) | Product documentation | -| [litegrip-ros2](https://github.com/nexform-tech/litegrip-ros2) | ROS 2 driver | -| [litegrip-ros1](https://github.com/nexform-tech/litegrip-ros1) | ROS 1 driver | +Per the agreed v1 scope: `grasp()`, `set_force()`, `move_at_speed*()` and the +public zero-gravity mode. `close(force_n=...)` accepts the argument, ignores it +and says so, because applying a grip force needs torque feed-forward and +verified force calibration. -## Repository standards +## Safety wiring -This repository follows the shared NEXFORM ROBOTICS repository standards: the -agent operating rules in [AGENTS.md](AGENTS.md), Conventional Commits, and -automated semantic-release versioning on every merge to `main`. +The motion path (`goto_rad` / `move_to` / `open` / `close` / `home`) passes +through `SafetyGuard::guard_motion_frame`, and rejections are **raised, not +clamped**. Two paths deliberately bypass it, each documented at its definition: +`stop()` (an emergency stop must work from outside the red lines — it asserts +the zero-torque invariant instead) and the calibration routines (they drive to +the mechanical stops, which lie outside the red lines). -## License +## Testing -Copyright © 2026 NEXFORM ROBOTICS. Licensed under the -[Apache License 2.0](LICENSE). +```bash +ctest --test-dir build --output-on-failure +``` + +Two kinds of test: + +- **Hand-lifted golden vectors** (`test_protocol.cpp`, `test_motor.cpp`) — the + cases from the Python suite, ported one for one, so a divergence in the port + shows up here rather than on hardware. +- **Generated parity harness** (`test_golden.cpp` + + `test/golden_generated.hpp`) — `test/generate_golden.py` drives the **real + Python SDK** and emits the exact bytes it produces (quantization, MIT packing, + status decoding, parameter frames); the C++ side must reproduce them. 1284 + checks. Regenerate after changing the Python original: + `python3 test/generate_golden.py`. + +`test_transport.cpp` is deliberately **non-transmitting** (a live gripper may be +on `can0`): it only reads an interface MTU, checks the missing-interface error +path, and opens/closes a socket. The send/receive path is not yet covered by a +test — it needs a vcan interface (root) or the real device. + +`test_bus.cpp`, `test_gripper.cpp`, `test_safety.cpp` and +`test_control_loop.cpp` cover the behaviour that must hold **without** hardware: +lifecycle refusals while disconnected, the missing-interface error path, config +plumbing, the injectable hold policy, the calibration file round-trip, every +safety criterion — including the adversarial "must reject" cases, since each of +those is a case where accepting it would move hardware — and the control loop +itself via `dry_run`. + +`dry_run` is not "do nothing": it runs the whole control path (rate limiting, +torque-budget allocation, the gate, the watchdogs) against a simulated plant and +only skips opening CAN and sending. That is what makes the loop, and every +deploy-config fail-closed rule, testable without a gripper. + +What is **not** covered here, and needs the real device: connect, `init`/enable, +motion, calibration, and the transport send/receive path. + +## Layers + +| Layer | Type | Ported from | +|---|---|---| +| `can::CanTransport` | SocketCAN raw transport (CAN / CAN-FD, RX id filter) | `can/transport.py` | +| `can` protocol | pure codec: MIT frames, status frames, param frames | `can/protocol.py` | +| `can::MotorState` | per-motor decoded state | `can/motor.py` | +| `can::MotorController` | multi-motor dispatch on one bus | `can/controller.py` | +| `GripperBus` | single-gripper bus API (init = hold) | `protocols/can_bus.py` | +| `LiteGrip` | high-level API | `gripper.py` | +| `SafetyGuard` + `SafetyLimits` | red lines, torque budget, watchdog, modes | `safety_limits.py` (core) | +| `ControlLoop` | background 200 Hz streaming + rate limit + gate | old ROS-side daemon | + +## Build + +Zero-dependency, plain CMake: + +```bash +cmake -S . -B build -DCMAKE_BUILD_TYPE=Release +cmake --build build -j +ctest --test-dir build --output-on-failure +``` + +It is also buildable by `colcon` (declared as a plain-cmake package via +`package.xml`) so a ROS workspace can build it beside the ROS packages. + +## Consume + +```cmake +find_package(litegrip_cpp REQUIRED) +target_link_libraries(my_node PRIVATE litegrip_cpp::litegrip_cpp) +``` + +```bash +pkg-config --cflags --libs litegrip_cpp +``` + +## Non-ROS usage + +```cpp +#include + +int main() { + litegrip::GripperConfig cfg; // can0, can_id 0x08, DM4310 + auto gripper = litegrip::LiteGrip::connect_raii(cfg); + gripper.init(); // enable and hold current position + gripper.open(); + gripper.goto_mm(40.0); + const auto state = gripper.get_state(); + return state.is_stale() ? 1 : 0; +} +``` + +## Safety invariants + +These are the safety argument and must not be relaxed: + +1. **Tighten only** — limits may only be a sub-interval of the shipped baseline. +2. **Reject, do not clamp** — an out-of-range command is refused with a reason, + never silently rewritten and sent. +3. **Fail-closed** — limits unavailable ⇒ reject everything; a value that cannot + be bounded ⇒ do not move that way. +4. **Strict numeric boundary** — NaN / ±inf / non-numbers are rejected before any + comparison. + +## Red lines are not yet unit-specific + +The packaged safety baseline carries the reference unit's hand-push measurement. +They must be re-derived per gripper (caliper + closed-end re-zero) before +real-hardware motion, and `ControlLoopConfig::max_feedback_velocity_rad_s` must +be calibrated first — until it is, the loop refuses to send any motion frame +(deliberate fail-closed). diff --git a/README_zh.md b/README_zh.md new file mode 100644 index 0000000..67c110a --- /dev/null +++ b/README_zh.md @@ -0,0 +1,131 @@ +# litegrip_cpp + +LiteGrip 自适应两指夹爪的 **ROS 无关 C++ SDK**。 + +本包是 litegrip 栈的第 1 层 —— `litegrip_cpp`（SDK）→ `litegrip_ros2_control` +（硬件接口）→ `litegrip_moveit_config`（MoveIt）。它直接讲 SocketCAN 与达妙 +DM4310 的 MIT 协议，依赖**只有** C++17 标准库、`pthread` 和 Linux SocketCAN +头文件：没有 ROS、没有 ament、没有第三方库。非 ROS 实现可以直接链接使用。 + +> 状态：**第 1 层已全部实现**（`can/*`、`GripperBus`、`LiteGrip`、 +> `json`/标定、`SafetyGuard`、`ControlLoop`）。 + +## 分层 + +| 层 | 类型 | 移植自 | +|---|---|---| +| `can::CanTransport` | SocketCAN 原始传输（CAN / CAN-FD、RX id 过滤） | `can/transport.py` | +| `can` 协议层 | 纯编解码：MIT 帧、状态帧、参数帧 | `can/protocol.py` | +| `can::MotorState` | 单电机解码状态 | `can/motor.py` | +| `can::MotorController` | 一条总线上的多电机派发 | `can/controller.py` | +| `GripperBus` | 单爪总线 API（`init` = 持位） | `protocols/can_bus.py` | +| `LiteGrip` | 高层 API | `gripper.py` | +| `SafetyGuard` + `SafetyLimits` | 红线、力矩预算、看门狗、模式 | `safety_limits.py`（核心） | +| `ControlLoop` | 后台 200 Hz 流式发送 + 限速 + 闸门 | 旧的 ROS 侧守护进程 | + +## 构建 + +零依赖，纯 CMake： + +```bash +cmake -S . -B build -DCMAKE_BUILD_TYPE=Release +cmake --build build -j +ctest --test-dir build --output-on-failure +``` + +它同时可以被 `colcon` 构建（通过 `package.xml` 声明为 plain-cmake 包）， +因此 ROS 工作空间能把它和其余 ROS 包一起构建。 + +## 消费方式 + +```cmake +find_package(litegrip_cpp REQUIRED) +target_link_libraries(my_node PRIVATE litegrip_cpp::litegrip_cpp) +``` + +```bash +pkg-config --cflags --libs litegrip_cpp +``` + +## 非 ROS 用法 + +```cpp +#include + +int main() { + litegrip::GripperConfig cfg; // can0, can_id 0x08, DM4310 + auto gripper = litegrip::LiteGrip::connect_raii(cfg); + gripper.init(); // 使能并保持当前位置 + gripper.open(); + gripper.goto_mm(40.0); + const auto state = gripper.get_state(); + return state.is_stale() ? 1 : 0; +} +``` + +## 本版本**不包含** + +按已确认的 v1 范围：`grasp()`、`set_force()`、`move_at_speed*()`，以及公开的零重力 +模式。`close(force_n=...)` 接受该参数但**忽略并明确告警**，因为施加夹持力需要力矩 +前馈与经过验证的力标定，两者都不在 v1 范围内。 + +## 安全不变量 + +这些是安全论证本身，**不得放宽**： + +1. **只能收紧** —— 限位只能是随包基线的子区间。 +2. **拒绝而非钳位** —— 越界命令带可诊断原因被拒绝，绝不静默改写后再发。 +3. **fail-closed** —— 取不到限位即拒绝一切；无法给出上界的量即不许朝那个方向动。 +4. **严格数值边界** —— NaN / ±inf / 非数在任何比较之前就被拒绝。 + +### 命令路径的接线 + +运动路径（`goto_rad` / `move_to` / `open` / `close` / `home`）会经过 +`SafetyGuard::guard_motion_frame`，且拒绝是**抛出而非钳位**。 + +有两条路径**有意**绕过该闸门，各自在定义处写明理由： + +- **`stop()`** —— 急停必须在红线之外也能工作。它改为断言零力矩不变量，这正是 + 该绕过合法的依据。 +- **标定流程** —— 它们本来就要把机构顶到**机械**端点，而机械端点在红线之外。 + （标定与红线之间的正确关系仍是一个待定事项，见方案中的未决项。） + +## 红线尚未按本台夹爪重建 + +随包的安全基线携带的是**参考台**的手推实测值，且文件内已明确标注 +**未在本机验证**。在真机运动之前必须重新标定（卡尺 + 闭合端重设零点）并以实测值 +重建红线；同时 `ControlLoopConfig::max_feedback_velocity_rad_s` 必须先行标定 —— +在该值给出之前，控制环**拒绝发送任何运动帧**（这是刻意的 fail-closed）。 + +推论：若某台夹爪的标定使闭合端落在红线之外，本 SDK 会（正确地）**拒绝一切运动**， +直到红线被重建。 + +## 测试 + +```bash +ctest --test-dir build --output-on-failure +``` + +两类测试： + +- **手工摘取的 golden vector**（`test_protocol.cpp`、`test_motor.cpp`）—— 把 + Python 测试里的用例逐条移植过来，使移植偏差在 CI 里暴露，而不是在真机上。 +- **生成式对拍**（`test_golden.cpp` + `test/golden_generated.hpp`）—— + `test/generate_golden.py` 驱动**真实的 Python SDK**，把它产生的字节原样导出 + （量化、MIT 打包、状态解码、参数帧），C++ 侧必须逐一复现。共 1284 项校验。 + 改动 Python 原件后重新生成：`python3 test/generate_golden.py`。 + +`test_bus.cpp`、`test_gripper.cpp`、`test_safety.cpp`、`test_control_loop.cpp` +覆盖**无硬件**时也必须成立的行为：未连接时的各项拒绝、接口缺失的错误路径、配置 +透传、可注入的持位策略、标定文件往返，以及安全闸门的每一条判据（含所有 +「必须拒绝」的对抗性用例 —— 每一条被放行都会让硬件动起来）。 + +`test_transport.cpp` 刻意**不发送任何帧**（本机可能接有真机）：它只读取接口 MTU、 +验证接口缺失的错误路径、并在 `can0` 上开/关 socket。**发送/接收路径尚无测试 +覆盖** —— 需要 vcan 接口（需 root）或真机。 + +同样没有覆盖、需要真机的部分：连接、`init`/使能、运动、标定。 + +## 许可证 + +MIT。 diff --git a/calibration/factory_calibration.json b/calibration/factory_calibration.json new file mode 100644 index 0000000..07684e2 --- /dev/null +++ b/calibration/factory_calibration.json @@ -0,0 +1,14 @@ +{ + "channel": "can0", + "can_id": 8, + "mst_id": 24, + "canfd_mode": false, + "zero_position_rad": 0.114, + "max_position_rad": -1.491, + "travel_range_rad": 1.605, + "rad_to_mm": 74.8, + "motor_type": "DM4310", + "kp": 100.0, + "kd": 2.0, + "grasp_torque_threshold": 0.5 +} diff --git a/calibration/safety_limits_025.json b/calibration/safety_limits_025.json new file mode 100644 index 0000000..60fc19d --- /dev/null +++ b/calibration/safety_limits_025.json @@ -0,0 +1,38 @@ +{ + "kind": "safety_limits", + "baseline_version": "0.25", + "is_default_baseline": false, + "supersedes": null, + "device": "LiteGrip", + "can_id": 8, + "mst_id": 24, + + "mechanical_observed_min_rad": -1.272793, + "mechanical_observed_max_rad": 0.051308, + "red_open_limit_rad": -1.24, + "red_close_limit_rad": -0.01, + "red_line_enabled": true, + + "hard_torque_limit_nm": 0.25, + + "torque_basis": { + "value_nm": 0.25, + "meaning": "the previous, much tighter torque baseline, kept as an independent rollback path", + "note": "the red lines are identical to the 3.5 baseline; only the torque ceiling differs" + }, + + "provenance": [ + "Retained so a rollback is an explicit, auditable version choice rather than an in-place edit of a file's contents.", + "It does NOT participate in default selection: only load_safety_baseline(\"0.25\") picks it up." + ], + + "warnings": [ + "★ NOT VERIFIED ON THIS UNIT — same caveat as safety_limits_350.json: the interval is the reference unit's measurement, to be re-derived before real-hardware motion.", + "★ This is a deliberately conservative ceiling, not a measured one: expect normal grasps to be torque-limited by it." + ], + + "invariants": [ + "mechanical_observed_min_rad <= red_open_limit_rad < red_close_limit_rad <= mechanical_observed_max_rad", + "A config file may only NARROW this baseline; any attempt to widen it is rejected." + ] +} diff --git a/calibration/safety_limits_350.json b/calibration/safety_limits_350.json new file mode 100644 index 0000000..5d2dba4 --- /dev/null +++ b/calibration/safety_limits_350.json @@ -0,0 +1,43 @@ +{ + "kind": "safety_limits", + "baseline_version": "3.5", + "is_default_baseline": true, + "supersedes": "0.25", + "device": "LiteGrip", + "can_id": 8, + "mst_id": 24, + + "mechanical_observed_min_rad": -1.272793, + "mechanical_observed_max_rad": 0.051308, + "red_open_limit_rad": -1.24, + "red_close_limit_rad": -0.01, + "red_line_enabled": true, + + "hard_torque_limit_nm": 3.5, + + "torque_basis": { + "value_nm": 3.5, + "meaning": "rated torque (continuous) of the DM-J4310-2EC module, output side", + "peak_note": "the 12.5 N.m peak is a short-time rating needing duty-cycle and temperature-rise data this project does not have, so the rated value is used", + "not_a_basis": "the 10 N.m MIT frame tau range is an encoding range, not a protection threshold" + }, + + "provenance": [ + "The interval values are inherited from the vendor calibration snapshot; the upstream SDK has no concept of red lines.", + "Open-side margin to the observed boundary: 0.032793 rad (about 2.99 mm). Closed-side: 0.061308 rad (about 5.60 mm).", + "Sign convention: the closed end has the larger rad value, the open end the smaller (more negative)." + ], + + "warnings": [ + "★ NOT VERIFIED ON THIS UNIT. These numbers are the REFERENCE unit's zero-gravity hand-push measurement, not a measurement of the gripper this SDK is deployed on. Treat them as an UPPER BOUND only.", + "★ The caliper calibration for the deployment unit has not been done, and the zero point was once reset with the 0xFE command, so the red lines' physical meaning may have shifted relative to the vendor zero.", + "★ Before real-hardware motion: re-derive the red lines (caliper measurement plus a closed-end zero reset) and then recalibrate ControlLoopConfig::max_feedback_velocity_rad_s. Until that is done the control loop refuses to send any motion frame, which is deliberate fail-closed behaviour." + ], + + "invariants": [ + "mechanical_observed_min_rad <= red_open_limit_rad < red_close_limit_rad <= mechanical_observed_max_rad", + "The red lines must lie inside the mechanically observed range, otherwise the loader refuses the file (fail-closed).", + "hard_torque_limit_nm is an upper bound only: the effective value is min(requested, this value, the SDK hard ceiling).", + "A config file may only NARROW this baseline; any attempt to widen it is rejected." + ] +} diff --git a/cmake/litegrip_cpp-config.cmake.in b/cmake/litegrip_cpp-config.cmake.in new file mode 100644 index 0000000..c7a59dc --- /dev/null +++ b/cmake/litegrip_cpp-config.cmake.in @@ -0,0 +1,15 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(Threads) + +include("${CMAKE_CURRENT_LIST_DIR}/litegrip_cppTargets.cmake") + +# Where the calibration / safety-baseline files were installed. Consumers need +# this to point the library at a baseline explicitly (LITEGRIP_DATA_DIR, or the +# safety_baseline parameter taking a full path) when the run-time search would +# not find them. +set_and_check(litegrip_cpp_DATA_DIR + "${PACKAGE_PREFIX_DIR}/@CMAKE_INSTALL_DATADIR@/litegrip_cpp/calibration") + +check_required_components(litegrip_cpp) diff --git a/cmake/litegrip_cpp.pc.in b/cmake/litegrip_cpp.pc.in new file mode 100644 index 0000000..c1563a5 --- /dev/null +++ b/cmake/litegrip_cpp.pc.in @@ -0,0 +1,16 @@ +# Relocatable: the .pc is installed into /lib/pkgconfig, so prefix is +# derived from its own location. That way `cmake --install --prefix ` +# produces a correct .pc without a reconfigure, and a relocated install keeps +# working. +prefix=${pcfiledir}/../.. +exec_prefix=${prefix} +libdir=${prefix}/@CMAKE_INSTALL_LIBDIR@ +includedir=${prefix}/@CMAKE_INSTALL_INCLUDEDIR@ +datadir=${prefix}/@CMAKE_INSTALL_DATADIR@ + +Name: litegrip_cpp +Description: ROS-agnostic C++ SDK for the LiteGrip adaptive two-finger gripper (SocketCAN / Damiao DM4310 MIT protocol) +Version: @PROJECT_VERSION@ +Libs: -L${libdir} -llitegrip_cpp +Libs.private: -lpthread +Cflags: -I${includedir} diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 0000000..85fa0b1 --- /dev/null +++ b/examples/CMakeLists.txt @@ -0,0 +1,15 @@ +# Examples are plain C++ programs that link the SDK. They are the +# "non-ROS consumer" proof: nothing here may include ROS headers. +# +# ⚠ Examples that open can0 and drive the motor are added in T2–T4; the T1 set +# is limited to things that cannot move hardware. + +set(LITEGRIP_CPP_EXAMPLES + version_info.cpp +) + +foreach(example_src IN LISTS LITEGRIP_CPP_EXAMPLES) + get_filename_component(example_name ${example_src} NAME_WE) + add_executable(litegrip_example_${example_name} ${example_src}) + target_link_libraries(litegrip_example_${example_name} PRIVATE litegrip_cpp::litegrip_cpp) +endforeach() diff --git a/examples/version_info.cpp b/examples/version_info.cpp new file mode 100644 index 0000000..8b032cc --- /dev/null +++ b/examples/version_info.cpp @@ -0,0 +1,10 @@ +// version_info.cpp — smallest possible consumer of the SDK. + +#include + +#include "litegrip/version.hpp" + +int main() { + std::cout << "litegrip_cpp " << litegrip::version() << "\n"; + return 0; +} diff --git a/include/litegrip/bus.hpp b/include/litegrip/bus.hpp new file mode 100644 index 0000000..24979c6 --- /dev/null +++ b/include/litegrip/bus.hpp @@ -0,0 +1,141 @@ +// litegrip/bus.hpp — single-gripper CAN bus layer (GripperBus). +// +// Port of litegrip_driver/litegrip/protocols/can_bus.py (LiteGripCAN). Renamed +// to GripperBus in the C++ SDK: it is no longer "the CAN module", it is the +// bus-level API for one gripper, and the name should say what it is. +// +// This layer knows about ONE gripper motor. For several motors on one bus use +// litegrip::can::MotorController directly. + +#pragma once + +#include +#include +#include + +#include "litegrip/can/controller.hpp" +#include "litegrip/can/motor.hpp" +#include "litegrip/can/transport.hpp" +#include "litegrip/hold_policy.hpp" +#include "litegrip/models.hpp" + +namespace litegrip { + +/// Gripper bus client: connect, register the motor, init/hold, stream MIT +/// frames, read state. Throws ConnectError / CommError / HardwareError / +/// NotInitializedError. +class GripperBus { + public: + explicit GripperBus(GripperConfig config = GripperConfig{}); + ~GripperBus(); + GripperBus(const GripperBus&) = delete; + GripperBus& operator=(const GripperBus&) = delete; + + // ── connection ──────────────────────────────────────────────────────── + + /// Open the CAN transport and register the gripper motor (auto-detecting + /// mst_id when the config leaves it unset). + bool connect(); + + /// Disable (when initialised) and close the transport. + void disconnect(); + + bool is_connected() const noexcept { return connected_; } + bool is_initialized() const noexcept { return initialized_; } + + /// Register the gripper motor; returns the actual (possibly detected) mst_id. + int register_gripper(int can_id = GripperParams::kCanId, + std::optional mst_id = std::nullopt, + can::MotorType motor_type = can::MotorType::kDM4310); + + // ── enable / init / hold ────────────────────────────────────────────── + + /// Full initialisation. Once enabled the motor is left HOLDING ITS CURRENT + /// POSITION (see HoldPolicy), not outputting zero torque — so it cannot jump + /// toward whatever target the previous session left behind. + /// + /// Returns true when the motor answers and is healthy (error 0 or 1). + /// Throws HardwareError on timeout / persistent fault / no feedback. + bool init(std::optional kp = std::nullopt, + std::optional kd = std::nullopt); + + /// Enable only (no full init). Prefer init(); the motor is likewise left + /// holding position, and a silent motor counts as failure, not success. + bool enable(std::optional kp = std::nullopt, + std::optional kd = std::nullopt); + + bool disable(); + + /// Clear a latched fault and re-enable, again ending in hold-at-current. + bool clear_fault(std::optional kp = std::nullopt, + std::optional kd = std::nullopt); + + /// Replace the hold strategy (R2). Takes effect on the next init()/enable(). + void set_hold_policy(std::unique_ptr policy); + const HoldPolicy& hold_policy() const noexcept { return *hold_policy_; } + + // ── motion ──────────────────────────────────────────────────────────── + + /// Send one MIT control frame. Returns false when not initialised. + bool control_mit(double q_target, double kp, double kd, + double dq_target = 0.0, double tau_feedforward = 0.0); + + /// Stream MIT frames for a duration (blocking). DM motors need a continuous + /// frame stream to sustain motion. + bool control_mit_stream(double q_target, double kp, double kd, + double duration_s, double dq_target = 0.0, + double tau_feedforward = 0.0, + double interval_s = 0.005); + + // ── status ──────────────────────────────────────────────────────────── + + /// Poll one frame; true when a status frame for this gripper was decoded. + bool poll(double timeout_s = 0.0); + + /// Poll until at least one new status frame arrives. + bool update_state(double timeout_s = 0.05); + + /// Send the 0xCC refresh and wait for the reply. A disabled motor does not + /// stream status frames on its own, so this is how position is read before + /// the first enable. Sends no motion command and changes no motor output. + bool refresh_status(double timeout_s = 0.5); + + double get_position() const; // rad + double get_velocity() const; // rad/s + double get_torque() const; // N.m + int get_error() const; // 0=disabled 1=enabled 0x9=UV ... + int get_temperature_mos() const; + int get_temperature_coil() const; + + /// Read a motor register by rid; throws CANTimeoutError. + double read_param(int rid, double timeout_s = 0.5); + + /// Write a motor register (no confirmation wait). + void write_param(int rid, double value); + + /// Expert access to the underlying motor state (may be null before connect). + can::MotorState* motor() noexcept { return motor_; } + + const GripperConfig& config() const noexcept { return config_; } + GripperConfig& config() noexcept { return config_; } + + private: + /// Send 0xFC, let the hold policy cover the enable window and settle, then + /// report the motor's error code from the first fresh status frame. + /// + /// Returns nullopt when no feedback arrived (the policy throws HardwareError, + /// which is caught here). This mirrors the Python original's + /// `_enable_and_hold`, including the distinction between "no answer" + /// (nullopt) and "answered with a fault" (the code). + std::optional enable_and_hold(const GripperConfig& effective_config); + + GripperConfig config_; + std::unique_ptr transport_; + std::unique_ptr controller_; + std::unique_ptr hold_policy_; + can::MotorState* motor_ = nullptr; // owned by controller_ + bool connected_ = false; + bool initialized_ = false; +}; + +} // namespace litegrip diff --git a/include/litegrip/calibration.hpp b/include/litegrip/calibration.hpp new file mode 100644 index 0000000..2ab1446 --- /dev/null +++ b/include/litegrip/calibration.hpp @@ -0,0 +1,62 @@ +// litegrip/calibration.hpp — calibration JSON load/save. +// +// Format is deliberately identical to the Python SDK's (same keys, same units) +// so the two can be compared side by side during the port's parallel-validation +// phase (see PLAN-litegrip-cpp.md D8). +// +// Path resolution mirrors the Python original: +// * env var LITEGRIP_CALIB, else ~/.litegrip/litegrip_calibration.json +// * when that file does not exist, fall back to the packaged, read-only +// factory_calibration.json + +#pragma once + +#include +#include + +#include "litegrip/models.hpp" + +namespace litegrip { + +/// Environment variable overriding the user calibration path. +inline constexpr const char* kCalibEnvVar = "LITEGRIP_CALIB"; + +/// Default user calibration path: $LITEGRIP_CALIB, else ~/.litegrip/litegrip_calibration.json. +/// +/// A stable absolute location, so a calibration saved without an explicit path +/// is picked up next run regardless of the process working directory. +std::string default_calibration_path(); + +/// Path of the packaged factory calibration (read-only fallback). +std::string factory_calibration_path(); + +/// Calibration file contents, decoded. +struct CalibrationFile { + bool has_closed = false; + double zero_position_rad = 0.0; // closed limit + bool has_open = false; + double max_position_rad = 0.0; // open limit + bool has_rad_to_mm = false; + double rad_to_mm = 0.0; + + // Optional fields (present in newer calibration files). + std::optional can_id; + std::optional mst_id; + std::optional channel; + std::optional canfd_mode; + std::optional kp; + std::optional kd; + std::optional grasp_torque_threshold; + std::optional motor_type; +}; + +/// Read + decode a calibration file. Returns nullopt when the file is missing +/// or malformed. +std::optional read_calibration_file(const std::string& path); + +/// Write a calibration file (creates parent directories). Throws CommError on +/// I/O failure. +void write_calibration_file(const std::string& path, const GripperConfig& config, + int can_id, int mst_id, const std::string& motor_type); + +} // namespace litegrip diff --git a/include/litegrip/can/controller.hpp b/include/litegrip/can/controller.hpp new file mode 100644 index 0000000..2556d2d --- /dev/null +++ b/include/litegrip/can/controller.hpp @@ -0,0 +1,109 @@ +// litegrip/can/controller.hpp — manages DM motors over one CAN transport. +// +// Port of litegrip_driver/litegrip/can/controller.py. Kept multi-motor capable +// even though LiteGrip uses a single motor: it is the reusable "outside ROS" +// surface for anyone driving several Damiao motors on one bus. + +#pragma once + +#include +#include +#include +#include +#include + +#include "litegrip/can/motor.hpp" +#include "litegrip/can/protocol.hpp" +#include "litegrip/can/transport.hpp" + +namespace litegrip::can { + +/// Owns the motors registered on one transport and dispatches CAN traffic. +/// +/// Not thread-safe. +class MotorController { + public: + explicit MotorController(CanTransport& transport) : transport_(transport) {} + ~MotorController(); + MotorController(const MotorController&) = delete; + MotorController& operator=(const MotorController&) = delete; + + // ── motor management ────────────────────────────────────────────────── + + /// Register a motor. When `mst_id` is nullopt it is auto-detected (register + /// read, then a status-refresh probe, then a hardcoded default). + /// + /// Returns a reference valid until remove_motor(). + MotorState& add_motor(int can_id, std::optional mst_id = std::nullopt, + MotorType motor_type = MotorType::kDM4310, + ControlMode control_mode = ControlMode::kMit); + + void remove_motor(const MotorState& motor); + + MotorState* get_motor(int mst_id); + MotorState* get_motor_by_can_id(int can_id); + + /// Registered motors, keyed by mst_id. + const std::unordered_map>& motors() const noexcept { + return motors_; + } + + // ── command dispatch ────────────────────────────────────────────────── + + /// Commands are repeated `count` times for reliability (per DM protocol). + void send_command(MotorState& motor, std::uint8_t cmd, int count = 5, + double interval_s = 0.002); + + void enable(MotorState& motor); + void disable(MotorState& motor); + void clear_fault(MotorState& motor); + void set_zero(MotorState& motor); + + /// Request a status frame refresh (0xCC). Does not change motor output. + void refresh_status(MotorState& motor); + + // ── control modes ───────────────────────────────────────────────────── + + /// Write CTRL_MODE and verify it took effect. Returns false when the motor + /// did not confirm the change. + bool switch_control_mode(MotorState& motor, ControlModeCode mode_code); + + // ── MIT control ─────────────────────────────────────────────────────── + + /// Send one MIT control frame. Call in a loop at >= 200 Hz for smooth motion. + void control_mit(MotorState& motor, double kp, double kd, double q, + double dq = 0.0, double tau = 0.0); + + // ── parameter access ────────────────────────────────────────────────── + + /// Read a register; throws CANTimeoutError when the motor does not answer. + double read_param(MotorState& motor, DmReg rid, double timeout_s = 0.5); + + /// Write a register (no confirmation wait). + void write_param(MotorState& motor, DmReg rid, double value); + + /// Save parameters to flash. The motor must be disabled first. + void save_params(MotorState& motor); + + // ── polling ─────────────────────────────────────────────────────────── + + /// Poll one frame; decode it if it belongs to a registered motor. + /// Returns the updated motor, or nullptr when no relevant frame arrived. + MotorState* poll(double timeout_s = 0.0); + + /// Poll until at least one new status frame arrives for `motor`. + bool poll_until(MotorState& motor, double timeout_s = 1.0); + + void close(); + + private: + /// MST_ID fallback: LiteGrip ships with mst_id == 0x18 (24). + static constexpr int kDefaultMstId = 0x18; + + int detect_mst_id(int can_id, double timeout_s = 0.5); + + CanTransport& transport_; + std::unordered_map> motors_; // by mst_id +}; + +} // namespace litegrip::can diff --git a/include/litegrip/can/motor.hpp b/include/litegrip/can/motor.hpp new file mode 100644 index 0000000..b2f3976 --- /dev/null +++ b/include/litegrip/can/motor.hpp @@ -0,0 +1,106 @@ +// litegrip/can/motor.hpp — per-motor configuration and decoded state. + +#pragma once + +#include + +#include "litegrip/can/protocol.hpp" + +namespace litegrip::can { + +/// Damiao motor model indices (matching DM_Motor_Type). +enum class MotorType : int { + kDM3507 = 0, + kDM4310 = 1, + kDM4310_48V = 2, + kDM4340 = 3, + kDM4340_48V = 4, + kDM6006 = 5, + kDM6248P = 6, + kDM8006 = 7, + kDM8009 = 8, + kDM10010L = 9, + kDM10010 = 10, + kDMH3510 = 11, + kDMH6215 = 12, + kDMS3519 = 13, + kDMG6220 = 14, +}; + +/// Configuration for a single motor. Mirrors motor.MotorParams. +struct MotorParams { + MotorType motor_type = MotorType::kDM4310; + int can_id = 0x08; + int mst_id = 0x18; // master id = CAN id the status frames arrive on + ControlMode control_mode = ControlMode::kMit; + MotorLimits limits{}; // derived from motor_type when default-constructed +}; + +/// Runtime state of one motor, updated from status frames. +/// +/// Not thread-safe: the caller must serialise updates (same contract as the +/// Python original). +class MotorState { + public: + explicit MotorState(const MotorParams& params = MotorParams{}); + + // ── configuration (immutable after construction, except control mode) ── + int can_id() const noexcept { return params_.can_id; } + int mst_id() const noexcept { return params_.mst_id; } + MotorType motor_type() const noexcept { return params_.motor_type; } + const MotorLimits& limits() const noexcept { return params_.limits; } + ControlMode control_mode() const noexcept { return params_.control_mode; } + + /// CAN id offset for the current control mode. + int mode_offset() const noexcept { + return static_cast(params_.control_mode); + } + + /// Change mode without sending a CAN command. + void set_mode(ControlMode mode) noexcept { params_.control_mode = mode; } + + // ── decoded feedback ────────────────────────────────────────────────── + double position() const noexcept { return position_; } + double velocity() const noexcept { return velocity_; } + double torque() const noexcept { return torque_; } + int error() const noexcept { return error_; } + int t_mos() const noexcept { return t_mos_; } + int t_coil() const noexcept { return t_coil_; } + + bool is_enabled() const noexcept { return error_ == 1; } + bool is_fault() const noexcept { return error_ != 0 && error_ != 1; } + + /// True once at least one status frame has been decoded. + /// + /// A disabled DM motor does not stream status frames, so before the first + /// enable every value here is still the constructor default — position 0.0 + /// with temperatures 0/0. Guard on this before trusting a reading. + bool has_data() const noexcept { return rx_count_ > 0; } + + /// Seconds since the last decoded status frame; infinity when none arrived. + double data_age_s() const noexcept; + + /// Monotonic time of the last decoded status frame (seconds, 0 = never). + /// Kept on the same clock as CanFrame::timestamp so data_age_s() is valid. + double last_update() const noexcept { return last_update_; } + + std::uint64_t rx_count() const noexcept { return rx_count_; } + + /// Called by MotorController when a status frame for this motor arrives. + void update_from_status(double position, double velocity, double torque, + int error, int t_mos, int t_coil, + double timestamp) noexcept; + + private: + MotorParams params_; + double position_ = 0.0; + double velocity_ = 0.0; + double torque_ = 0.0; + int error_ = 0; + int t_mos_ = 0; + int t_coil_ = 0; + std::uint64_t rx_count_ = 0; + double last_update_ = 0.0; +}; + +} // namespace litegrip::can diff --git a/include/litegrip/can/protocol.hpp b/include/litegrip/can/protocol.hpp new file mode 100644 index 0000000..80a6127 --- /dev/null +++ b/include/litegrip/can/protocol.hpp @@ -0,0 +1,174 @@ +// litegrip/can/protocol.hpp — Damiao motor CAN protocol codec (pure functions). +// +// Port of litegrip_driver/litegrip/can/protocol.py. State-free: everything here +// is a pure encode/decode. The bit layouts are protocol-spec, do not "tidy" +// them. +// +// Reference: DM4310/DM4340/DM6248P CAN protocol specification. + +#pragma once + +#include +#include +#include +#include + +namespace litegrip::can { + +/// Broadcast CAN ID used for parameter read/write/save and status refresh. +/// +/// ⚠ On the LiteGrip deployment bus this id is filtered by the STM32 firmware +/// (see the plan's notes), so broadcast-dependent SDK features are unavailable +/// there. Kept because the protocol layer must stay faithful/portable. +inline constexpr int kBroadcastId = 0x7FF; + +/// CAN ID offsets for control frames (added to the motor's can_id). +enum class ControlMode : int { + kMit = 0x000, + kPosVel = 0x100, + kVel = 0x200, + kPosForce = 0x300, +}; + +/// CTRL_MODE register values. +enum class ControlModeCode : int { + kMit = 1, + kPosVel = 2, + kVel = 3, + kPosForce = 4, +}; + +/// Damiao motor register IDs. +enum class DmReg : std::uint8_t { + kUvValue = 0, + kKtValue = 1, + kOtValue = 2, + kOcValue = 3, + kAcc = 4, + kDec = 5, + kMaxSpd = 6, + kMstId = 7, + kEscId = 8, + kTimeout = 9, + kCtrlMode = 10, + kDamp = 11, + kInertia = 12, + kHwVer = 13, + kSwVer = 14, + kSn = 15, + kNpp = 16, + kRs = 17, + kLs = 18, + kFlux = 19, + kGr = 20, + kPmax = 21, + kVmax = 22, + kTmax = 23, + kIBw = 24, + kKpAsr = 25, + kKiAsr = 26, + kKpApr = 27, + kKiApr = 28, + kOvValue = 29, + kGref = 30, + kDeta = 31, + kVBw = 32, + kIqC1 = 33, + kVlC1 = 34, + kCanBr = 35, + kSubVer = 36, +}; + +/// True for registers whose value is an integer (little-endian uint32 on the +/// wire) rather than an IEEE-754 float32. Mirrors protocol._INT_REG_RANGES. +bool is_int_register(int rid); + +// ── quantization helpers ────────────────────────────────────────────────── + +/// Quantize a float to an unsigned integer of the given bit width. +std::uint32_t float_to_uint(double value, double value_min, double value_max, + int bits) noexcept; + +/// Dequantize an unsigned integer back to a float. +double uint_to_float(std::uint32_t value, double value_min, double value_max, + int bits) noexcept; + +// ── MIT control frame ───────────────────────────────────────────────────── + +/// MIT frame quantization limits for a motor type. +struct MotorLimits { + double q_max = 12.5; // rad + double dq_max = 30.0; // rad/s + double tau_max = 10.0; // N.m +}; + +/// Limits for a DM_Motor_Type index; falls back to DM4310 for unknown types. +MotorLimits get_motor_limits(int motor_type); + +/// 8-byte MIT control payload: q/dq/kp/kd/tau mapped into their bit fields. +std::array pack_mit_frame(double q, double dq, double kp, + double kd, double tau, + const MotorLimits& limits) noexcept; + +// ── status frame (motor -> host) ────────────────────────────────────────── + +/// Parsed motor status frame. +struct ParsedStatus { + int err = 0; // 4-bit error code (0=disabled, 1=enabled, 0x9=UV, ...) + int can_id = 0; // 4-bit CAN id (low nibble of data[0]) + double q = 0.0; // rad + double dq = 0.0; // rad/s + double tau = 0.0; // N.m + int t_mos = 0; // MOS temperature (degC) + int t_coil = 0; // coil temperature (degC) +}; + +/// Parse an 8-byte status frame. Returns std::nullopt if `size < 8`. +std::optional unpack_status_frame(const std::uint8_t* data, + std::size_t size, + const MotorLimits& limits) noexcept; + +// ── command frames (host -> motor) ──────────────────────────────────────── + +inline constexpr std::uint8_t kCmdEnable = 0xFC; +inline constexpr std::uint8_t kCmdDisable = 0xFD; +inline constexpr std::uint8_t kCmdClearFault = 0xFB; +inline constexpr std::uint8_t kCmdSetZero = 0xFE; + +/// 8 bytes of 0xFF except byte 7 which holds the command. +std::array pack_command_frame(std::uint8_t cmd) noexcept; + +/// 4-byte status refresh request: [can_id_lo, can_id_hi, 0xCC, 0x00]. +std::array pack_refresh_frame(int can_id) noexcept; + +// ── parameter read/write/save frames ────────────────────────────────────── + +/// 8-byte read request: [can_id_lo, can_id_hi, 0x33, rid, 0, 0, 0, 0]. +std::array pack_read_param_frame(int can_id, int rid) noexcept; + +/// 8-byte write request: [can_id_lo, can_id_hi, 0x55, rid, data[0..3]]. +/// Int registers take a uint32 (little-endian); float registers an IEEE-754 +/// float32 (little-endian). +std::array pack_write_param_frame(int can_id, int rid, + double value) noexcept; + +/// 8-byte save-to-flash request: [can_id_lo, can_id_hi, 0xAA, 0x01, 0,0,0,0]. +/// The motor must be disabled before saving. +std::array pack_save_param_frame(int can_id) noexcept; + +// ── parameter response frame (motor -> host) ────────────────────────────── + +/// Parsed parameter read/write response. +struct ParsedParamResponse { + int can_id = 0; // motor CAN id (data[0] & 0x0F) + int opcode = 0; // 0x33 = read, 0x55 = write, 0xAA = save + int rid = 0; // register id + double value = 0.0; // decoded value (int registers cast to double) +}; + +/// Parse a parameter response. Returns std::nullopt when the frame is too +/// short or the opcode is not one of 0x33/0x55/0xAA. +std::optional unpack_param_response(const std::uint8_t* data, + std::size_t size) noexcept; + +} // namespace litegrip::can diff --git a/include/litegrip/can/transport.hpp b/include/litegrip/can/transport.hpp new file mode 100644 index 0000000..55d7a7c --- /dev/null +++ b/include/litegrip/can/transport.hpp @@ -0,0 +1,104 @@ +// litegrip/can/transport.hpp — raw SocketCAN transport. +// +// Port of litegrip_driver/litegrip/can/transport.py. Nothing but Linux +// SocketCAN and the C++ standard library; no libsocketcan, no third-party +// dependency. + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace litegrip::can { + +/// CAN interface mode. +enum class CanMode : int { + kCan = 0, // classic CAN, 8-byte payload + kCanFd = 1, // CAN FD, up to 64-byte payload +}; + +/// Classic CAN frame MTU / CAN FD frame MTU, as reported by SIOCGIFMTU. +inline constexpr int kCanMtu = 16; +inline constexpr int kCanFdMtu = 72; + +/// A single CAN frame. +struct CanFrame { + int can_id = 0; + std::array data{}; + std::size_t dlc = 0; + bool is_extended = false; + bool is_fd = false; + double timestamp = 0.0; // monotonic seconds + + const std::uint8_t* bytes() const noexcept { return data.data(); } +}; + +/// SocketCAN transport: opens one CAN socket and provides send/recv. +/// +/// Not thread-safe. `recv()` may be called from a control loop; `send()` from +/// the same thread. +class CanTransport { + public: + /// `mode` is a *preference*: open() reconciles it with the interface MTU + /// (classic CAN sockets on FD-capable interfaces may silently drop frames). + explicit CanTransport(std::string channel, CanMode mode = CanMode::kCan); + ~CanTransport(); + + CanTransport(const CanTransport&) = delete; + CanTransport& operator=(const CanTransport&) = delete; + CanTransport(CanTransport&& other) noexcept; + CanTransport& operator=(CanTransport&& other) noexcept; + + /// Open and bind the CAN socket. Throws ConnectError if the interface is + /// missing or down. + void open(); + + void close(); + bool is_open() const noexcept { return fd_ >= 0; } + + const std::string& channel() const noexcept { return channel_; } + CanMode mode() const noexcept { return mode_; } + + /// Restrict which CAN ids the kernel delivers to this socket. + /// + /// On a shared bus (e.g. a full arm) other devices chatter at high rate; + /// without a hardware filter every foreign frame lands in this socket's RX + /// buffer, a one-frame-per-cycle reader falls behind, and the motor's own + /// status frames are delayed or dropped — making read-back appear frozen + /// while the motor is actually moving. Empty clears the filter. + void set_id_filter(const std::vector& can_ids); + void add_id_filter(int can_id); + + /// Send one frame. Retries on EAGAIN until `timeout_s`; on exhaustion the + /// frame is dropped and a warning is logged (matching the Python original). + void send(int can_id, const std::uint8_t* data, std::size_t size, + double timeout_s = 0.1); + + /// Send the same frame `count` times, spacing by `interval_s`. + /// DM motors need repeated config commands for reliability. + void send_multi(int can_id, const std::uint8_t* data, std::size_t size, + int count, double interval_s = 0.002); + + /// Receive one frame; nullopt on timeout. `timeout_s == 0` = non-blocking. + std::optional recv(double timeout_s = 0.0); + + /// Read and discard everything pending. + void drain(); + + private: + void apply_id_filter(); + + std::string channel_; + CanMode mode_; + int fd_ = -1; + std::vector accept_ids_; +}; + +/// Read interface MTU (kCanMtu / kCanFdMtu); nullopt on failure. +std::optional iface_mtu(int fd, const std::string& iface) noexcept; + +} // namespace litegrip::can diff --git a/include/litegrip/constants.hpp b/include/litegrip/constants.hpp new file mode 100644 index 0000000..e8b8ba6 --- /dev/null +++ b/include/litegrip/constants.hpp @@ -0,0 +1,82 @@ +// litegrip/constants.hpp — gripper parameters, unit conversion, error codes. + +#pragma once + +#include +#include + +#include "litegrip/can/motor.hpp" +#include "litegrip/can/protocol.hpp" + +namespace litegrip { + +/// LiteGrip default parameters. Mirrors constants.GripperParams. +struct GripperParams { + static constexpr int kCanId = 0x08; + static constexpr int kMstId = 0x18; + static constexpr can::MotorType kMotorType = can::MotorType::kDM4310; + static constexpr can::ControlMode kControlMode = can::ControlMode::kMit; + + // Position limits (rad). Closed is numerically *larger* than open. + static constexpr double kPosClosedRad = 0.0; + static constexpr double kPosOpenRad = 1.14; + + // MIT quantization limits (DM4310). + static constexpr double kQMax = 12.5; // rad + static constexpr double kDqMax = 30.0; // rad/s + static constexpr double kTauMax = 10.0; // N.m + + // Default control gains. + static constexpr double kDefaultKp = 100.0; + static constexpr double kDefaultKd = 2.0; + + // Fault recovery. + static constexpr int kFaultClearRetries = 5; + static constexpr double kFaultClearDelayS = 0.02; +}; + +/// Unit conversion coefficients. Mirrors constants.UnitConversion. +/// +/// Nominal values for a 120 mm stroke gripper; run calibrate() for per-unit +/// accuracy. +struct UnitConversion { + static constexpr double kRadToMm = 120.0 / 1.14; // ~= 105.26 mm/rad + static constexpr double kMmToRad = 1.14 / 120.0; // ~= 0.0095 rad/mm + static constexpr double kNmToN = 10.0; // approximate N per N.m + static constexpr double kNToNm = 0.1; // approximate N.m per N +}; + +/// Damiao motor error codes (status frame data[0] >> 4). +enum class ErrorCode : std::uint8_t { + kDisabled = 0x0, + kEnabled = 0x1, + kOvFault = 0x8, + kUvFault = 0x9, + kOcFault = 0xA, + kMosOverTemp = 0xB, + kCoilOverTemp = 0xC, + kCommLoss = 0xD, + kOverload = 0xE, +}; + +/// Human-readable description for a motor error code. +std::string describe_error(int code); + +/// Damiao motor status word -> enabled? +constexpr bool error_code_is_enabled(int code) noexcept { return code == 0x1; } + +/// Damiao motor status word -> fault? (0 = disabled and 1 = enabled are not faults) +constexpr bool error_code_is_fault(int code) noexcept { + return code != 0x0 && code != 0x1; +} + +/// System defaults. Mirrors constants.DefaultParams. +struct DefaultParams { + static constexpr const char* kCanChannel = "can0"; + static constexpr int kCanBitrate = 1000000; // 1 Mbps classic CAN + static constexpr bool kCanfdMode = false; + static constexpr double kTimeoutS = 0.1; + static constexpr double kInitTimeoutS = 2.0; +}; + +} // namespace litegrip diff --git a/include/litegrip/control_loop.hpp b/include/litegrip/control_loop.hpp new file mode 100644 index 0000000..c42931c --- /dev/null +++ b/include/litegrip/control_loop.hpp @@ -0,0 +1,164 @@ +// litegrip/control_loop.hpp — background streaming control loop. +// +// Replaces the old ros2_control Python daemon (hw_daemon.py + sdk_adapter.py + +// the shared-memory bridge). With a C++ SDK the two-process split has no reason +// to exist: the ros2_control hardware component links this class directly. +// +// R1: the loop runs on its own background thread. A DM motor needs a continuous +// MIT frame stream (~900 ms of silence latches the 0xD comm-loss fault), and +// the controller_manager cycle is not guaranteed stable — so the plugin's +// write() only posts a target and its read() only reads a cached snapshot. +// +// What the loop owns (all of it used to be spread across safety_gate.py, +// driver_adapter.py's TrajectoryLimiter and sdk_adapter.py's _send_motion): +// target -> per-cycle rate limit -> torque budget split -> safety gate -> +// MIT frame -> poll feedback -> watchdog / temperature / fault checks. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "litegrip/models.hpp" +#include "litegrip/safety.hpp" + +namespace litegrip { + +/// Deployment configuration. Mirrors config/litegrip_hw.yaml: these describe +/// "how this gripper is allowed to move", not "what this command wants", so +/// they are fixed at construction and never decided per command. +struct ControlLoopConfig { + // ── hardware channel ────────────────────────────────────────────────── + std::string channel = DefaultParams::kCanChannel; + int can_id = GripperParams::kCanId; + std::optional mst_id = std::nullopt; + bool canfd_mode = DefaultParams::kCanfdMode; + + // ── the dual switch (both required for the real-hardware path) ───────── + // They block two different things: dry_run blocks "do not touch hardware + // while debugging", hardware_enable blocks "is a gripper actually attached + // to this machine". Merged into one, reporting the true state would require + // letting the loop drive the motor. + bool dry_run = true; + bool hardware_enable = false; + + // ── control loop ───────────────────────────────────────────────────── + double control_rate_hz = 200.0; + /// How long without a NEW feedback frame counts as comm loss. + double feedback_timeout_s = 0.5; + /// Temperature ceiling (degC), checked for MOS and coil. This is "stop before + /// the driver trips on its own" — the driver's overtemperature fault is + /// already past that point. + int temperature_limit_c = 80; + /// Stale-command criterion: hold position once no new command arrives for + /// this long. + double command_timeout_s = 0.2; + + // ── safety: rate ceiling and torque budget ─────────────────────────── + /// Command-trajectory advance rate ceiling (rad/s). NOT the dq field of the + /// MIT frame (that stays 0) — the loop advances the target towards the + /// commanded one by at most max_velocity * elapsed per cycle. Can only be + /// lowered, never raised above kMaxCommandVelocityCeilingRadS. + double max_velocity_rad_s = kMaxCommandVelocityCeilingRadS; + /// Total control torque budget (N.m), allocated between kp and kd using + /// worst-case bounds (damping first). Not written into any frame field. + /// Can only be lowered, never raised above kTorqueLimitCeilingNm. + double torque_limit_nm = kTorqueLimitCeilingNm; + /// Versioned safety baseline to load; must exist (fail-closed). + std::string safety_baseline = kDefaultSafetyBaseline; + + /// Worst-case position error bound (rad). -1 = derive from the red-line width. + double max_position_error_rad = -1.0; + + /// Worst-case feedback velocity bound (rad/s). + /// -1 (the default) means NOT GIVEN, which means REFUSE TO SEND ANY MOTION + /// FRAME. This is deliberate fail-closed, not a value to muddle through: the + /// real-hardware path must calibrate it first. + double max_feedback_velocity_rad_s = -1.0; + + /// MIT gain upper bounds; the loop re-allocates from the budget each frame. + double kp = 20.0; + double kd = 0.5; + + // ── calibration (needed to accept targets in millimetres) ───────────── + /// Motor angle at the closed end (0 mm); numerically LARGER than pos_open_rad. + double pos_closed_rad = GripperParams::kPosClosedRad; + /// Motor angle at full opening; numerically smaller. + double pos_open_rad = GripperParams::kPosOpenRad; + /// rad -> mm conversion for this unit. + double rad_to_mm = UnitConversion::kRadToMm; +}; + +/// Fault codes reported to the layer above. Values match the old bridge layer's +/// fault_code so existing consumers keep working. +enum class FaultCode : int { + kNone = 0, + kCommandRejected = 1, // out of range / non-finite / over budget + kNoFeedback = 2, // comm loss / sampler failed + kInternal = 3, + kUnsafeInitialState = 4, // hardware state does not permit enabling + kHardwareSafeStop = 5, // a safe stop was executed and latched +}; + +/// Motor faults occupy another segment: kMotorFaultBase + raw error code. +inline constexpr int kMotorFaultBase = 100; + +/// Owns the CAN connection, the safety gate and the streaming thread. +class ControlLoop { + public: + explicit ControlLoop(ControlLoopConfig config); + ~ControlLoop(); + ControlLoop(const ControlLoop&) = delete; + ControlLoop& operator=(const ControlLoop&) = delete; + + /// Load the safety baseline, connect, run init() (hold-at-current) and start + /// the control thread. Throws on any failure — a partially started loop is + /// never left behind. + bool start(); + + /// Stop the thread, safe-stop the motor and close the bus. Idempotent. + void stop(); + + bool is_running() const noexcept { return running_; } + + /// The configuration this loop was constructed with. + const ControlLoopConfig& config() const noexcept; + + // ── the plugin-facing surface (all thread-safe, non-blocking) ───────── + + /// Post a target. Values are clamped into the commandable range; the gate + /// still rejects anything outside the red lines. + void set_target_rad(double rad); + void set_target_mm(double mm); + + /// Enable/disable request (mirrors the old command interface's enable field). + void set_enable(bool enable); + void emergency_stop(); + + /// Cached state snapshot. Never touches CAN. + GripperState state() const; + + /// Latched fault code (0 = none). + int fault_code() const; + + /// Diagnostic read-only view of the safety limits in effect. + const SafetyLimits& safety_limits() const noexcept; + + private: + void thread_main(); + void hardware_cycle(double elapsed); + void dry_run_cycle(double elapsed); + void safe_stop(); + + struct Impl; + std::unique_ptr impl_; + std::atomic running_{false}; + std::thread thread_; +}; + +} // namespace litegrip diff --git a/include/litegrip/exceptions.hpp b/include/litegrip/exceptions.hpp new file mode 100644 index 0000000..02c9158 --- /dev/null +++ b/include/litegrip/exceptions.hpp @@ -0,0 +1,115 @@ +// litegrip/exceptions.hpp — exception hierarchy. +// +// Mirrors litegrip_driver/litegrip/exceptions.py, plus the safety exceptions +// that live in the safety-aware SDK variant's safety_limits.py +// (LimitViolation / SafetyFault / SafetyConfigError). +// +// Messages are English (library convention; the SDK is meant to be reusable +// outside this project). The Python original used Chinese messages — that is a +// deliberate, easily-reverted difference. + +#pragma once + +#include +#include +#include + +namespace litegrip { + +/// Base class for every LiteGrip SDK error. +/// +/// `error_code` mirrors the Python SDK: it carries the Damiao motor error code +/// when the failure came from the motor, and is absent otherwise. +class LiteGripError : public std::runtime_error { + public: + explicit LiteGripError(std::string message, int error_code = kNoErrorCode) + : std::runtime_error(render(message, error_code)), + message_(std::move(message)), + error_code_(error_code) {} + + /// Message without the "[0x..]" suffix. + const std::string& message() const noexcept { return message_; } + + /// Motor error code, or kNoErrorCode when the failure is not motor-originated. + int error_code() const noexcept { return error_code_; } + + bool has_error_code() const noexcept { return error_code_ != kNoErrorCode; } + + /// Sentinel meaning "no error code attached" (Python used None). + static constexpr int kNoErrorCode = -1; + + private: + static std::string render(const std::string& message, int error_code); + + std::string message_; + int error_code_; +}; + +/// CAN bus read/write failure. +class CommError : public LiteGripError { + public: + using LiteGripError::LiteGripError; +}; + +/// CAN interface unavailable / motor did not answer. +class ConnectError : public LiteGripError { + public: + using LiteGripError::LiteGripError; +}; + +/// Motor refused the command, or a parameter was out of range. +class CommandError : public LiteGripError { + public: + using LiteGripError::LiteGripError; +}; + +/// No answer from the bus within the timeout. +class CANTimeoutError : public LiteGripError { + public: + using LiteGripError::LiteGripError; +}; + +/// Damiao motor fault (undervoltage / overcurrent / overtemperature / ...). +class HardwareError : public LiteGripError { + public: + using LiteGripError::LiteGripError; +}; + +/// Operation requires a connection or an enabled motor that is not present. +class NotInitializedError : public LiteGripError { + public: + using LiteGripError::LiteGripError; +}; + +// ── safety ──────────────────────────────────────────────────────────────── + +/// A command violated a limit (red line / torque budget / velocity ceiling). +/// +/// The safety layer *rejects* rather than clamping — it never rewrites the +/// value and sends it. Callers must not swallow this. +class LimitViolation : public LiteGripError { + public: + using LiteGripError::LiteGripError; +}; + +/// A persistent safety condition (latched fault, watchdog, unsafe initial +/// state). Unlike LimitViolation this is written by the observer path and is +/// not cleared by the next valid command. +class SafetyFault : public LiteGripError { + public: + using LiteGripError::LiteGripError; +}; + +/// Force feed-forward was requested while force calibration is not verified. +class ForceCalibrationRequired : public SafetyFault { + public: + using ForceCalibrationRequired::SafetyFault::SafetyFault; +}; + +/// A safety-limits configuration is malformed or tries to *loosen* a baseline. +class SafetyConfigError : public LiteGripError { + public: + using LiteGripError::LiteGripError; +}; + +} // namespace litegrip diff --git a/include/litegrip/gripper.hpp b/include/litegrip/gripper.hpp new file mode 100644 index 0000000..6db5ed6 --- /dev/null +++ b/include/litegrip/gripper.hpp @@ -0,0 +1,207 @@ +// litegrip/gripper.hpp — high-level gripper API (LiteGrip). +// +// Port of litegrip_driver/litegrip/gripper.py. This is the entry point most +// non-ROS consumers use: connect, init (hold), open/close/goto, calibrate, +// read state. +// +// v1 capability scope (PLAN-litegrip-cpp.md D6). Deliberately NOT in v1: +// * grasp() / set_force() — force control, deferred +// * move_at_speed() / move_at_speed_rad() — deferred +// * public zero-gravity mode — the calibration flows use zero-torque +// streaming internally +// +// Naming: `goto` is a C++ keyword, so the millimetre-target method is goto_mm(). + +#pragma once + +#include +#include +#include +#include + +#include "litegrip/bus.hpp" +#include "litegrip/models.hpp" +#include "litegrip/safety.hpp" + +namespace litegrip { + +/// LiteGrip adaptive two-finger gripper. +/// +/// Lifetime (R4): construction does NOT touch the bus — a GripperConfig object +/// must be valid without hardware. connect() opens it explicitly (or use +/// connect_raii()); the destructor always disconnects, so a connected gripper +/// is never left enabled by accident. +class LiteGrip { + public: + explicit LiteGrip(GripperConfig config = GripperConfig{}); + ~LiteGrip(); + LiteGrip(const LiteGrip&) = delete; + LiteGrip& operator=(const LiteGrip&) = delete; + /// Movable so connect_raii() can return one. The moved-from object is left + /// disconnected and inert, so its destructor does nothing. + LiteGrip(LiteGrip&& other) noexcept; + LiteGrip& operator=(LiteGrip&& other) noexcept; + + /// Connect and return a live instance (throws ConnectError on failure). + /// The RAII one-liner for the non-ROS case. + static LiteGrip connect_raii(GripperConfig config = GripperConfig{}); + + // ── properties ──────────────────────────────────────────────────────── + const std::string& channel() const noexcept { return config_.can_channel; } + int can_id() const noexcept { return config_.can_id; } + std::optional mst_id() const noexcept { return mst_id_; } + bool is_connected() const noexcept { return connected_; } + bool is_enabled() const noexcept { return enabled_; } + const GripperConfig& config() const noexcept { return config_; } + GripperConfig& config() noexcept { return config_; } + + // ── connection ──────────────────────────────────────────────────────── + + /// Open the bus and register the motor. When the config leaves mst_id unset + /// it is auto-detected here. + bool connect(); + void disconnect(); + + // ── enable / init / fault ───────────────────────────────────────────── + + /// Enable, clearing a latched fault first if one is present. The motor ends + /// up holding its current position (see HoldPolicy / init()). + bool enable(); + + /// Initialise = enable and hold. This SDK's `init` means exactly that and + /// nothing else; the behaviour lives behind HoldPolicy (R2) so it can be + /// replaced without touching anything else. + bool init(); + + bool disable(); + + /// Clear latched faults (UV / OC / OT): disable -> clear (0xFB) -> enable, + /// retried up to kFaultClearRetries times. Throws HardwareError on failure. + bool clear_fault(); + + /// Emergency stop: send a zero-torque MIT frame. Does NOT disable the motor — + /// it stays enabled but exerts zero torque, so it can be back-driven. + void stop(); + + // ── low-level frame access ──────────────────────────────────────────── + + /// Send a single MIT control frame (expert use; sustained motion should go + /// through goto_rad() / move_to()). For custom control loops that manage + /// their own timing. + bool send_mit_frame(double q, double kp, double kd, double dq = 0.0, + double tau = 0.0); + + /// Poll one CAN frame and update the cached state. + bool poll(double timeout_s = 0.0); + + // ── motion ──────────────────────────────────────────────────────────── + + /// Move to the closed (zero) position. + bool home(); + + bool open(std::optional kp = std::nullopt, + std::optional kd = std::nullopt, double duration = 1.0); + + /// Close the gripper. + /// + /// `force_n` is accepted for source compatibility with the Python API but is + /// IGNORED in v1 and logs a warning: applying a grip force needs torque + /// feed-forward, which requires force calibration (kForceCalibrationVerified + /// is false in this SDK) and is out of v1 scope. + bool close(std::optional kp = std::nullopt, + std::optional kd = std::nullopt, + std::optional force_n = std::nullopt, double duration = 1.0); + + /// Move to an absolute position in millimetres (0 = closed). + bool goto_mm(double position_mm, std::optional kp = std::nullopt, + std::optional kd = std::nullopt, double duration = 0.5); + + /// Move to an absolute position in radians (streams MIT frames). + bool goto_rad(double position_rad, std::optional kp = std::nullopt, + std::optional kd = std::nullopt, double dq_target = 0.0, + double tau_feedforward = 0.0, double duration = 0.5); + + /// Sustained move to a target position (longer default duration). + bool move_to(double target_rad, std::optional kp = std::nullopt, + std::optional kd = std::nullopt, + double tau_feedforward = 0.0, double duration = 1.0); + + // ── calibration ─────────────────────────────────────────────────────── + + /// Automatic calibration: back off, step toward close until stall, back off, + /// step toward open until stall, then derive the conversion factor. + CalibrationData calibrate(double kp = 60.0, double kd = 2.0, + double step_rad = 0.1, double stall_delta = 0.0003, + int stall_cycles = 8, int max_iter = 30); + + /// Guided two-step calibration with the operator confirming each limit. + CalibrationData calibrate_guided(double kp = 60.0, double kd = 2.0, + double step_rad = 0.08, + double stall_delta = 0.0004, + int stall_cycles = 6, int max_iter = 40); + + /// Calibrate by hand-moving the gripper while it is in zero-torque mode. + CalibrationData calibrate_manual(double duration = 30.0, + double settle_time = 2.0, + double sample_interval = 0.01); + + /// Persist the current calibration. Returns the path written. + std::string save_calibration(std::optional path = std::nullopt); + + /// Load calibration into config. Tries `path` (default: the user path), then + /// falls back to the packaged factory calibration. Call after connect() and + /// before enable(). + bool load_calibration(std::optional path = std::nullopt); + + // ── state ───────────────────────────────────────────────────────────── + + /// Current state. `wait` = wait up to 50 ms for a fresh status frame; + /// false = return the cached snapshot immediately (control loops). + /// + /// A disabled motor does not stream status frames, so the snapshot may hold + /// constructor defaults — check is_stale() / data_age_s before trusting it, + /// or send a refresh frame first. + GripperState get_state(bool wait = true); + + /// Request a status frame even while disabled (0xCC refresh). + bool refresh_status(double timeout_s = 0.5); + + double get_position_mm(); + double get_position_rad(); + /// Estimated gripping force in N. Non-const on purpose: like its siblings it + /// refreshes the state, which is bus I/O. + double get_force(); + double get_torque(); + int get_error(); + std::pair get_temperature(); // (mos, coil) degC + GripperInfo get_info() const; + + bool is_moving(); + /// Torque exceeds the configured grasp-detection threshold. + bool is_grasped(); + bool wait_for_ready(double timeout = 5.0); + + // ── parameter access (expert) ───────────────────────────────────────── + + double read_param(int rid, double timeout_s = 0.5); + void write_param(int rid, double value); + + // ── safety (expert) ─────────────────────────────────────────────────── + + /// The safety guard in effect, for diagnostics. + const SafetyGuard& safety() const noexcept { return *safety_; } + + private: + void check_connected() const; + void check_enabled() const; + + GripperConfig config_; + std::unique_ptr bus_; + std::unique_ptr safety_; + std::optional mst_id_; + bool connected_ = false; + bool enabled_ = false; + GripperStatus status_flags_ = GripperStatus::kNone; +}; + +} // namespace litegrip diff --git a/include/litegrip/hold_policy.hpp b/include/litegrip/hold_policy.hpp new file mode 100644 index 0000000..d3886d2 --- /dev/null +++ b/include/litegrip/hold_policy.hpp @@ -0,0 +1,64 @@ +// litegrip/hold_policy.hpp — the replaceable hold-position seam (R2). +// +// `init` means exactly one thing in this SDK: enable the motor and leave it +// HOLDING ITS CURRENT POSITION. Every other initialisation action the Python +// original performed (mode switching, fault-retry bookkeeping) is either part +// of that or has been dropped. +// +// The user intends to rewrite this behaviour. So it is isolated behind an +// interface: derive a new HoldPolicy, inject it with +// GripperBus::set_hold_policy(), and nothing else in the stack changes. +// +// Why the seam needs to exist at all (the hazard the default closes): +// in MIT mode the motor keeps executing the last target frame it received +// (tau = kp*(q_target - q) + kd*(dq_target - dq) + tau_ff, all five values from +// that frame). The 0xFC enable command carries no target of its own and does +// not clear the target registers — it only re-engages the control loop. So the +// instant enable takes effect, a motor left over from a previous session would +// resume driving toward THAT session's target (e.g. you Ctrl+C'd mid-close and +// the register still holds q=closed, kp=100). + +#pragma once + +namespace litegrip { + +class GripperBus; +struct GripperConfig; + +/// Strategy for "enable and hold where you are". +class HoldPolicy { + public: + virtual ~HoldPolicy() = default; + + /// Called after the motor's 0xFC enable has been sent. Implementations must + /// leave the motor holding its measured current position. + /// + /// Precondition to respect: the measured position is only meaningful once a + /// status frame has been decoded (MotorState::has_data()); holding at the + /// 0.0 default with a real gain would drive the gripper to a bogus target. + virtual void init(GripperBus& bus, const GripperConfig& config) = 0; + + /// Re-assert the hold at the current position (used by exit_zero_gravity and + /// by the calibration flows). + virtual void hold(GripperBus& bus, const GripperConfig& config) = 0; + + /// Diagnostic name, for logs. + virtual const char* name() const noexcept = 0; +}; + +/// The ported default: 0xFC enable -> streamed zero-gain cover frames -> wait +/// for a fresh status frame -> hold at the freshly-read position with kp/kd. +/// +/// The zero-gain cover is *streamed* rather than a single frame because one lost +/// frame means the motor keeps running the stale target indefinitely rather +/// than for ~10 ms. While waiting for feedback the loop must keep feeding the +/// motor: an enabled motor that hears nothing for ~900 ms latches the 0xD +/// comm-loss fault — the very fault this wait is trying to detect. +class DefaultHoldPolicy : public HoldPolicy { + public: + void init(GripperBus& bus, const GripperConfig& config) override; + void hold(GripperBus& bus, const GripperConfig& config) override; + const char* name() const noexcept override { return "DefaultHoldPolicy"; } +}; + +} // namespace litegrip diff --git a/include/litegrip/json.hpp b/include/litegrip/json.hpp new file mode 100644 index 0000000..0924bb1 --- /dev/null +++ b/include/litegrip/json.hpp @@ -0,0 +1,101 @@ +// litegrip/json.hpp — minimal JSON value/parser/serializer (zero dependency). +// +// The SDK must stay dependency-free (see PLAN-litegrip-cpp.md D7), and its +// JSON needs are tiny and fixed: read/write the flat calibration object and +// read the flat safety-limits baseline. So instead of pulling in +// nlohmann/json this implements exactly what is needed, and no more. +// +// Supported: null, bool, number (double), string, array, object. +// Not supported (deliberately): UTF-8 escape validation beyond \uXXXX, +// BigInt/precision beyond double, comments, trailing commas. + +#pragma once + +#include +#include +#include +#include +#include + +namespace litegrip::json { + +/// A JSON value. Objects keep insertion order via the ordered key vector. +class Value { + public: + enum class Type { kNull, kBool, kNumber, kString, kArray, kObject }; + + Value() = default; + Value(std::nullptr_t) {} + Value(bool b) : type_(Type::kBool), bool_(b) {} + Value(double n) : type_(Type::kNumber), number_(n) {} + Value(int n) : type_(Type::kNumber), number_(static_cast(n)) {} + Value(std::string s) : type_(Type::kString), string_(std::move(s)) {} + Value(const char* s) : type_(Type::kString), string_(s) {} + + static Value make_array(); + static Value make_object(); + + Type type() const noexcept { return type_; } + bool is_null() const noexcept { return type_ == Type::kNull; } + bool is_bool() const noexcept { return type_ == Type::kBool; } + bool is_number() const noexcept { return type_ == Type::kNumber; } + bool is_string() const noexcept { return type_ == Type::kString; } + bool is_array() const noexcept { return type_ == Type::kArray; } + bool is_object() const noexcept { return type_ == Type::kObject; } + + // ── scalar accessors (never throw; fall back to the given default) ──── + bool as_bool(bool fallback = false) const noexcept; + double as_number(double fallback = 0.0) const noexcept; + std::string as_string(const std::string& fallback = "") const; + + // ── object access ──────────────────────────────────────────────────── + bool contains(const std::string& key) const noexcept; + + /// Member access; returns nullopt when absent or when this is not an object. + const Value* find(const std::string& key) const noexcept; + + /// Read a member with a typed default and an optional range check. These are + /// what the calibration / safety loaders use. + double get_number(const std::string& key, double fallback) const noexcept; + int get_int(const std::string& key, int fallback) const noexcept; + bool get_bool(const std::string& key, bool fallback) const noexcept; + std::string get_string(const std::string& key, const std::string& fallback) const; + + void set(const std::string& key, Value value); + void set(const std::string& key, double value) { set(key, Value(value)); } + void set(const std::string& key, int value) { set(key, Value(value)); } + void set(const std::string& key, bool value) { set(key, Value(value)); } + void set(const std::string& key, const char* value) { set(key, Value(value)); } + + // ── array access ───────────────────────────────────────────────────── + std::size_t size() const noexcept; + void push_back(Value value); + /// Bounds-checked; throws std::out_of_range when out of range. + const Value& operator[](std::size_t index) const; + + /// Serialize. `indent <= 0` produces compact output. + std::string dump(int indent = 2) const; + + // ── parsing ────────────────────────────────────────────────────────── + /// Parse `text`; nullopt when malformed. + static std::optional parse(const std::string& text); + + /// Read + parse a file; nullopt when missing or malformed. + static std::optional parse_file(const std::string& path); + + /// Write `dump(indent)` to `path`, creating parent directories. + /// Returns false on I/O failure. + bool write_file(const std::string& path, int indent = 2) const; + + private: + void dump_to(std::string& out, int indent, int depth) const; + + Type type_ = Type::kNull; + bool bool_ = false; + double number_ = 0.0; + std::string string_; + std::vector array_; + std::vector> object_; +}; + +} // namespace litegrip::json diff --git a/include/litegrip/litegrip.hpp b/include/litegrip/litegrip.hpp new file mode 100644 index 0000000..d010188 --- /dev/null +++ b/include/litegrip/litegrip.hpp @@ -0,0 +1,22 @@ +// litegrip/litegrip.hpp — umbrella header. +// +// Includes the whole public API. Consumers may equally include the individual +// headers; this is a convenience, not a facade requirement. + +#pragma once + +#include "litegrip/bus.hpp" +#include "litegrip/calibration.hpp" +#include "litegrip/can/controller.hpp" +#include "litegrip/can/motor.hpp" +#include "litegrip/can/protocol.hpp" +#include "litegrip/can/transport.hpp" +#include "litegrip/constants.hpp" +#include "litegrip/control_loop.hpp" +#include "litegrip/exceptions.hpp" +#include "litegrip/gripper.hpp" +#include "litegrip/hold_policy.hpp" +#include "litegrip/json.hpp" +#include "litegrip/models.hpp" +#include "litegrip/safety.hpp" +#include "litegrip/version.hpp" diff --git a/include/litegrip/models.hpp b/include/litegrip/models.hpp new file mode 100644 index 0000000..2899939 --- /dev/null +++ b/include/litegrip/models.hpp @@ -0,0 +1,143 @@ +// litegrip/models.hpp — data models (state snapshot, config, info, calibration). + +#pragma once + +#include +#include +#include +#include +#include + +#include "litegrip/constants.hpp" + +namespace litegrip { + +/// A status frame older than this no longer represents the present. +/// +/// DM motors emit status at ~10 Hz while enabled, so 0.5 s is ~5 missed +/// frames; the motor's own CAN-timeout fault trips at ~0.9 s. +inline constexpr double kStaleAfterS = 0.5; + +/// Gripper control mode. Mirrors models.GripperMode. +enum class GripperMode : int { + kMit = 0, + kPosition = 1, + kVelocity = 2, + kForce = 3, +}; + +/// Gripper status flags (bitmask). Mirrors models.GripperStatus (IntFlag). +enum class GripperStatus : std::uint8_t { + kNone = 0x00, + kEnabled = 0x01, + kMoving = 0x02, + kAtTarget = 0x04, + kGrasped = 0x08, + kError = 0x10, +}; + +constexpr GripperStatus operator|(GripperStatus a, GripperStatus b) noexcept { + return static_cast(static_cast(a) | + static_cast(b)); +} +constexpr GripperStatus operator&(GripperStatus a, GripperStatus b) noexcept { + return static_cast(static_cast(a) & + static_cast(b)); +} +constexpr GripperStatus operator~(GripperStatus a) noexcept { + return static_cast(~static_cast(a)); +} +inline GripperStatus& operator|=(GripperStatus& a, GripperStatus b) noexcept { + return a = a | b; +} +inline GripperStatus& operator&=(GripperStatus& a, GripperStatus b) noexcept { + return a = a & b; +} +constexpr bool has_flag(GripperStatus v, GripperStatus f) noexcept { + return (static_cast(v) & static_cast(f)) != 0; +} + +/// Live gripper state snapshot. Mirrors models.GripperState. +struct GripperState { + double position_rad = 0.0; + double velocity_rad_s = 0.0; + double torque_nm = 0.0; + int temperature_mos = 0; + int temperature_coil = 0; + int error_code = 0; + double timestamp = 0.0; // unix seconds (wall clock) + + /// Age of the status frame these values came from; infinity = never received. + /// Check this before trusting position/force/temperature. + double data_age_s = std::numeric_limits::infinity(); + + // Convenience — computed from the raw values with unit conversion. + double position_mm = 0.0; + double force_n = 0.0; + + bool has_data() const noexcept { return !std::isinf(data_age_s); } + bool is_stale() const noexcept { return data_age_s > kStaleAfterS; } + bool is_enabled() const noexcept { return error_code == 1; } + bool is_error() const noexcept { return error_code != 0 && error_code != 1; } + bool is_moving() const noexcept { return std::abs(velocity_rad_s) > 0.01; } + + /// Opening distance in mm (single-side displacement). Multiply by 2 for the + /// total jaw separation. + double aperture_mm() const noexcept { return position_mm; } +}; + +/// Gripper configuration — tune these for your hardware. +/// Mirrors models.GripperConfig. +struct GripperConfig { + // CAN + std::string can_channel = DefaultParams::kCanChannel; + int can_id = GripperParams::kCanId; + std::optional mst_id = std::nullopt; // nullopt = auto-detect + bool canfd_mode = DefaultParams::kCanfdMode; + + // Control gains (MIT mode) + double kp = GripperParams::kDefaultKp; + double kd = GripperParams::kDefaultKd; + + // Position limits (rad), updated by calibrate(). + // closed (0 mm) is numerically *larger* than open (full stroke). + double pos_closed_rad = GripperParams::kPosClosedRad; + double pos_open_rad = GripperParams::kPosOpenRad; + + // Mechanical stroke (mm) — set to match your gripper's physical travel. + double max_stroke_mm = 120.0; + + // Unit conversion — update after calibration. + double rad_to_mm = UnitConversion::kRadToMm; + double nm_to_n = UnitConversion::kNmToN; + + // Grasp detection + double grasp_torque_threshold = 0.5; // N.m +}; + +/// Static device information. Mirrors models.GripperInfo. +struct GripperInfo { + std::string model = "LiteGrip"; + std::string motor_type = "DM4310"; + int can_id = GripperParams::kCanId; + int mst_id = GripperParams::kMstId; + std::string firmware_version; + std::string serial_number; +}; + +/// Result of a calibration run. Mirrors models.CalibrationData. +struct CalibrationData { + double zero_position = 0.0; // closed limit (rad) + double max_position = 1.14; // open limit (rad) + double travel_range = 1.14; // |max - zero| (rad) + double rad_to_mm = UnitConversion::kRadToMm; + std::string motor_type = "DM4310"; + int can_id = GripperParams::kCanId; + int mst_id = GripperParams::kMstId; + std::string calibration_time; + + /// Full stroke in mm. + double travel_mm() const noexcept { return travel_range * rad_to_mm; } +}; + +} // namespace litegrip diff --git a/include/litegrip/safety.hpp b/include/litegrip/safety.hpp new file mode 100644 index 0000000..a3b1cd7 --- /dev/null +++ b/include/litegrip/safety.hpp @@ -0,0 +1,371 @@ +// litegrip/safety.hpp — the safety core gate (red lines / budgets / watchdog). +// +// Ported from the safety-aware SDK variant's safety_limits.py (core only — +// exclude the contact/force/stall/no-load physics models, see +// PLAN-litegrip-cpp.md D5), plus the ROS-side gate's two hard ceilings +// (TORQUE_LIMIT_CEILING_NM / MAX_COMMAND_VELOCITY_CEILING_RAD_S) so that the +// whole stack keeps a single source of truth now that the ROS-side Python is +// going away (D2/D4). +// +// The Python original's long-form Chinese rationale is NOT copied here — it +// lives in safety_limits.py and in the plan. What is preserved is the set of +// invariants, because they are the safety argument itself: +// +// 1. TIGHTEN ONLY. Limits may only be a sub-interval of the shipped +// baseline; assert_no_wider_than() rejects anything wider. There is no +// "allow_widen" path and no switch that turns the red lines off. +// 2. REJECT, DO NOT CLAMP. An out-of-range command is refused with a +// diagnosable reason; it is never silently rewritten and sent. +// 3. FAIL-CLOSED. Limits unavailable => reject everything. A value that +// cannot be bounded => do not move in that direction. Never degrade +// "cannot get the limits" into "no validation". +// 4. STRICT NUMERIC BOUNDARY. Every field that takes part in a comparison is +// a plain built-in number: NaN / +-inf / bool / non-numbers are rejected +// before any comparison. (The Python original's reason: a float subclass +// can overload the comparison operators used by every criterion below.) +// 5. The red lines are a CLOSED interval: being exactly on an endpoint is +// not a violation. +// +// R5: in the Python original the latch / mode stack / active limits are +// process-global. Here they live in SafetyGuard's instance, so one process can +// drive several grippers without cross-talk. Semantics are unchanged. + +#pragma once + +#include +#include +#include +#include +#include + +namespace litegrip { + +// ── package baseline constants ──────────────────────────────────────────── + +/// Closed-side (numerically larger) red line, rad. +inline constexpr double kPackageRedMax = -0.01; +/// Open-side (numerically smaller) red line, rad. +inline constexpr double kPackageRedMin = -1.24; +inline constexpr double kPackageMechMin = -1.272793; +inline constexpr double kPackageMechMax = 0.051308; + +/// Rated torque of the DM-J4310-2EC (output side), N.m — the torque ceiling. +/// Evidence and the "output side" determination live with the source constant +/// in safety_limits.py; deliberately not duplicated (a second copy drifts). +inline constexpr double kPackageTauMaxNm = 3.5; + +/// 16-bit MIT position field range, rad (1 LSB = 25/65535 ~ 3.814755e-4). +inline constexpr double kProtocolQMaxRad = 12.5; + +/// Placeholder `q` for a zero-torque frame when no usable feedback exists. +/// The field is inert when kp == kd == 0, but must still not be NaN / out of +/// protocol range / a fake 0 read from nothing. +inline constexpr double kZeroTorqueQFallback = -0.6; + +/// ROS-side torque hard ceiling, N.m. Read-only: limits may only go below it. +inline constexpr double kTorqueLimitCeilingNm = 3.5; + +/// Command-trajectory velocity hard ceiling, rad/s. Read-only. +/// +/// ★★ CHANGED 0.436 -> 1.5 at the operator's request. Read this before trusting +/// either number again, because the change is NOT a re-derivation from new +/// evidence — it is a target, and the model below was adjusted to admit it. +/// +/// The ceiling is meant to be DERIVED from the stopping-distance model: the +/// fastest the target may advance while the stopping distance still lands inside +/// the margin between the red line and the mechanical observed bound. At the +/// original baseline (a = 5.0 rad/s^2, t_comm = 0.02 s) that derivation gave +/// 0.436591 rad/s on the open side and 0.657020 on the closed side, and the +/// smaller one, floored to 3 decimals, was 0.436. +/// +/// 1.5 rad/s is NOT reachable from that baseline at any a: the model saturates +/// at (side margin - reserve) / t_comm, which for the open side +/// (0.032793 - 0.005) / 0.02 = 1.3897 rad/s. So honouring the request forced +/// t_comm_s down as well as a_max_rad_s2 up — see TemporaryParams below, where +/// both are marked as the assumptions they are. +/// +/// NOTE this is NOT the worst-case bound on *measured* velocity (that is a +/// separate deployment parameter); mixing the two turns "I assert the axis +/// cannot turn that fast" into "I command it to turn that fast". +inline constexpr double kMaxCommandVelocityCeilingRadS = 1.5; + +/// Torque sign convention was verified on hardware (tau > 0 => closing). +inline constexpr bool kTorqueDirectionVerified = true; +/// Force/contact calibration was NOT verified — force feed-forward is refused. +inline constexpr bool kForceCalibrationVerified = false; + +/// Control and protection ceilings. +/// +/// WARNING: every default here is a provisional experimental value with no +/// real-hardware measurement behind it. Reach them through the tighten-only +/// path; do not raise them to "make something work". +/// +/// ★★ The two kinematic values below were RAISED to admit the 1.5 rad/s ceiling +/// requested by the operator. That is the loosening direction, and the +/// project's own rule is that a loosening must be backed by evidence — these +/// are NOT backed by a measurement. They are recorded here as the assumptions +/// they are, so that whoever measures the real values (see below) replaces +/// them rather than inheriting them as if they were facts. +/// +/// How to replace them with measurements: +/// a_max_rad_s2 drive the gripper at a known speed, cut the command, record +/// the stopping profile from CAN timestamps, and fit the +/// deceleration. (Currently assumed 88.0.) +/// t_comm_s time one command-to-feedback round trip, e.g. with +/// `candump -ta can0`. (Currently assumed 0.010.) +/// With measured values, re-derive the ceiling as min(open side, closed side) of +/// v_allow() at each side's full margin, floored to 3 decimals, and update +/// kMaxCommandVelocityCeilingRadS to match. +struct TemporaryParams { + // Kinematics, used by the deceleration zone. + // ⚠ Assumption raised to admit the 1.5 rad/s ceiling — NOT measured. + double a_max_rad_s2 = 88.0; + // ⚠ Assumption lowered (smaller = more permissive) for the same reason — the + // ceiling is asymptotic in this quantity, so 1.5 is unreachable at 0.02. + double t_comm_s = 0.010; + double safety_reserve_rad = 0.005; + + // Command ceilings. + double kp_max = 200.0; // never "push harder" against a red line + double kd_max = 5.0; // protocol hard limit is 5.0 + double tau_max_nm = kPackageTauMaxNm; + double max_motion_duration_s = 10.0; + + // Recovery mode: only outward->inward, slow and weak. + double recovery_kp_max = 50.0; + double recovery_dq_max = 0.5; + double recovery_tau_max = 0.2; + + /// All fields finite and strictly positive; throws SafetyConfigError. + void validate() const; +}; + +/// Allowed frame types and watchdog behaviour. +/// +/// No mode can widen the red lines — a mode only decides which frame class is +/// allowed and whether an out-of-range *feedback* reading latches. +enum class FrameMode { + kNormal, // motion frames only, target AND measured inside the red lines + kZeroGravity, // zero-torque frames only; feedback past a red line only warns + kMaintenance, // low-speed low-torque motion frames (calibration at a stop) + kRecovery, // outward->inward only; feedback past a red line does not latch +}; + +/// One set of safety limits. +/// +/// Default values are the 2026-09-11 whole-gripper zero-gravity hand-push +/// measurement of the reference unit — NOT of every unit. See the plan's R3: +/// they must be re-derived per gripper before real-hardware use. +struct SafetyLimits { + // Mechanical observed range (hand-push observation, not hard stops). + double mech_min_rad = kPackageMechMin; + double mech_max_rad = kPackageMechMax; + // Software red lines: no frame that can produce motion may cross them. + double red_min_rad = kPackageRedMin; // open side + double red_max_rad = kPackageRedMax; // closed side + /// Red-line master switch. Must always be true; validate() rejects false, and + /// no legal path produces a disabled instance. + bool enabled = true; + + TemporaryParams params{}; + + /// Self-consistency: mech_min < red_min < red_max < mech_max, all finite. + /// Throws SafetyConfigError. + void validate() const; + + /// Assert this set is not wider than `baseline` (4 position fields + enabled + /// + every TemporaryParams field, direction-aware: t_comm_s and + /// safety_reserve_rad are "smaller is wider"). + /// Throws SafetyConfigError. + void assert_no_wider_than(const SafetyLimits& baseline) const; + + /// A fresh copy with every numeric field pushed through the strict numeric + /// boundary. The canonical way to take ownership of externally-supplied + /// limits. Throws SafetyConfigError. + SafetyLimits sanitized_copy(const std::string& source) const; + + /// Quantize `q` to the 16-bit MIT field and step inwards until the *decoded* + /// value lies within [red_min, red_max]. Returns the decoded value. + /// Throws LimitViolation if 4 steps are not enough (should not happen). + static double quantize_toward_interior(double q, double red_min, + double red_max); + + /// Maximum speed allowed at measured position `q_act` heading in + /// `direction` (>0 closing, <0 opening), so that the stopping distance + /// v^2/(2a) + v*t_comm + reserve <= min(side margin, distance to red line) + /// stays satisfied. Returns 0.0 when no margin remains, and 0.0 (fail-closed) + /// when the closed form would overflow. + double v_allow(double q_act, double direction) const; + + bool contains_red(double q) const; + bool contains_mech(double q) const; + + /// Working range in mm implied by the red lines: (mm_min, mm_max). + /// Display/conversion-check only — it does not change the red lines. + std::pair mm_range(double pos_closed_rad, + double rad_to_mm) const; +}; + +// ── strict numeric boundary ─────────────────────────────────────────────── + +/// Push a value through the strict numeric boundary, returning a plain double. +/// Rejects bool / non-numbers / NaN / +-inf with LimitViolation. +double normalize_scalar(double value, const std::string& name, + const std::string& source); + +// ── mode stack / latch / watchdog + the guards (instance scoped, R5) ────── + +/// Holds the safety state and applies the guards to one gripper. +/// +/// The latch, the mode stack and the watchdog are per-instance (R5) — the +/// Python original kept them as process globals. +class SafetyGuard { + public: + /// `limits` is copied through sanitized_copy + validate; a wider-than-baseline + /// set throws SafetyConfigError (fail-closed: refuse to construct). + explicit SafetyGuard(const SafetyLimits& limits); + + const SafetyLimits& limits() const noexcept { return limits_; } + + // ── modes ───────────────────────────────────────────────────────────── + FrameMode mode() const noexcept; + + /// Enter a persistent mode (survives across calls) until pop_mode(). + /// FrameMode::kNormal cannot be pushed — it only exists as the stack bottom. + void push_mode(FrameMode mode, const std::string& reason = ""); + FrameMode pop_mode(); + + /// Scope-type mode (the C++ replacement for Python's `with ...:`). Nestable; + /// returns to the previous mode on destruction. + class ModeScope { + public: + ModeScope(SafetyGuard& owner, FrameMode mode, const std::string& reason); + ~ModeScope(); + ModeScope(const ModeScope&) = delete; + ModeScope& operator=(const ModeScope&) = delete; + ModeScope(ModeScope&&) = default; + + private: + SafetyGuard* owner_; + }; + + ModeScope zero_gravity_scope(const std::string& reason = "zero-gravity hand-push"); + ModeScope maintenance_scope(const std::string& reason = "maintenance/calibration"); + ModeScope recovery_scope(const std::string& reason = "recovery from outside red line"); + + // ── latch / watchdog ────────────────────────────────────────────────── + bool is_fault_latched() const noexcept { return fault_latched_; } + const std::string& latch_reason() const noexcept { return latch_reason_; } + + /// Latch the fault state; only the first reason is kept. + void latch_fault(const std::string& reason); + + /// Manually clear the latch. Never automatic. Leaves the watchdog disarmed — + /// it re-arms once the measured position is back inside the red lines, or the + /// recovery motion would be blocked by itself on its first frame. + void clear_safety_latch(); + + bool is_watchdog_armed() const noexcept { return armed_; } + + /// Disarm without clearing the latch (for "stuck outside the red lines, drive + /// back in"); auto re-arms once back inside. + void disarm_watchdog(const std::string& reason); + + // ── guards ──────────────────────────────────────────────────────────── + + /// Normal-motion precondition: the MEASURED position must be inside the red + /// lines (strict; endpoints pass). nullopt means "never received feedback". + /// Returns the normalized value, which the caller must use from here on. + /// Throws SafetyFault (no feedback) / LimitViolation. + double require_act_within_red(std::optional q_act, + const std::string& source); + + /// Validate a frame that can produce motion; returns the safe (quantized) + /// target. Throws, and the frame is NOT sent, on any failure. + double guard_motion_frame(double q_target, double kp, double kd, + double dq_target, double tau_feedforward, + std::optional q_act, + std::optional dq_act, + std::optional tau_act, + const std::string& source = "motion"); + + /// Validate a recovery frame: only outward->inward, within the recovery + /// ceilings. This is the ONLY path that may move while the measured position + /// is outside the red lines. Returns the normalized target. + double guard_recovery_frame(double q_target, double kp, double kd, + double dq_target, double tau_feedforward, + std::optional q_act, + std::optional dq_act, + const std::string& source = "recovery"); + + /// Validate a zero-torque frame: kp / kd / dq / tau must all be exactly 0. + /// This is what makes "zero-torque frames bypass the red lines" acceptable. + void guard_zero_torque_frame(double kp, double kd, double dq, double tau, + const std::string& source = "zero_torque"); + + /// Validate a feedback reading; latches + throws on violation. + /// ZERO_GRAVITY / RECOVERY only warn for a red-line crossing; exceeding the + /// mechanical observed range latches in every mode. + void guard_feedback_position(double q, const std::string& source = "feedback"); + + /// Placeholder `q` for a zero-torque frame (see kZeroTorqueQFallback). + static double zero_torque_q(std::optional q_act, bool has_feedback); + + private: + void check_not_latched(const std::string& source) const; + void arm_watchdog() noexcept { armed_ = true; } + + SafetyLimits limits_; + bool fault_latched_ = false; + std::string latch_reason_; + bool armed_ = true; // disarmed right after a latch clear / mode entry + std::vector mode_stack_{FrameMode::kNormal}; +}; + +// ── configuration loading ───────────────────────────────────────────────── + +/// Delivered safety-baseline versions -> file names (relative to the SDK's +/// data directory). The version is selected explicitly, never "whatever file +/// happens to be there" — otherwise a rollback becomes an unauditable in-place +/// edit. Both files share the same red lines; only hard_torque_limit_nm differs. +struct SafetyBaselineVersion { + const char* version; + const char* file_name; +}; + +/// Known baseline versions. The single place to change the default is +/// kDefaultSafetyBaseline. +extern const SafetyBaselineVersion kSafetyBaselineVersions[]; +extern const std::size_t kSafetyBaselineVersionsCount; +inline constexpr const char* kDefaultSafetyBaseline = "3.5"; + +/// Absolute path of a version's file. Does NOT check existence. +/// Throws SafetyConfigError for an unknown version. +std::string safety_baseline_path(const std::string& version); + +/// Load red-line config from JSON. The loaded config may only TIGHTEN the +/// packaged baseline; anything wider is rejected. Returns nullopt when no file +/// is found — that is part of the contract, and the caller must handle it +/// explicitly rather than being handed some shared fallback object. +/// Throws SafetyConfigError on malformed content or an attempt to widen. +std::optional load_safety_limits( + const std::optional& path = std::nullopt); + +/// Load a versioned baseline. FAIL-CLOSED: a missing/invalid/widened file +/// throws rather than silently falling back, so "the config was lost" cannot +/// look identical to "the config is fine". +/// Throws SafetyConfigError. +SafetyLimits load_safety_baseline( + const std::optional& version = std::nullopt); + +/// The canonical baseline, freshly constructed per call (never a shared object +/// that a caller could mutate in place). +SafetyLimits canonical_baseline(); + +/// Validate / sanitize / compare against the canonical baseline, returning the +/// independent sanitized copy. The single gate every loaded or injected limit +/// set goes through. Throws SafetyConfigError. +SafetyLimits checked_limits(const SafetyLimits& candidate, + const std::string& source); + +} // namespace litegrip diff --git a/include/litegrip/version.hpp b/include/litegrip/version.hpp new file mode 100644 index 0000000..e4ddfdd --- /dev/null +++ b/include/litegrip/version.hpp @@ -0,0 +1,20 @@ +// litegrip/version.hpp — build-time version of the litegrip_cpp SDK. + +#pragma once + +// Must stay in sync with CMakeLists.txt's project(... VERSION ...). +#define LITEGRIP_CPP_VERSION_MAJOR 0 +#define LITEGRIP_CPP_VERSION_MINOR 1 +#define LITEGRIP_CPP_VERSION_PATCH 0 +#define LITEGRIP_CPP_VERSION_STRING "0.1.0" + +namespace litegrip { + +/// Version as a "MAJOR.MINOR.PATCH" string. +const char* version() noexcept; + +int version_major() noexcept; +int version_minor() noexcept; +int version_patch() noexcept; + +} // namespace litegrip diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..e75b366 --- /dev/null +++ b/package.xml @@ -0,0 +1,26 @@ + + + + litegrip_cpp + 0.1.0 + + ROS-agnostic C++ SDK for the LiteGrip adaptive two-finger gripper. + + This is the bottom layer of the litegrip stack (sdk → ros2_control → + moveit_config). It speaks SocketCAN and the Damiao DM4310 MIT protocol + directly and depends on nothing but the C++17 standard library and the + Linux SocketCAN headers — deliberately no ROS, no ament, no third-party + libraries, so that non-ROS implementations can reuse it as-is. + + Declared as a plain-cmake package (build_type cmake) purely so a colcon + workspace can build it alongside the ROS packages; the library itself is + built and installed like any ordinary CMake project. + + TODO + MIT + + cmake + + cmake + + diff --git a/src/bus.cpp b/src/bus.cpp new file mode 100644 index 0000000..7ac4442 --- /dev/null +++ b/src/bus.cpp @@ -0,0 +1,356 @@ +// bus.cpp — GripperBus, ported from +// litegrip_driver/litegrip/protocols/can_bus.py (LiteGripCAN). + +#include "litegrip/bus.hpp" + +#include +#include +#include +#include + +#include "litegrip/exceptions.hpp" + +namespace litegrip { +namespace { + +double monotonic_now() noexcept { + return std::chrono::duration( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +void sleep_s(double seconds) { + if (seconds > 0.0) { + std::this_thread::sleep_for(std::chrono::duration(seconds)); + } +} + +} // namespace + +GripperBus::GripperBus(GripperConfig config) + : config_(std::move(config)), + hold_policy_(std::make_unique()) {} + +GripperBus::~GripperBus() { disconnect(); } + +bool GripperBus::connect() { + if (connected_) { + return true; + } + + const can::CanMode mode = + config_.canfd_mode ? can::CanMode::kCanFd : can::CanMode::kCan; + + transport_ = std::make_unique(config_.can_channel, mode); + transport_->open(); // throws ConnectError when the interface is unusable + + controller_ = std::make_unique(*transport_); + connected_ = true; + + register_gripper(config_.can_id, config_.mst_id, GripperParams::kMotorType); + return true; +} + +void GripperBus::disconnect() { + if (!connected_) { + return; + } + + // Disable on the way out. The condition also covers a bare enable() that was + // never preceded by init(): the Python original would have left that motor + // enabled (holding position with torque) after disconnect. + if (motor_ != nullptr && (initialized_ || motor_->is_enabled())) { + disable(); + } + + if (controller_) { + controller_->close(); + } else if (transport_) { + transport_->close(); + } + + controller_.reset(); + transport_.reset(); + motor_ = nullptr; + connected_ = false; + initialized_ = false; +} + +int GripperBus::register_gripper(int can_id, std::optional mst_id, + can::MotorType motor_type) { + if (!connected_ || controller_ == nullptr) { + throw CommError("CAN not connected"); + } + + try { + motor_ = &controller_->add_motor(can_id, mst_id, motor_type, + can::ControlMode::kMit); + } catch (const LiteGripError& error) { + throw CommError(std::string("could not register the gripper motor: ") + + error.what()); + } + + if (!config_.mst_id.has_value()) { + config_.mst_id = motor_->mst_id(); + } + return motor_->mst_id(); +} + +void GripperBus::set_hold_policy(std::unique_ptr policy) { + if (policy != nullptr) { + hold_policy_ = std::move(policy); + } +} + +std::optional GripperBus::enable_and_hold( + const GripperConfig& effective_config) { + if (controller_ == nullptr || motor_ == nullptr) { + return std::nullopt; + } + + controller_->enable(*motor_); // 0xFC + + try { + // The policy covers the window between 0xFC taking effect and the first + // hold frame, waits for a fresh status frame, then holds the measured + // position. + hold_policy_->init(*this, effective_config); + } catch (const HardwareError&) { + return std::nullopt; // no feedback + } + return motor_->error(); +} + +bool GripperBus::enable(std::optional kp, std::optional kd) { + if (controller_ == nullptr || motor_ == nullptr) { + return false; + } + GripperConfig effective = config_; + if (kp.has_value()) { + effective.kp = *kp; + } + if (kd.has_value()) { + effective.kd = *kd; + } + + // nullopt (no feedback) is not 0 and not 1, so a silent motor is reported as + // a failed enable rather than a successful one. + const std::optional error = enable_and_hold(effective); + return error.has_value() && (*error == 0 || *error == 1); +} + +bool GripperBus::init(std::optional kp, std::optional kd) { + if (!connected_ || controller_ == nullptr || motor_ == nullptr) { + throw NotInitializedError("not connected or no gripper registered"); + } + + GripperConfig effective = config_; + if (kp.has_value()) { + effective.kp = *kp; + } + if (kd.has_value()) { + effective.kd = *kd; + } + + int last_error = -1; + + for (int attempt = 0; attempt < GripperParams::kFaultClearRetries; ++attempt) { + // 1. Disable, so the mode switch and the enable start from a known state. + controller_->disable(*motor_); + sleep_s(0.01); + + // 2. Switch to MIT. A failed verification is not fatal here: the motor may + // already be in MIT, and the enable/hold below is what actually proves + // the link. + if (!controller_->switch_control_mode(*motor_, can::ControlModeCode::kMit)) { + std::fprintf(stderr, + "[litegrip] MIT mode switch could not be verified; " + "continuing anyway\n"); + } + sleep_s(0.05); + + // 3 + 4. Enable, cover the window, verify feedback, hold. The motor is left + // holding the position it actually has, so it cannot jump towards + // whatever target the previous session left behind. + const std::optional error = enable_and_hold(effective); + if (!error.has_value()) { + last_error = -2; // timeout / no feedback + } else if (*error == 0 || *error == 1) { + initialized_ = true; + return true; + } else { + last_error = *error; + } + + // Retry: clear the fault before the next attempt. No bare enable() here — + // the next iteration disables immediately, and an uncovered enable is + // exactly the window this method avoids. + if (attempt < GripperParams::kFaultClearRetries - 1) { + controller_->clear_fault(*motor_); + sleep_s(0.01); + } + } + + initialized_ = false; + + if (last_error == 0x9) { + throw HardwareError("motor undervoltage (UV_FAULT) — check the gripper's supply", + 0x9); + } + if (last_error == -2) { + throw HardwareError( + "no motor feedback within the init timeout — check power and CAN wiring"); + } + if (last_error != 0 && last_error != 1 && last_error != -3) { + throw HardwareError("uncorrectable motor fault", last_error); + } + throw HardwareError("initialisation failed: all retries exhausted"); +} + +bool GripperBus::disable() { + if (controller_ == nullptr || motor_ == nullptr) { + return false; + } + controller_->disable(*motor_); + initialized_ = false; + return true; +} + +bool GripperBus::clear_fault(std::optional kp, std::optional kd) { + if (controller_ == nullptr || motor_ == nullptr) { + return false; + } + + GripperConfig effective = config_; + if (kp.has_value()) { + effective.kp = *kp; + } + if (kd.has_value()) { + effective.kd = *kd; + } + + for (int attempt = 0; attempt < GripperParams::kFaultClearRetries; ++attempt) { + // Strategy 1: direct clear + enable (proven for UV faults, where the motor + // is already effectively disabled). + controller_->clear_fault(*motor_); + sleep_s(0.005); + const std::optional first = enable_and_hold(effective); + if (first.has_value() && (*first == 0 || *first == 1)) { + return true; + } + + // Strategy 2: full disable -> clear -> enable. + controller_->disable(*motor_); + sleep_s(0.01); + controller_->clear_fault(*motor_); + sleep_s(0.01); + const std::optional second = enable_and_hold(effective); + if (second.has_value() && (*second == 0 || *second == 1)) { + return true; + } + } + + return false; +} + +bool GripperBus::control_mit(double q_target, double kp, double kd, + double dq_target, double tau_feedforward) { + if (controller_ == nullptr || motor_ == nullptr) { + return false; + } + controller_->control_mit(*motor_, kp, kd, q_target, dq_target, + tau_feedforward); + return true; +} + +bool GripperBus::control_mit_stream(double q_target, double kp, double kd, + double duration_s, double dq_target, + double tau_feedforward, double interval_s) { + if (controller_ == nullptr || motor_ == nullptr) { + return false; + } + + const double deadline = monotonic_now() + duration_s; + while (monotonic_now() < deadline) { + controller_->control_mit(*motor_, kp, kd, q_target, dq_target, + tau_feedforward); + controller_->poll(0.0); + sleep_s(interval_s); + } + return true; +} + +bool GripperBus::poll(double timeout_s) { + if (controller_ == nullptr) { + return false; + } + return controller_->poll(timeout_s) != nullptr; +} + +bool GripperBus::update_state(double timeout_s) { + if (controller_ == nullptr || motor_ == nullptr) { + return false; + } + return controller_->poll_until(*motor_, timeout_s); +} + +bool GripperBus::refresh_status(double timeout_s) { + if (controller_ == nullptr || motor_ == nullptr) { + return false; + } + + // A disabled motor does not stream status frames on its own, so the refresh + // command (which is answered regardless of enable state) is how position is + // read before the first enable. Sends no motion command. + const std::uint64_t prev_rx = motor_->rx_count(); + controller_->refresh_status(*motor_); + + const double deadline = monotonic_now() + timeout_s; + while (monotonic_now() < deadline) { + controller_->poll(0.01); + if (motor_->rx_count() > prev_rx) { + return true; + } + sleep_s(0.005); + } + return false; +} + +double GripperBus::get_position() const { + return motor_ != nullptr ? motor_->position() : 0.0; +} + +double GripperBus::get_velocity() const { + return motor_ != nullptr ? motor_->velocity() : 0.0; +} + +double GripperBus::get_torque() const { + return motor_ != nullptr ? motor_->torque() : 0.0; +} + +int GripperBus::get_error() const { return motor_ != nullptr ? motor_->error() : -1; } + +int GripperBus::get_temperature_mos() const { + return motor_ != nullptr ? motor_->t_mos() : 0; +} + +int GripperBus::get_temperature_coil() const { + return motor_ != nullptr ? motor_->t_coil() : 0; +} + +double GripperBus::read_param(int rid, double timeout_s) { + if (controller_ == nullptr || motor_ == nullptr) { + throw NotInitializedError("not connected or no gripper registered"); + } + return controller_->read_param(*motor_, static_cast(rid), + timeout_s); +} + +void GripperBus::write_param(int rid, double value) { + if (controller_ == nullptr || motor_ == nullptr) { + throw NotInitializedError("not connected or no gripper registered"); + } + controller_->write_param(*motor_, static_cast(rid), value); +} + +} // namespace litegrip diff --git a/src/calibration.cpp b/src/calibration.cpp new file mode 100644 index 0000000..b4bef53 --- /dev/null +++ b/src/calibration.cpp @@ -0,0 +1,148 @@ +// calibration.cpp — calibration file load/save and path resolution. +// +// The JSON keys and units are identical to the Python SDK's, so the two can be +// diffed during parallel validation. + +#include "litegrip/calibration.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "litegrip/exceptions.hpp" +#include "litegrip/json.hpp" + +namespace litegrip { +namespace { + +bool file_exists(const std::string& path) { + struct stat info {}; + return ::stat(path.c_str(), &info) == 0 && S_ISREG(info.st_mode); +} + +const char* env_or_null(const char* name) { + const char* value = std::getenv(name); + return (value != nullptr && value[0] != '\0') ? value : nullptr; +} + +} // namespace + +std::string default_calibration_path() { + if (const char* override_path = env_or_null(kCalibEnvVar)) { + return override_path; + } + const char* home = env_or_null("HOME"); + if (home == nullptr) { + return ".litegrip/litegrip_calibration.json"; + } + return std::string(home) + "/.litegrip/litegrip_calibration.json"; +} + +std::string factory_calibration_path() { + // No machine-dependent path is baked into the source: the installed location + // is discovered at run time, with an explicit override for deployments that + // move the data elsewhere. + if (const char* explicit_path = env_or_null("LITEGRIP_FACTORY_CALIB")) { + return explicit_path; + } + + std::vector candidates; + if (const char* data_dir = env_or_null("LITEGRIP_DATA_DIR")) { + candidates.push_back(std::string(data_dir) + "/calibration/factory_calibration.json"); + } + candidates.emplace_back("calibration/factory_calibration.json"); + candidates.emplace_back("../calibration/factory_calibration.json"); + candidates.emplace_back("factory_calibration.json"); + + for (const std::string& candidate : candidates) { + if (file_exists(candidate)) { + return candidate; + } + } + // Nothing found: return the primary candidate so the caller's error message + // names a path someone can actually act on. + return candidates.front(); +} + +std::optional read_calibration_file(const std::string& path) { + const auto document = json::Value::parse_file(path); + if (!document.has_value() || !document->is_object()) { + return std::nullopt; + } + + // The three fields the calibration is meaningless without. A file missing any + // of them is treated as malformed (the Python original would raise KeyError). + for (const char* required : {"zero_position_rad", "max_position_rad", + "rad_to_mm"}) { + if (!document->contains(required)) { + return std::nullopt; + } + } + + CalibrationFile out; + out.has_closed = true; + out.zero_position_rad = document->get_number("zero_position_rad", 0.0); + out.has_open = true; + out.max_position_rad = document->get_number("max_position_rad", 0.0); + out.has_rad_to_mm = true; + out.rad_to_mm = document->get_number("rad_to_mm", 0.0); + + // Optional fields, present from newer calibration files onwards. + if (document->contains("can_id")) { + out.can_id = document->get_int("can_id", 0); + } + if (document->contains("mst_id")) { + out.mst_id = document->get_int("mst_id", 0); + } + if (document->contains("channel")) { + out.channel = document->get_string("channel", ""); + } + if (document->contains("canfd_mode")) { + out.canfd_mode = document->get_bool("canfd_mode", false); + } + if (document->contains("kp")) { + out.kp = document->get_number("kp", 0.0); + } + if (document->contains("kd")) { + out.kd = document->get_number("kd", 0.0); + } + if (document->contains("grasp_torque_threshold")) { + out.grasp_torque_threshold = + document->get_number("grasp_torque_threshold", 0.0); + } + if (document->contains("motor_type")) { + out.motor_type = document->get_string("motor_type", ""); + } + return out; +} + +void write_calibration_file(const std::string& path, const GripperConfig& config, + int can_id, int mst_id, + const std::string& motor_type) { + // Key order matches the Python SDK's save_calibration(), so files produced by + // either implementation diff cleanly. + json::Value document = json::Value::make_object(); + document.set("channel", config.can_channel); + document.set("can_id", can_id); + document.set("mst_id", mst_id); + document.set("canfd_mode", config.canfd_mode); + document.set("zero_position_rad", config.pos_closed_rad); + document.set("max_position_rad", config.pos_open_rad); + document.set("travel_range_rad", + std::abs(config.pos_open_rad - config.pos_closed_rad)); + document.set("rad_to_mm", config.rad_to_mm); + document.set("motor_type", motor_type); + document.set("kp", config.kp); + document.set("kd", config.kd); + document.set("grasp_torque_threshold", config.grasp_torque_threshold); + + if (!document.write_file(path)) { + throw CommError("cannot write calibration file: " + path); + } +} + +} // namespace litegrip diff --git a/src/can/controller.cpp b/src/can/controller.cpp new file mode 100644 index 0000000..9da5fd4 --- /dev/null +++ b/src/can/controller.cpp @@ -0,0 +1,315 @@ +// controller.cpp — MotorController, ported from +// litegrip_driver/litegrip/can/controller.py. + +#include "litegrip/can/controller.hpp" + +#include +#include +#include + +#include "litegrip/exceptions.hpp" + +namespace litegrip::can { +namespace { + +double monotonic_now() noexcept { + return std::chrono::duration( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +void sleep_s(double seconds) { + if (seconds > 0.0) { + std::this_thread::sleep_for(std::chrono::duration(seconds)); + } +} + +} // namespace + +MotorController::~MotorController() { close(); } + +MotorState& MotorController::add_motor(int can_id, std::optional mst_id, + MotorType motor_type, + ControlMode control_mode) { + if (!mst_id.has_value()) { + mst_id = detect_mst_id(can_id); + } + + MotorParams params; + params.motor_type = motor_type; + params.can_id = can_id; + params.mst_id = *mst_id; + params.control_mode = control_mode; + + auto owned = std::make_unique(params); + const int key = *mst_id; + MotorState* raw = owned.get(); + motors_[key] = std::move(owned); + + // Install the hardware RX filter for every registered mst_id. Done AFTER + // mst_id detection (detection needs unfiltered RX). On a shared bus this + // stops foreign frames from flooding the RX buffer and starving this motor's + // status frames — the root cause of "state read-back freezes while the motor + // keeps moving". Param responses arrive on the same mst_id, so read_param + // still works. + std::vector ids; + ids.reserve(motors_.size()); + for (const auto& entry : motors_) { + ids.push_back(entry.first); + } + transport_.set_id_filter(ids); + + return *raw; +} + +void MotorController::remove_motor(const MotorState& motor) { + motors_.erase(motor.mst_id()); + std::vector ids; + ids.reserve(motors_.size()); + for (const auto& entry : motors_) { + ids.push_back(entry.first); + } + transport_.set_id_filter(ids); +} + +MotorState* MotorController::get_motor(int mst_id) { + const auto it = motors_.find(mst_id); + return it == motors_.end() ? nullptr : it->second.get(); +} + +MotorState* MotorController::get_motor_by_can_id(int can_id) { + for (auto& entry : motors_) { + if (entry.second->can_id() == can_id) { + return entry.second.get(); + } + } + return nullptr; +} + +void MotorController::send_command(MotorState& motor, std::uint8_t cmd, + int count, double interval_s) { + const int can_id = motor.can_id() + motor.mode_offset(); + const auto data = pack_command_frame(cmd); + transport_.send_multi(can_id, data.data(), data.size(), count, interval_s); +} + +void MotorController::enable(MotorState& motor) { + send_command(motor, kCmdEnable); +} + +void MotorController::disable(MotorState& motor) { + send_command(motor, kCmdDisable); +} + +void MotorController::clear_fault(MotorState& motor) { + send_command(motor, kCmdClearFault); +} + +void MotorController::set_zero(MotorState& motor) { + send_command(motor, kCmdSetZero); +} + +void MotorController::refresh_status(MotorState& motor) { + const auto data = pack_refresh_frame(motor.can_id()); + transport_.send(kBroadcastId, data.data(), data.size()); +} + +bool MotorController::switch_control_mode(MotorState& motor, + ControlModeCode mode_code) { + write_param(motor, DmReg::kCtrlMode, static_cast(mode_code)); + sleep_s(0.02); + + try { + const int actual = static_cast(read_param(motor, DmReg::kCtrlMode)); + if (actual != static_cast(mode_code)) { + return false; + } + } catch (const LiteGripError&) { + return false; + } + + ControlMode mode = ControlMode::kMit; + switch (mode_code) { + case ControlModeCode::kMit: + mode = ControlMode::kMit; + break; + case ControlModeCode::kPosVel: + mode = ControlMode::kPosVel; + break; + case ControlModeCode::kVel: + mode = ControlMode::kVel; + break; + case ControlModeCode::kPosForce: + mode = ControlMode::kPosForce; + break; + } + motor.set_mode(mode); + return true; +} + +void MotorController::control_mit(MotorState& motor, double kp, double kd, + double q, double dq, double tau) { + const auto data = pack_mit_frame(q, dq, kp, kd, tau, motor.limits()); + const int can_id = motor.can_id() + motor.mode_offset(); + transport_.send(can_id, data.data(), data.size()); +} + +double MotorController::read_param(MotorState& motor, DmReg rid, + double timeout_s) { + const int rid_value = static_cast(rid); + const auto request = pack_read_param_frame(motor.can_id(), rid_value); + transport_.send(kBroadcastId, request.data(), request.size()); + + const double deadline = monotonic_now() + timeout_s; + while (monotonic_now() < deadline) { + const auto frame = transport_.recv(0.01); + if (!frame.has_value()) { + continue; + } + const auto resp = unpack_param_response(frame->bytes(), frame->dlc); + if (!resp.has_value()) { + continue; + } + // Only our motor's answer, and only for the register we asked about. + if (resp->can_id != (motor.can_id() & 0x0F)) { + continue; + } + if ((resp->opcode == 0x33 || resp->opcode == 0x55) && resp->rid == rid_value) { + return resp->value; + } + } + throw CANTimeoutError("read_param timeout"); +} + +void MotorController::write_param(MotorState& motor, DmReg rid, double value) { + const auto data = + pack_write_param_frame(motor.can_id(), static_cast(rid), value); + transport_.send(kBroadcastId, data.data(), data.size()); +} + +void MotorController::save_params(MotorState& motor) { + const auto data = pack_save_param_frame(motor.can_id()); + transport_.send(kBroadcastId, data.data(), data.size()); +} + +MotorState* MotorController::poll(double timeout_s) { + const auto frame = transport_.recv(timeout_s); + if (!frame.has_value()) { + return nullptr; + } + + const auto it = motors_.find(frame->can_id); + if (it == motors_.end()) { + return nullptr; + } + MotorState& motor = *it->second; + + if (frame->dlc < 8) { + return nullptr; + } + + // Skip parameter response frames (0x33/0x55/0xAA): they share the motor's + // mst_id but have a different byte layout and would corrupt motor state if + // parsed as status. + // + // A status frame's data[2] is the velocity low byte, which can collide with + // an opcode value (e.g. 0x55), so data[2] alone is NOT a reliable + // discriminator. Require all three structural matches before discarding, so + // a normal status frame is never dropped. + const std::uint8_t opcode = frame->data[2]; + if ((opcode == 0x33 || opcode == 0x55 || opcode == 0xAA) && + (frame->data[0] >> 4) == 0 && + frame->data[0] == (motor.can_id() & 0xFF) && + frame->data[1] == ((motor.can_id() >> 8) & 0xFF)) { + return nullptr; + } + + const auto status = + unpack_status_frame(frame->data.data(), frame->dlc, motor.limits()); + if (!status.has_value()) { + return nullptr; + } + + motor.update_from_status(status->q, status->dq, status->tau, status->err, + status->t_mos, status->t_coil, frame->timestamp); + return &motor; +} + +bool MotorController::poll_until(MotorState& motor, double timeout_s) { + const std::uint64_t prev_rx = motor.rx_count(); + const double deadline = monotonic_now() + timeout_s; + while (monotonic_now() < deadline) { + MotorState* updated = poll(0.01); + if (updated == &motor && motor.rx_count() > prev_rx) { + return true; + } + sleep_s(0.001); + } + return false; +} + +int MotorController::detect_mst_id(int can_id, double timeout_s) { + const int can_id_low = can_id & 0x0F; + + // Detection must see frames on ids not yet known, so any active RX filter is + // cleared for the duration; add_motor() reinstalls it afterwards. + transport_.set_id_filter({}); + + // Strategy 1: read the MST_ID register. The motor answers on its real + // mst_id; match by the can_id embedded in the parameter response. + { + transport_.drain(); + const auto request = pack_read_param_frame(can_id, static_cast(DmReg::kMstId)); + transport_.send(kBroadcastId, request.data(), request.size()); + + const double deadline = monotonic_now() + timeout_s; + while (monotonic_now() < deadline) { + const auto frame = transport_.recv(0.01); + if (!frame.has_value()) { + continue; + } + const auto resp = unpack_param_response(frame->bytes(), frame->dlc); + if (!resp.has_value() || resp->rid != static_cast(DmReg::kMstId)) { + continue; + } + if (resp->can_id != can_id_low) { + continue; + } + const int mst = static_cast(resp->value); + if (mst >= 1 && mst <= 0xFE) { + return mst; + } + } + } + + // Strategy 2: send a status refresh; the status frame's arrival CAN id *is* + // the mst_id. + { + transport_.drain(); + const auto request = pack_refresh_frame(can_id); + transport_.send(kBroadcastId, request.data(), request.size()); + + const double deadline = monotonic_now() + timeout_s; + while (monotonic_now() < deadline) { + const auto frame = transport_.recv(0.01); + if (!frame.has_value() || frame->dlc < 8) { + continue; + } + if ((frame->data[0] & 0x0F) != can_id_low) { + continue; + } + const int mst = frame->can_id & 0x7FF; + if (mst >= 1 && mst <= 0xFE) { + return mst; + } + } + } + + // Neither strategy worked: fall back to the shipped default. The caller is + // expected to override it explicitly if that is wrong. + return kDefaultMstId; +} + +void MotorController::close() { transport_.close(); } + +} // namespace litegrip::can diff --git a/src/can/motor.cpp b/src/can/motor.cpp new file mode 100644 index 0000000..c951363 --- /dev/null +++ b/src/can/motor.cpp @@ -0,0 +1,49 @@ +// motor.cpp — MotorState, ported from litegrip_driver/litegrip/can/motor.py. + +#include "litegrip/can/motor.hpp" + +#include +#include + +namespace litegrip::can { +namespace { + +/// Same clock the transport timestamps frames with, so data_age_s() is valid. +double monotonic_now() noexcept { + return std::chrono::duration( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +} // namespace + +MotorState::MotorState(const MotorParams& params) : params_(params) { + // The Python original derived `limits` from the motor type in + // MotorParams.__post_init__ when none were given. A C++ aggregate cannot run + // code after member initialisation, so it happens here instead — and it is + // always taken from the motor type, which is what the field documents. + params_.limits = get_motor_limits(static_cast(params_.motor_type)); +} + +double MotorState::data_age_s() const noexcept { + if (rx_count_ == 0) { + return std::numeric_limits::infinity(); + } + const double age = monotonic_now() - last_update_; + return age < 0.0 ? 0.0 : age; +} + +void MotorState::update_from_status(double position, double velocity, + double torque, int error, int t_mos, + int t_coil, double timestamp) noexcept { + position_ = position; + velocity_ = velocity; + torque_ = torque; + error_ = error; + t_mos_ = t_mos; + t_coil_ = t_coil; + last_update_ = timestamp; + ++rx_count_; +} + +} // namespace litegrip::can diff --git a/src/can/protocol.cpp b/src/can/protocol.cpp new file mode 100644 index 0000000..5be60d9 --- /dev/null +++ b/src/can/protocol.cpp @@ -0,0 +1,202 @@ +// protocol.cpp — Damiao motor CAN protocol codec. +// +// Port of litegrip_driver/litegrip/can/protocol.py. The bit layouts below are +// protocol-spec and are asserted against the Python implementation's own test +// vectors (test/test_protocol.cpp, lifted from tests/test_protocol.py). + +#include "litegrip/can/protocol.hpp" + +#include + +namespace litegrip::can { +namespace { + +// Integer registers: MST_ID..CTRL_MODE (7..10), hw_ver..SN (13..16), +// can_br..sub_ver (35..36). Mirrors protocol._INT_REG_RANGES. +constexpr int kIntRegRanges[][2] = {{7, 10}, {13, 16}, {35, 36}}; + +} // namespace + +bool is_int_register(int rid) { + for (const auto& range : kIntRegRanges) { + if (rid >= range[0] && rid <= range[1]) { + return true; + } + } + return false; +} + +std::uint32_t float_to_uint(double value, double value_min, double value_max, + int bits) noexcept { + if (value < value_min) { + value = value_min; + } else if (value > value_max) { + value = value_max; + } + const double span = value_max - value_min; + const double offset = value - value_min; + const double max_code = static_cast((1u << bits) - 1u); + // Python uses int(), i.e. truncation toward zero. offset/span >= 0 here. + return static_cast(offset * max_code / span); +} + +double uint_to_float(std::uint32_t value, double value_min, double value_max, + int bits) noexcept { + const double span = value_max - value_min; + const double max_code = static_cast((1u << bits) - 1u); + return static_cast(value) * span / max_code + value_min; +} + +MotorLimits get_motor_limits(int motor_type) { + switch (motor_type) { + case 3: // DM4340 + return MotorLimits{12.5, 10.0, 28.0}; + case 6: // DM6248P + return MotorLimits{12.566, 20.0, 120.0}; + case 1: // DM4310 + default: // unknown types fall back to DM4310 + return MotorLimits{12.5, 30.0, 10.0}; + } +} + +std::array pack_mit_frame(double q, double dq, double kp, + double kd, double tau, + const MotorLimits& limits) noexcept { + const std::uint32_t q_uint = + float_to_uint(q, -limits.q_max, limits.q_max, 16); + const std::uint32_t dq_uint = + float_to_uint(dq, -limits.dq_max, limits.dq_max, 12); + const std::uint32_t kp_uint = float_to_uint(kp, 0.0, 500.0, 12); + const std::uint32_t kd_uint = float_to_uint(kd, 0.0, 5.0, 12); + const std::uint32_t tau_uint = + float_to_uint(tau, -limits.tau_max, limits.tau_max, 12); + + std::array data{}; + data[0] = static_cast((q_uint >> 8) & 0xFF); + data[1] = static_cast(q_uint & 0xFF); + data[2] = static_cast((dq_uint >> 4) & 0xFF); + data[3] = static_cast(((dq_uint & 0x0F) << 4) | + ((kp_uint >> 8) & 0x0F)); + data[4] = static_cast(kp_uint & 0xFF); + data[5] = static_cast((kd_uint >> 4) & 0xFF); + data[6] = static_cast(((kd_uint & 0x0F) << 4) | + ((tau_uint >> 8) & 0x0F)); + data[7] = static_cast(tau_uint & 0xFF); + return data; +} + +std::optional unpack_status_frame(const std::uint8_t* data, + std::size_t size, + const MotorLimits& limits) noexcept { + if (data == nullptr || size < 8) { + return std::nullopt; + } + + ParsedStatus out; + out.err = (data[0] >> 4) & 0x0F; + out.can_id = data[0] & 0x0F; + + const std::uint32_t q_uint = + (static_cast(data[1]) << 8 | data[2]) & 0xFFFFu; + const std::uint32_t dq_uint = + ((static_cast(data[3]) << 4) | (data[4] >> 4)) & 0xFFFu; + const std::uint32_t tau_uint = + ((static_cast(data[4] & 0x0F) << 8) | data[5]) & 0xFFFu; + + out.q = uint_to_float(q_uint, -limits.q_max, limits.q_max, 16); + out.dq = uint_to_float(dq_uint, -limits.dq_max, limits.dq_max, 12); + out.tau = uint_to_float(tau_uint, -limits.tau_max, limits.tau_max, 12); + out.t_mos = data[6]; + out.t_coil = data[7]; + return out; +} + +std::array pack_command_frame(std::uint8_t cmd) noexcept { + std::array data{}; + data.fill(0xFF); + data[7] = cmd; + return data; +} + +std::array pack_refresh_frame(int can_id) noexcept { + return {static_cast(can_id & 0xFF), + static_cast((can_id >> 8) & 0xFF), 0xCC, 0x00}; +} + +std::array pack_read_param_frame(int can_id, int rid) noexcept { + return {static_cast(can_id & 0xFF), + static_cast((can_id >> 8) & 0xFF), 0x33, + static_cast(rid & 0xFF), 0, 0, 0, 0}; +} + +std::array pack_write_param_frame(int can_id, int rid, + double value) noexcept { + std::uint8_t payload[4] = {0, 0, 0, 0}; + if (is_int_register(rid)) { + // Python: int(value).to_bytes(4, "little", signed=False) — truncate toward + // zero, then little-endian. + const std::uint32_t as_int = static_cast( + static_cast(value)); + payload[0] = static_cast(as_int & 0xFF); + payload[1] = static_cast((as_int >> 8) & 0xFF); + payload[2] = static_cast((as_int >> 16) & 0xFF); + payload[3] = static_cast((as_int >> 24) & 0xFF); + } else { + // IEEE-754 float32, little-endian. + const float f = static_cast(value); + std::uint32_t bits = 0; + std::memcpy(&bits, &f, sizeof(bits)); + payload[0] = static_cast(bits & 0xFF); + payload[1] = static_cast((bits >> 8) & 0xFF); + payload[2] = static_cast((bits >> 16) & 0xFF); + payload[3] = static_cast((bits >> 24) & 0xFF); + } + + return {static_cast(can_id & 0xFF), + static_cast((can_id >> 8) & 0xFF), 0x55, + static_cast(rid & 0xFF), payload[0], payload[1], + payload[2], payload[3]}; +} + +std::array pack_save_param_frame(int can_id) noexcept { + return {static_cast(can_id & 0xFF), + static_cast((can_id >> 8) & 0xFF), 0xAA, 0x01, 0, 0, 0, + 0}; +} + +std::optional unpack_param_response(const std::uint8_t* data, + std::size_t size) noexcept { + if (data == nullptr || size < 8) { + return std::nullopt; + } + + const int opcode = data[2]; + if (opcode != 0x33 && opcode != 0x55 && opcode != 0xAA) { + return std::nullopt; + } + + ParsedParamResponse out; + out.can_id = data[0] & 0x0F; + out.opcode = opcode; + out.rid = data[3]; + + if (is_int_register(out.rid)) { + // Little-endian uint32: data[7] is the most significant byte. + const std::uint32_t raw = (static_cast(data[7]) << 24) | + (static_cast(data[6]) << 16) | + (static_cast(data[5]) << 8) | + static_cast(data[4]); + out.value = static_cast(raw); + } else { + const std::uint32_t bits = static_cast(data[4]) | + (static_cast(data[5]) << 8) | + (static_cast(data[6]) << 16) | + (static_cast(data[7]) << 24); + float f = 0.0f; + std::memcpy(&f, &bits, sizeof(f)); + out.value = static_cast(f); + } + return out; +} + +} // namespace litegrip::can diff --git a/src/can/transport.cpp b/src/can/transport.cpp new file mode 100644 index 0000000..7a2b0a2 --- /dev/null +++ b/src/can/transport.cpp @@ -0,0 +1,342 @@ +// transport.cpp — SocketCAN transport. +// +// Port of litegrip_driver/litegrip/can/transport.py. Uses the kernel's own +// can_frame / canfd_frame layouts () instead of the Python +// original's hand-rolled struct formats — same wire layout, less to get wrong. +// +// One deliberate divergence: the Python code computed +// `is_extended = bool(can_id & CAN_EFF_MASK)` (CAN_EFF_MASK = 0x1FFFFFFF), +// which is true for *every* standard frame. That value is never consumed by +// the SDK, so rather than port a latent bug the canonical kernel flag +// CAN_EFF_FLAG is used here. + +#include "litegrip/can/transport.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "litegrip/exceptions.hpp" + +namespace litegrip::can { +namespace { + +double monotonic_now() noexcept { + return std::chrono::duration( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +std::string errno_text() { return std::strerror(errno); } + +std::string describe_interface(const std::string& channel) { + return "CAN interface '" + channel + "'"; +} + +} // namespace + +std::optional iface_mtu(int fd, const std::string& iface) noexcept { + if (fd < 0) { + return std::nullopt; + } + struct ifreq ifr{}; + std::strncpy(ifr.ifr_name, iface.c_str(), IFNAMSIZ - 1); + if (::ioctl(fd, SIOCGIFMTU, &ifr) < 0) { + return std::nullopt; + } + return ifr.ifr_mtu; +} + +CanTransport::CanTransport(std::string channel, CanMode mode) + : channel_(std::move(channel)), mode_(mode) {} + +CanTransport::~CanTransport() { close(); } + +CanTransport::CanTransport(CanTransport&& other) noexcept + : channel_(std::move(other.channel_)), + mode_(other.mode_), + fd_(other.fd_), + accept_ids_(std::move(other.accept_ids_)) { + other.fd_ = -1; +} + +CanTransport& CanTransport::operator=(CanTransport&& other) noexcept { + if (this != &other) { + close(); + channel_ = std::move(other.channel_); + mode_ = other.mode_; + fd_ = other.fd_; + accept_ids_ = std::move(other.accept_ids_); + other.fd_ = -1; + } + return *this; +} + +void CanTransport::open() { + if (is_open()) { + return; + } + + const int fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (fd < 0) { + throw ConnectError("socket(PF_CAN) failed: " + errno_text()); + } + + // Some USB-CAN adapters (gs_usb) and kernel versions need CAN FD socket mode + // on FD-capable interfaces; a classic CAN socket on such an interface may + // silently drop frames. Reconcile the requested mode with the interface MTU. + if (const auto mtu = iface_mtu(fd, channel_)) { + if (*mtu >= kCanFdMtu && mode_ == CanMode::kCan) { + mode_ = CanMode::kCanFd; + } else if (*mtu < kCanFdMtu && mode_ == CanMode::kCanFd) { + std::fprintf(stderr, + "[litegrip] %s has classic-CAN MTU %d but CAN FD was " + "requested; using classic CAN.\n", + describe_interface(channel_).c_str(), *mtu); + mode_ = CanMode::kCan; + } + } + + if (mode_ == CanMode::kCanFd) { + const int on = 1; + if (::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &on, sizeof(on)) < 0) { + std::fprintf(stderr, + "[litegrip] failed to enable CAN FD on %s (%s); falling " + "back to classic CAN.\n", + describe_interface(channel_).c_str(), errno_text().c_str()); + mode_ = CanMode::kCan; + } + } + + const unsigned int ifindex = ::if_nametoindex(channel_.c_str()); + if (ifindex == 0) { + ::close(fd); + throw ConnectError(describe_interface(channel_) + " not found"); + } + + struct sockaddr_can addr{}; + addr.can_family = AF_CAN; + addr.can_ifindex = static_cast(ifindex); + if (::bind(fd, reinterpret_cast(&addr), sizeof(addr)) < 0) { + const std::string why = errno_text(); + ::close(fd); + throw ConnectError("bind(" + describe_interface(channel_) + + ") failed: " + why); + } + + const int flags = ::fcntl(fd, F_GETFL, 0); + if (flags >= 0) { + ::fcntl(fd, F_SETFL, flags | O_NONBLOCK); + } + + fd_ = fd; + if (!accept_ids_.empty()) { + apply_id_filter(); + } +} + +void CanTransport::close() { + if (fd_ < 0) { + return; + } + ::close(fd_); + fd_ = -1; +} + +void CanTransport::set_id_filter(const std::vector& can_ids) { + accept_ids_.clear(); + for (const int id : can_ids) { + const int masked = id & CAN_SFF_MASK; + bool seen = false; + for (const int existing : accept_ids_) { + if (existing == masked) { + seen = true; + break; + } + } + if (!seen) { + accept_ids_.push_back(masked); + } + } + if (is_open()) { + apply_id_filter(); + } +} + +void CanTransport::add_id_filter(int can_id) { + std::vector ids = accept_ids_; + ids.push_back(can_id); + set_id_filter(ids); +} + +void CanTransport::apply_id_filter() { + if (fd_ < 0) { + return; + } + + // An empty filter list means "receive nothing" to SocketCAN, so removing the + // filter must be done with a single match-all entry instead. + std::vector filters; + if (!accept_ids_.empty()) { + filters.reserve(accept_ids_.size()); + for (const int id : accept_ids_) { + filters.push_back(can_filter{static_cast(id), + static_cast(CAN_SFF_MASK)}); + } + } else { + filters.push_back(can_filter{0, 0}); + } + + if (::setsockopt(fd_, SOL_CAN_RAW, CAN_RAW_FILTER, filters.data(), + static_cast(filters.size() * + sizeof(can_filter))) < 0) { + std::fprintf(stderr, "[litegrip] failed to set CAN RX filter on %s: %s\n", + channel_.c_str(), errno_text().c_str()); + } +} + +void CanTransport::send(int can_id, const std::uint8_t* data, std::size_t size, + double timeout_s) { + if (!is_open()) { + throw CommError("transport not open"); + } + if (data == nullptr && size > 0) { + throw CommandError("null payload with non-zero size"); + } + + const void* frame = nullptr; + std::size_t frame_len = 0; + struct can_frame classic{}; + struct canfd_frame fd_frame{}; + + if (mode_ == CanMode::kCanFd) { + if (size > sizeof(fd_frame.data)) { + throw CommandError("CAN FD frames hold at most 64 bytes"); + } + fd_frame.can_id = static_cast(can_id) & CAN_EFF_MASK; + fd_frame.len = static_cast<__u8>(size); + if (size > 0) { + std::memcpy(fd_frame.data, data, size); + } + frame = &fd_frame; + frame_len = CANFD_MTU; + } else { + if (size > sizeof(classic.data)) { + throw CommandError("classic CAN frames hold at most 8 bytes"); + } + classic.can_id = static_cast(can_id) & CAN_SFF_MASK; + classic.can_dlc = static_cast<__u8>(size); + if (size > 0) { + std::memcpy(classic.data, data, size); + } + frame = &classic; + frame_len = CAN_MTU; + } + + // The socket is non-blocking: retry while the send buffer is full. + const double deadline = monotonic_now() + timeout_s; + const char* bytes = static_cast(frame); + while (true) { + const ssize_t written = ::write(fd_, bytes, frame_len); + if (written >= 0) { + return; + } + if (errno != EAGAIN && errno != EWOULDBLOCK && errno != EINTR) { + throw CommError("CAN send failed on " + channel_ + ": " + errno_text()); + } + const double remaining = deadline - monotonic_now(); + if (remaining <= 0.0) { + std::fprintf(stderr, + "[litegrip] CAN send timeout on %s (buffer full >%.1fs), " + "frame to 0x%03X dropped\n", + channel_.c_str(), timeout_s, can_id & CAN_SFF_MASK); + return; + } + struct pollfd pfd{}; + pfd.fd = fd_; + pfd.events = POLLOUT; + ::poll(&pfd, 1, static_cast(remaining * 1000.0) + 1); + } +} + +void CanTransport::send_multi(int can_id, const std::uint8_t* data, + std::size_t size, int count, double interval_s) { + for (int i = 0; i < count; ++i) { + send(can_id, data, size); + if (count > 1 && interval_s > 0.0) { + std::this_thread::sleep_for( + std::chrono::duration(interval_s)); + } + } +} + +std::optional CanTransport::recv(double timeout_s) { + if (!is_open()) { + return std::nullopt; + } + + struct pollfd pfd{}; + pfd.fd = fd_; + pfd.events = POLLIN; + + const int timeout_ms = + timeout_s <= 0.0 ? 0 : static_cast(timeout_s * 1000.0) + 1; + const int ready = ::poll(&pfd, 1, timeout_ms); + if (ready <= 0) { + return std::nullopt; + } + + std::uint8_t buffer[CANFD_MTU] = {}; + const ssize_t n = ::read(fd_, buffer, sizeof(buffer)); + if (n < 0) { + return std::nullopt; + } + + CanFrame out; + out.timestamp = monotonic_now(); + + if (mode_ == CanMode::kCanFd) { + if (n < static_cast(sizeof(struct canfd_frame))) { + return std::nullopt; + } + struct canfd_frame fd_frame{}; + std::memcpy(&fd_frame, buffer, sizeof(fd_frame)); + out.is_extended = (fd_frame.can_id & CAN_EFF_FLAG) != 0; + out.is_fd = true; + out.can_id = static_cast(fd_frame.can_id & CAN_SFF_MASK); + out.dlc = fd_frame.len > sizeof(out.data) ? sizeof(out.data) : fd_frame.len; + std::memcpy(out.data.data(), fd_frame.data, out.dlc); + } else { + if (n < static_cast(sizeof(struct can_frame))) { + return std::nullopt; + } + struct can_frame classic{}; + std::memcpy(&classic, buffer, sizeof(classic)); + out.is_extended = (classic.can_id & CAN_EFF_FLAG) != 0; + out.is_fd = false; + out.can_id = static_cast(classic.can_id & CAN_SFF_MASK); + out.dlc = classic.can_dlc > sizeof(out.data) ? sizeof(out.data) + : classic.can_dlc; + std::memcpy(out.data.data(), classic.data, out.dlc); + } + return out; +} + +void CanTransport::drain() { + while (recv(0.0).has_value()) { + } +} + +} // namespace litegrip::can diff --git a/src/constants.cpp b/src/constants.cpp new file mode 100644 index 0000000..b37a617 --- /dev/null +++ b/src/constants.cpp @@ -0,0 +1,40 @@ +// constants.cpp — error-code descriptions. +// +// The Python original's messages were Chinese; the C++ SDK uses English (see +// PLAN-litegrip-cpp.md). The *set* of codes and their meanings is unchanged. + +#include "litegrip/constants.hpp" + +#include + +namespace litegrip { + +std::string describe_error(int code) { + switch (code) { + case 0x0: + return "disabled"; + case 0x1: + return "enabled"; + case 0x8: + return "overvoltage fault (OV)"; + case 0x9: + return "undervoltage fault (UV)"; + case 0xA: + return "overcurrent fault (OC)"; + case 0xB: + return "MOS over-temperature fault"; + case 0xC: + return "coil over-temperature fault"; + case 0xD: + return "communication loss (CAN timeout)"; + case 0xE: + return "overload fault"; + default: + break; + } + char buf[48]; + std::snprintf(buf, sizeof(buf), "unknown error (0x%X)", code); + return buf; +} + +} // namespace litegrip diff --git a/src/control_loop.cpp b/src/control_loop.cpp new file mode 100644 index 0000000..6c8a456 --- /dev/null +++ b/src/control_loop.cpp @@ -0,0 +1,575 @@ +// control_loop.cpp — the background control loop. +// +// Replaces the old ros2_control Python daemon + shared-memory bridge: with a +// C++ SDK the two-process split has no reason to exist, so "rate-limit the +// target, allocate the torque budget, gate the frame, stream it, watch the +// feedback" now lives in one thread inside the library. +// +// R1: the loop owns a background thread. A DM motor needs a continuous MIT +// frame stream (~900 ms of silence latches the 0xD comm-loss fault) and the +// controller_manager cycle is not guaranteed stable, so the layer above only +// posts a target and reads a cached snapshot. +// +// dry_run is not "do nothing": it runs the whole control path — rate limiting, +// torque-budget allocation and the safety gate — against a simulated plant and +// only skips opening CAN and sending. That makes the gate and the +// deploy-config fail-closed behaviour testable without hardware, and is what +// makes a dry-run ros2_control stack meaningful. + +#include "litegrip/control_loop.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "litegrip/bus.hpp" +#include "litegrip/constants.hpp" +#include "litegrip/exceptions.hpp" + +namespace litegrip { +namespace { + +double monotonic_now() noexcept { + return std::chrono::duration( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +/// MIT gains allocated from a torque budget. +/// +/// The frame's kp/kd are handed to the driver, which evaluates them against the +/// error it measures in real time over the whole control period. So the budget +/// is met using WORST-CASE bounds (e_b, v_b) rather than the current samples: +/// scaling by the sampling instant would only guarantee that one instant stays +/// within budget. Damping is allocated first, so when the budget is short kp is +/// cut first — and if the damping term alone fills the budget, kp goes to 0 and +/// the frame is nothing but a brake. +struct Gains { + double kp = 0.0; + double kd = 0.0; + bool usable = false; +}; + +Gains allocate_gains(double budget_nm, double max_position_error_rad, + double max_feedback_velocity_rad_s, double kp_ceiling, + double kd_ceiling) { + Gains gains; + if (!(budget_nm > 0.0) || !(max_position_error_rad > 0.0) || + !(max_feedback_velocity_rad_s > 0.0)) { + // "Not given" is not "use a default": without a worst-case velocity bound + // there is no way to argue the budget holds, so refuse to send motion at all + // rather than send something nobody can bound. + return gains; + } + gains.kd = std::min(kd_ceiling, budget_nm / max_feedback_velocity_rad_s); + const double after_damping = + budget_nm - gains.kd * max_feedback_velocity_rad_s; + gains.kp = after_damping > 0.0 + ? std::min(kp_ceiling, after_damping / max_position_error_rad) + : 0.0; + gains.usable = true; + return gains; +} + +} // namespace + +struct ControlLoop::Impl { + explicit Impl(ControlLoopConfig cfg) : config(std::move(cfg)) {} + + ControlLoopConfig config; + + std::unique_ptr bus; + std::unique_ptr safety; + bool initialized = false; + + // Shared with the plugin-facing surface. + mutable std::mutex mutex; + GripperState state; + double target_rad = 0.0; + bool enable_request = false; + bool estop_request = false; + double last_command_time = 0.0; + + /// Latched fault code. Atomic because it is written by the loop thread and + /// read by fault_code() without taking the mutex. Keeps the FIRST code, like + /// a latch, so the original cause is not masked by its consequences. + std::atomic fault_code{0}; + + // Loop-thread-only state. + double trajectory_rad = 0.0; + bool trajectory_started = false; + double last_cycle_time = 0.0; + double last_feedback_time = 0.0; + std::uint64_t last_rx_count = 0; + + void set_fault(FaultCode code) { set_fault(static_cast(code)); } + void set_fault(int code) { + int expected = 0; + fault_code.compare_exchange_strong(expected, code); + } + + /// Put the motor in a known non-driving state, and KEEP STREAMING it. + /// + /// Going silent instead would let the motor latch its own comm-loss fault + /// after ~900 ms, and that manufactured fault would then be what the operator + /// sees instead of the real cause. + void stream_zero_torque() { + if (bus == nullptr) { + return; + } + try { + safety->guard_zero_torque_frame(0.0, 0.0, 0.0, 0.0, "zero_torque"); + bus->control_mit(0.0, 0.0, 0.0, 0.0, 0.0); + bus->poll(0.0); + } catch (const LiteGripError&) { + // Best effort: this path must not itself throw. + } + } + + void publish_snapshot(can::MotorState& motor, int error_code, int t_mos, + int t_coil, double now) { + std::lock_guard lock(mutex); + state.position_rad = motor.position(); + state.velocity_rad_s = motor.velocity(); + state.torque_nm = motor.torque(); + state.temperature_mos = t_mos; + state.temperature_coil = t_coil; + state.error_code = error_code; + state.timestamp = now; + state.data_age_s = motor.data_age_s(); + state.position_mm = + (config.pos_closed_rad - motor.position()) * config.rad_to_mm; + } +}; + +ControlLoop::ControlLoop(ControlLoopConfig config) + : impl_(std::make_unique(std::move(config))) {} + +ControlLoop::~ControlLoop() { stop(); } + +const ControlLoopConfig& ControlLoop::config() const noexcept { + return impl_->config; +} + +bool ControlLoop::start() { + if (running_) { + return true; + } + + const ControlLoopConfig& cfg = impl_->config; + + // Deploy configuration may only be TIGHTER than the SDK's hard ceilings. + if (cfg.max_velocity_rad_s > kMaxCommandVelocityCeilingRadS) { + throw SafetyConfigError( + "max_velocity_rad_s exceeds the hard ceiling — it may only be lowered"); + } + if (cfg.torque_limit_nm > kTorqueLimitCeilingNm) { + throw SafetyConfigError( + "torque_limit_nm exceeds the hard ceiling — it may only be lowered"); + } + if (!(cfg.control_rate_hz > 0.0)) { + throw SafetyConfigError("control_rate_hz must be positive"); + } + + const bool dry_run = cfg.dry_run; + if (!dry_run && !cfg.hardware_enable) { + // The dual switch: dry_run blocks "do not touch hardware while debugging", + // hardware_enable blocks "is a gripper actually attached to this machine". + // With both off there is no legitimate way to proceed. + throw SafetyConfigError( + "dry_run=false requires hardware_enable=true: without the second switch " + "the loop cannot tell whether a gripper is actually attached"); + } + + // Load the versioned baseline first: fail-closed, a missing file throws. + impl_->safety = + std::make_unique(load_safety_baseline(cfg.safety_baseline)); + + if (!dry_run) { + GripperConfig bus_config; + bus_config.can_channel = cfg.channel; + bus_config.can_id = cfg.can_id; + bus_config.mst_id = cfg.mst_id; + bus_config.canfd_mode = cfg.canfd_mode; + bus_config.pos_closed_rad = cfg.pos_closed_rad; + bus_config.pos_open_rad = cfg.pos_open_rad; + bus_config.rad_to_mm = cfg.rad_to_mm; + bus_config.kp = cfg.kp; + bus_config.kd = cfg.kd; + + impl_->bus = std::make_unique(bus_config); + impl_->bus->connect(); // throws ConnectError + impl_->bus->init(cfg.kp, cfg.kd); // enable + hold at the current position + + impl_->initialized = true; + impl_->last_feedback_time = monotonic_now(); + if (can::MotorState* motor = impl_->bus->motor()) { + impl_->last_rx_count = motor->rx_count(); + impl_->trajectory_rad = motor->position(); + impl_->trajectory_started = true; + + // ★ Publish the first snapshot NOW, before the thread runs. + // + // The layer above latches its command interface from state() the moment + // it activates, and it may activate before the first control cycle has + // produced a snapshot. Leaving state() default-constructed means it + // reads 0.0 rad, converts that to an opening, and commands the gripper + // THERE — which on this hardware is the closed end, so a gripper sitting + // at 40 mm would travel to 3 mm the instant the stack came up. The + // measurement exists (bus->init() waited for a real status frame), so + // there is no reason to hand out a placeholder. + impl_->publish_snapshot(*motor, motor->error(), motor->t_mos(), + motor->t_coil(), monotonic_now()); + } + } else { + // Simulated plant: start at the calibrated closed end, CLAMPED into the red + // lines so it begins in a commandable position. + // + // The clamp is not cosmetic. A real gripper at rest is usually fully closed, + // and "fully closed" can legitimately lie outside the red lines — the red + // line exists precisely to keep the last millimetre of travel off limits. + // Starting the simulation there would make the gate refuse every frame and + // latch a fault, which is correct behaviour but a useless demonstration, and + // it would look like a bug in the demo rather than the safety rule working. + const SafetyLimits& limits = impl_->safety->limits(); + const double red_lo = std::min(limits.red_min_rad, limits.red_max_rad); + const double red_hi = std::max(limits.red_min_rad, limits.red_max_rad); + const double start_rad = std::clamp(cfg.pos_closed_rad, red_lo, red_hi); + + impl_->state.position_rad = start_rad; + impl_->state.position_mm = + (cfg.pos_closed_rad - start_rad) * cfg.rad_to_mm; + // The simulated plant always has "fresh" feedback, and reports enabled. + // Without this the snapshot would look like a motor that has never answered, + // and a consumer that (correctly) refuses to latch a command against a + // placeholder would refuse to activate at all. + impl_->state.data_age_s = 0.0; + impl_->state.error_code = 1; + impl_->state.timestamp = monotonic_now(); + impl_->trajectory_rad = start_rad; + impl_->trajectory_started = true; + impl_->last_feedback_time = monotonic_now(); + } + + impl_->fault_code = 0; + impl_->last_command_time = monotonic_now(); + impl_->last_cycle_time = monotonic_now(); + running_ = true; + thread_ = std::thread([this] { thread_main(); }); + return true; +} + +void ControlLoop::stop() { + if (running_) { + running_ = false; + if (thread_.joinable()) { + thread_.join(); + } + } + + if (impl_->bus == nullptr) { + return; + } + + // Leave the mechanism safe: zero torque, then disable (which also closes the + // transport). + try { + impl_->safety->guard_zero_torque_frame(0.0, 0.0, 0.0, 0.0, + "control_loop_stop"); + impl_->bus->control_mit(0.0, 0.0, 0.0, 0.0, 0.0); + impl_->bus->update_state(0.02); + } catch (const LiteGripError&) { + // Best effort: stopping must not itself throw. + } + impl_->bus->disconnect(); + impl_->bus.reset(); + impl_->initialized = false; +} + +void ControlLoop::thread_main() { + const double period = 1.0 / impl_->config.control_rate_hz; + + while (running_) { + const double cycle_start = monotonic_now(); + const double elapsed = std::max(0.0, cycle_start - impl_->last_cycle_time); + impl_->last_cycle_time = cycle_start; + + try { + if (impl_->config.dry_run) { + dry_run_cycle(elapsed); + } else { + hardware_cycle(elapsed); + } + } catch (const LiteGripError& error) { + // A gate rejection or a hardware error: latch a fault and stop driving. + impl_->set_fault(FaultCode::kHardwareSafeStop); + std::fprintf(stderr, "[litegrip] control loop fault: %s\n", error.what()); + safe_stop(); + } + + const double remaining = period - (monotonic_now() - cycle_start); + if (remaining > 0.0) { + std::this_thread::sleep_for(std::chrono::duration(remaining)); + } + } +} + +void ControlLoop::hardware_cycle(double elapsed) { + can::MotorState* motor = impl_->bus->motor(); + if (motor == nullptr) { + impl_->set_fault(FaultCode::kInternal); + return; + } + + // ── emergency stop wins over everything ─────────────────────────────── + bool estop = false; + { + std::lock_guard lock(impl_->mutex); + estop = impl_->estop_request; + impl_->estop_request = false; + } + if (estop) { + impl_->set_fault(FaultCode::kHardwareSafeStop); + safe_stop(); + return; + } + + // ── feedback watchdogs ──────────────────────────────────────────────── + const double now = monotonic_now(); + impl_->bus->poll(0.0); + if (motor->rx_count() != impl_->last_rx_count) { + impl_->last_rx_count = motor->rx_count(); + impl_->last_feedback_time = now; + + // Feedback crossed a red line => latch (or, in a mode that permits it, only + // warn). This is the feedback-side half of the gate. + impl_->safety->guard_feedback_position(motor->position(), "control_loop"); + } else if (now - impl_->last_feedback_time > impl_->config.feedback_timeout_s) { + impl_->set_fault(FaultCode::kNoFeedback); + safe_stop(); + return; + } + + const int error_code = motor->error(); + const int t_mos = motor->t_mos(); + const int t_coil = motor->t_coil(); + + if (t_mos > impl_->config.temperature_limit_c || + t_coil > impl_->config.temperature_limit_c) { + // Stop BEFORE the driver trips on its own overtemperature fault: by the time + // the driver reports it, the point of stopping early is already lost. + impl_->set_fault(FaultCode::kHardwareSafeStop); + safe_stop(); + return; + } + if (error_code != 0 && error_code != 1) { + // Motor faults occupy their own segment so they cannot be confused with the + // bridge-layer codes. + impl_->set_fault(kMotorFaultBase + error_code); + safe_stop(); + return; + } + + // ── the command ─────────────────────────────────────────────────────── + double target = 0.0; + bool enabled = false; + { + std::lock_guard lock(impl_->mutex); + target = impl_->target_rad; + enabled = impl_->enable_request; + if ((now - impl_->last_command_time) > impl_->config.command_timeout_s) { + // Stale command: hold where we are rather than keep driving toward a + // target nobody is maintaining. + target = motor->position(); + impl_->trajectory_started = false; + } + } + + if (impl_->fault_code.load() != 0) { + // A fault is latched: hold a known non-driving state, but keep streaming — + // see stream_zero_torque() for why silence would be worse. + impl_->stream_zero_torque(); + impl_->publish_snapshot(*motor, error_code, t_mos, t_coil, now); + return; + } + + if (!enabled) { + // Not following a command: hold the measured position, STILL STREAMING. A DM + // motor that hears nothing for ~900 ms latches its comm-loss fault, so + // going idle would manufacture the very fault this loop watches for. + target = motor->position(); + impl_->trajectory_started = false; + } + + // ── rate limit the trajectory ───────────────────────────────────────── + const std::optional measured_position = + motor->has_data() ? std::optional(motor->position()) + : std::nullopt; + if (!impl_->trajectory_started) { + // The first frame has no previous trajectory point to advance from, so it + // starts AT the measured position (step 0): there is no jump straight to the + // target on the first frame after power-up. + impl_->trajectory_rad = measured_position.value_or(target); + impl_->trajectory_started = true; + } + const double max_step = impl_->config.max_velocity_rad_s * elapsed; + impl_->trajectory_rad += + std::max(-max_step, std::min(max_step, target - impl_->trajectory_rad)); + + // ── torque budget ───────────────────────────────────────────────────── + double max_position_error = impl_->config.max_position_error_rad; + if (max_position_error < 0.0) { + // -1 means "derive it": the widest error the red lines permit. Both the + // target and the reading are forced inside the red lines, so their width is + // a real bound rather than a made-up one. + const SafetyLimits& limits = impl_->safety->limits(); + max_position_error = limits.red_max_rad - limits.red_min_rad; + } + const Gains gains = allocate_gains( + impl_->config.torque_limit_nm, max_position_error, + impl_->config.max_feedback_velocity_rad_s, impl_->config.kp, + impl_->config.kd); + if (!gains.usable) { + // No worst-case velocity bound => no bounded torque budget => no motion. + impl_->set_fault(FaultCode::kCommandRejected); + return; + } + + // ── gate and send ───────────────────────────────────────────────────── + const double q_safe = impl_->safety->guard_motion_frame( + impl_->trajectory_rad, gains.kp, gains.kd, 0.0, 0.0, measured_position, + motor->velocity(), motor->torque(), "control_loop"); + + // The dq field stays 0: the rate ceiling is applied to the TARGET above, not + // sent as a velocity setpoint. Sending it as dq would mean "charge on at this + // speed forever" — a different and more dangerous motion with no endpoint. + impl_->bus->control_mit(q_safe, gains.kp, gains.kd, 0.0, 0.0); + + // ── publish the snapshot ────────────────────────────────────────────── + impl_->publish_snapshot(*motor, error_code, t_mos, t_coil, now); +} + +void ControlLoop::dry_run_cycle(double elapsed) { + const double now = monotonic_now(); + + bool estop = false; + { + std::lock_guard lock(impl_->mutex); + estop = impl_->estop_request; + impl_->estop_request = false; + } + if (estop) { + impl_->set_fault(FaultCode::kHardwareSafeStop); + return; + } + + double target = 0.0; + bool enabled = false; + { + std::lock_guard lock(impl_->mutex); + target = impl_->target_rad; + enabled = impl_->enable_request; + if ((now - impl_->last_command_time) > impl_->config.command_timeout_s) { + target = impl_->state.position_rad; + impl_->trajectory_started = false; + } + } + + if (!enabled || impl_->fault_code.load() != 0) { + return; + } + + if (!impl_->trajectory_started) { + impl_->trajectory_rad = impl_->state.position_rad; + impl_->trajectory_started = true; + } + const double previous = impl_->trajectory_rad; + const double max_step = impl_->config.max_velocity_rad_s * elapsed; + impl_->trajectory_rad += + std::max(-max_step, std::min(max_step, target - impl_->trajectory_rad)); + + // The same budget allocation and the same gate as the real path, so the + // deploy-config fail-closed behaviour is exercised here too. + double max_position_error = impl_->config.max_position_error_rad; + if (max_position_error < 0.0) { + const SafetyLimits& limits = impl_->safety->limits(); + max_position_error = limits.red_max_rad - limits.red_min_rad; + } + const Gains gains = allocate_gains( + impl_->config.torque_limit_nm, max_position_error, + impl_->config.max_feedback_velocity_rad_s, impl_->config.kp, + impl_->config.kd); + if (!gains.usable) { + impl_->set_fault(FaultCode::kCommandRejected); + return; + } + + const double simulated_velocity = + elapsed > 0.0 ? (impl_->trajectory_rad - previous) / elapsed : 0.0; + const std::optional measured_position(impl_->state.position_rad); + const double q_safe = impl_->safety->guard_motion_frame( + impl_->trajectory_rad, gains.kp, gains.kd, 0.0, 0.0, measured_position, + simulated_velocity, 0.0, "control_loop(dry_run)"); + + std::lock_guard lock(impl_->mutex); + impl_->state.position_rad = q_safe; + impl_->state.velocity_rad_s = simulated_velocity; + impl_->state.torque_nm = 0.0; + impl_->state.temperature_mos = 0; + impl_->state.temperature_coil = 0; + impl_->state.error_code = 1; // simulated: enabled + impl_->state.timestamp = now; + impl_->state.data_age_s = 0.0; + impl_->state.position_mm = + (impl_->config.pos_closed_rad - q_safe) * impl_->config.rad_to_mm; +} + +void ControlLoop::safe_stop() { impl_->stream_zero_torque(); } + +// ── plugin-facing surface ───────────────────────────────────────────────── + +void ControlLoop::set_target_rad(double rad) { + std::lock_guard lock(impl_->mutex); + impl_->target_rad = rad; + impl_->last_command_time = monotonic_now(); +} + +void ControlLoop::set_target_mm(double mm) { + const double rad = + impl_->config.pos_closed_rad - mm / impl_->config.rad_to_mm; + set_target_rad(rad); +} + +void ControlLoop::set_enable(bool enable) { + std::lock_guard lock(impl_->mutex); + impl_->enable_request = enable; + if (enable) { + impl_->last_command_time = monotonic_now(); + } +} + +void ControlLoop::emergency_stop() { + std::lock_guard lock(impl_->mutex); + impl_->estop_request = true; +} + +GripperState ControlLoop::state() const { + std::lock_guard lock(impl_->mutex); + return impl_->state; +} + +int ControlLoop::fault_code() const { return impl_->fault_code.load(); } + +const SafetyLimits& ControlLoop::safety_limits() const noexcept { + // Built in start() before the thread exists and never replaced afterwards, so + // reading it here cannot race with the thread. + static const SafetyLimits kFallback{}; + return impl_->safety != nullptr ? impl_->safety->limits() : kFallback; +} + +} // namespace litegrip diff --git a/src/exceptions.cpp b/src/exceptions.cpp new file mode 100644 index 0000000..3f3ffca --- /dev/null +++ b/src/exceptions.cpp @@ -0,0 +1,19 @@ +// exceptions.cpp — LiteGripError::render. + +#include "litegrip/exceptions.hpp" + +#include + +namespace litegrip { + +std::string LiteGripError::render(const std::string& message, int error_code) { + if (error_code == kNoErrorCode) { + return message; + } + // Matches the Python SDK's __str__: " [0xNNNN]". + char suffix[16]; + std::snprintf(suffix, sizeof(suffix), " [0x%04X]", error_code); + return message + suffix; +} + +} // namespace litegrip diff --git a/src/gripper.cpp b/src/gripper.cpp new file mode 100644 index 0000000..7dc8643 --- /dev/null +++ b/src/gripper.cpp @@ -0,0 +1,788 @@ +// gripper.cpp — LiteGrip, the high-level API, ported from +// litegrip_driver/litegrip/gripper.py and wired to the safety core. +// +// Scope is v1 (plan D6): lifecycle, hold-based init, calibration, position +// motion, state and parameter access. Force control (grasp / set_force), +// constant-speed moves and the public zero-gravity mode are deliberately absent. +// +// Safety wiring (D4): the MOTION path (goto_rad / move_to / open / close / home) +// goes through SafetyGuard::guard_motion_frame, so a target outside the red +// lines, a measured position already outside them, over-ceiling gains, an +// over-budget feed-forward torque, or a velocity beyond the deceleration zone +// are all refused with a diagnosable reason and nothing is sent. +// +// The expert paths that must keep working when the gripper is outside the red +// lines do NOT go through that gate, and each says why at its definition: +// stop() / zero-torque frames (an emergency stop has to work from anywhere) and +// the calibration routines (they deliberately drive to the mechanical stops). + +#include "litegrip/gripper.hpp" + +#include +#include +#include +#include +#include +#include + +#include "litegrip/calibration.hpp" +#include "litegrip/constants.hpp" +#include "litegrip/exceptions.hpp" + +namespace litegrip { +namespace { + +double monotonic_now() noexcept { + return std::chrono::duration( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +double wall_clock_now() noexcept { + return std::chrono::duration( + std::chrono::system_clock::now().time_since_epoch()) + .count(); +} + +void sleep_s(double seconds) { + if (seconds > 0.0) { + std::this_thread::sleep_for(std::chrono::duration(seconds)); + } +} + +const char* motor_type_name(can::MotorType type) noexcept { + switch (type) { + case can::MotorType::kDM3507: + return "DM3507"; + case can::MotorType::kDM4310: + return "DM4310"; + case can::MotorType::kDM4310_48V: + return "DM4310_48V"; + case can::MotorType::kDM4340: + return "DM4340"; + case can::MotorType::kDM4340_48V: + return "DM4340_48V"; + case can::MotorType::kDM6006: + return "DM6006"; + case can::MotorType::kDM6248P: + return "DM6248P"; + case can::MotorType::kDM8006: + return "DM8006"; + case can::MotorType::kDM8009: + return "DM8009"; + case can::MotorType::kDM10010L: + return "DM10010L"; + case can::MotorType::kDM10010: + return "DM10010"; + case can::MotorType::kDMH3510: + return "DMH3510"; + case can::MotorType::kDMH6215: + return "DMH6215"; + case can::MotorType::kDMS3519: + return "DMS3519"; + case can::MotorType::kDMG6220: + return "DMG6220"; + } + return "DM4310"; +} + +/// Measured feedback as the optional triple the safety gate wants. A motor that +/// has never produced a status frame reports position 0.0, which is a fake +/// value — hence nullopt rather than 0.0. +struct Feedback { + std::optional position; + std::optional velocity; + std::optional torque; +}; + +Feedback measured_feedback(can::MotorState* motor) { + if (motor == nullptr || !motor->has_data()) { + return Feedback{std::nullopt, std::nullopt, std::nullopt}; + } + return Feedback{motor->position(), motor->velocity(), motor->torque()}; +} + +} // namespace + +LiteGrip::LiteGrip(GripperConfig config) + : config_(std::move(config)), + bus_(std::make_unique(config_)), + safety_(std::make_unique(canonical_baseline())), + mst_id_(config_.mst_id) {} + +LiteGrip::~LiteGrip() { disconnect(); } + +LiteGrip::LiteGrip(LiteGrip&& other) noexcept + : config_(std::move(other.config_)), + bus_(std::move(other.bus_)), + safety_(std::move(other.safety_)), + mst_id_(other.mst_id_), + connected_(other.connected_), + enabled_(other.enabled_), + status_flags_(other.status_flags_) { + other.mst_id_.reset(); + other.connected_ = false; + other.enabled_ = false; + other.status_flags_ = GripperStatus::kNone; +} + +LiteGrip& LiteGrip::operator=(LiteGrip&& other) noexcept { + if (this != &other) { + disconnect(); + config_ = std::move(other.config_); + bus_ = std::move(other.bus_); + safety_ = std::move(other.safety_); + mst_id_ = other.mst_id_; + connected_ = other.connected_; + enabled_ = other.enabled_; + status_flags_ = other.status_flags_; + other.mst_id_.reset(); + other.connected_ = false; + other.enabled_ = false; + other.status_flags_ = GripperStatus::kNone; + } + return *this; +} + +LiteGrip LiteGrip::connect_raii(GripperConfig config) { + LiteGrip instance(std::move(config)); + instance.connect(); + return instance; +} + +// ── connection ──────────────────────────────────────────────────────────── + +bool LiteGrip::connect() { + if (connected_) { + return true; + } + + bus_->connect(); // throws ConnectError + + // register_gripper() fills in mst_id when the config left it unset, so the + // detected value comes back through the bus config. + config_.mst_id = bus_->config().mst_id; + mst_id_ = config_.mst_id; + connected_ = true; + status_flags_ = GripperStatus::kNone; + return true; +} + +void LiteGrip::disconnect() { + if (!connected_) { + return; + } + if (enabled_) { + disable(); + } + if (bus_ != nullptr) { + bus_->disconnect(); + } + connected_ = false; + enabled_ = false; + status_flags_ = GripperStatus::kNone; +} + +// ── enable / init / fault ───────────────────────────────────────────────── + +bool LiteGrip::enable() { + check_connected(); + + // A latched fault has to be cleared before the motor will accept an enable; + // init() below is what actually proves the link. + const int error = get_error(); + if (error != 0 && error != 1) { + clear_fault(); + } + return init(); +} + +bool LiteGrip::init() { + check_connected(); + + try { + enabled_ = bus_->init(config_.kp, config_.kd); + } catch (const HardwareError&) { + enabled_ = false; + throw; + } catch (const LiteGripError& error) { + enabled_ = false; + throw HardwareError(std::string("enable failed: ") + error.what()); + } + + if (enabled_) { + status_flags_ |= GripperStatus::kEnabled; + } + return enabled_; +} + +bool LiteGrip::disable() { + check_connected(); + const bool result = bus_->disable(); + enabled_ = false; + status_flags_ &= ~GripperStatus::kEnabled; + return result; +} + +bool LiteGrip::clear_fault() { + check_connected(); + const bool cleared = bus_->clear_fault(config_.kp, config_.kd); + if (!cleared) { + const int error = bus_->get_error(); + throw HardwareError( + std::string("could not clear the fault: ") + describe_error(error), + error); + } + enabled_ = true; + status_flags_ |= GripperStatus::kEnabled; + return true; +} + +void LiteGrip::stop() { + if (bus_ == nullptr || !enabled_) { + return; + } + // Deliberately NOT routed through guard_motion_frame: an emergency stop must + // work even when the gripper is outside the red lines. The zero-torque + // invariant is asserted instead, which is what makes the bypass legitimate. + safety_->guard_zero_torque_frame(0.0, 0.0, 0.0, 0.0, "stop"); + bus_->control_mit(0.0, 0.0, 0.0, 0.0, 0.0); + bus_->update_state(0.02); +} + +// ── low-level frame access ──────────────────────────────────────────────── + +bool LiteGrip::send_mit_frame(double q, double kp, double kd, double dq, + double tau) { + if (bus_ == nullptr || !enabled_) { + return false; + } + return bus_->control_mit(q, kp, kd, dq, tau); +} + +bool LiteGrip::poll(double timeout_s) { + return bus_ != nullptr && bus_->poll(timeout_s); +} + +// ── motion ──────────────────────────────────────────────────────────────── + +bool LiteGrip::home() { + check_connected(); + check_enabled(); + return move_to(GripperParams::kPosClosedRad, std::nullopt, std::nullopt, 0.0, + 1.0); +} + +bool LiteGrip::open(std::optional kp, std::optional kd, + double duration) { + check_connected(); + check_enabled(); + return move_to(config_.pos_open_rad, kp, kd, 0.0, duration); +} + +bool LiteGrip::close(std::optional kp, std::optional kd, + std::optional force_n, double duration) { + check_connected(); + check_enabled(); + + if (force_n.has_value()) { + // Applying a grip force needs torque feed-forward, which needs force + // calibration — not verified in this SDK, and force control is out of v1 + // scope. Silently ignoring it would be worse than saying so. + std::fprintf(stderr, + "[litegrip] close(force_n=...) is not supported in this " + "version: applying a grip force needs torque feed-forward and " + "verified force calibration. The value is ignored.\n"); + } + return move_to(config_.pos_closed_rad, kp, kd, 0.0, duration); +} + +bool LiteGrip::goto_mm(double position_mm, std::optional kp, + std::optional kd, double duration) { + check_connected(); + check_enabled(); + // The motor angle decreases as the gripper opens. + const double position_rad = + config_.pos_closed_rad - position_mm / config_.rad_to_mm; + return goto_rad(position_rad, kp, kd, 0.0, 0.0, duration); +} + +bool LiteGrip::goto_rad(double position_rad, std::optional kp, + std::optional kd, double dq_target, + double tau_feedforward, double duration) { + check_connected(); + check_enabled(); + + const double effective_kp = kp.has_value() ? *kp : config_.kp; + const double effective_kd = kd.has_value() ? *kd : config_.kd; + + // Clamp into the commandable range first. The model layer's opening range can + // be wider than what the red lines allow, so without this clamp a "fully open" + // target would be refused wholesale instead of moving as far as it may. + const double lower = std::min(config_.pos_closed_rad, config_.pos_open_rad); + const double upper = std::max(config_.pos_closed_rad, config_.pos_open_rad); + const double clamped = std::max(lower, std::min(upper, position_rad)); + + // The gate may throw (LimitViolation / SafetyFault); those propagate, because + // the safety layer rejects rather than clamps, and swallowing them here would + // hide exactly the condition the caller needs to know about. + const Feedback feedback = measured_feedback(bus_->motor()); + const double q_safe = safety_->guard_motion_frame( + clamped, effective_kp, effective_kd, dq_target, tau_feedforward, + feedback.position, feedback.velocity, feedback.torque, "goto_rad"); + + return bus_->control_mit_stream(q_safe, effective_kp, effective_kd, duration, + dq_target, tau_feedforward); +} + +bool LiteGrip::move_to(double target_rad, std::optional kp, + std::optional kd, double tau_feedforward, + double duration) { + return goto_rad(target_rad, kp, kd, 0.0, tau_feedforward, duration); +} + +// ── calibration ─────────────────────────────────────────────────────────── +// +// All three routines deliberately drive the mechanism to its MECHANICAL stops, +// which lie OUTSIDE the software red lines. They therefore cannot go through +// guard_motion_frame, and they run inside the safety MODE that matches what they +// do (variant B's structure): zero-gravity for the hand-pushed routine, +// maintenance for the self-probing ones. The mode does not widen the red lines — +// it documents intent and controls whether an out-of-range FEEDBACK reading +// latches. See the plan's open item on calibration vs. the red lines. + +CalibrationData LiteGrip::calibrate(double kp, double kd, double step_rad, + double stall_delta, int stall_cycles, + int max_iter) { + check_connected(); + check_enabled(); + + auto scope = safety_->maintenance_scope("calibrate"); + + bus_->update_state(0.1); + const double initial = bus_->get_position(); + std::printf("[litegrip] calibrate: initial position %.4f rad\n", initial); + + const auto find_limit = [&](bool closing) -> double { + const double sign = closing ? 1.0 : -1.0; + bus_->update_state(0.05); + double current = bus_->get_position(); + double target = current; + int stall = 0; + + for (int i = 0; i < max_iter; ++i) { + target += sign * step_rad; + bus_->control_mit_stream(target, kp, kd, 0.3, 0.0, 0.0, 0.005); + bus_->update_state(0.1); + + const double measured = bus_->get_position(); + const double delta = std::fabs(measured - current); + std::printf("[litegrip] [%d] target=%+.3f pos=%.4f d=%.5f stall=%d\n", i, + target, measured, delta, stall); + + if (delta < stall_delta) { + if (++stall >= stall_cycles) { + std::printf("[litegrip] reached %s limit: %.6f rad\n", + closing ? "closed" : "open", measured); + return measured; + } + } else { + stall = 0; + } + current = measured; + } + std::printf("[litegrip] safety stop at the iteration cap: %.4f rad\n", + current); + return current; + }; + + // Back off first, so probing does not start against a stop. + bus_->control_mit_stream(initial + 0.2, 80.0, kd, 0.5); + bus_->update_state(0.1); + + const double closed = find_limit(true); + bus_->control_mit_stream(closed + 0.3, 80.0, kd, 0.5); + bus_->update_state(0.1); + const double opened = find_limit(false); + + const double travel = closed - opened; // closed is numerically larger + if (travel <= 0.0) { + throw CommError("calibration failed: the travel range is not positive"); + } + const double rad_to_mm = config_.max_stroke_mm / travel; + + CalibrationData result; + result.zero_position = closed; + result.max_position = opened; + result.travel_range = travel; + result.rad_to_mm = rad_to_mm; + result.motor_type = motor_type_name(GripperParams::kMotorType); + result.can_id = config_.can_id; + result.mst_id = mst_id_.value_or(0); + + config_.pos_closed_rad = result.zero_position; + config_.pos_open_rad = result.max_position; + config_.rad_to_mm = result.rad_to_mm; + + std::printf( + "[litegrip] calibrate: closed(0mm)=%.6f rad open=%.6f rad travel=%.6f " + "rad (%.1f mm) scale=%.1f mm/rad\n", + result.zero_position, result.max_position, result.travel_range, + result.travel_mm(), result.rad_to_mm); + return result; +} + +CalibrationData LiteGrip::calibrate_guided(double kp, double kd, + double step_rad, + double stall_delta, int stall_cycles, + int max_iter) { + check_connected(); + check_enabled(); + + auto scope = safety_->maintenance_scope("calibrate_guided"); + + const auto step_to_limit = [&](bool closing, const char* label) -> double { + std::printf("[litegrip] probing the %s limit; press Enter to confirm\n", + label); + bus_->update_state(0.05); + double current = bus_->get_position(); + int stall = 0; + + for (int i = 0; i < max_iter; ++i) { + const double target = current + (closing ? 1.0 : -1.0) * step_rad; + bus_->control_mit_stream(target, kp, kd, 0.3, 0.0, 0.0, 0.005); + bus_->update_state(0.1); + + const double measured = bus_->get_position(); + const double delta = std::fabs(measured - current); + std::printf("[litegrip] [%d] pos=%.4f d=%.5f stall=%d\n", i, measured, + delta, stall); + + if (delta < stall_delta) { + if (++stall >= stall_cycles) { + std::printf("[litegrip] detected the %s limit: %.6f rad\n", label, + measured); + return measured; + } + } else { + stall = 0; + } + current = measured; + } + std::printf("[litegrip] safety stop at the iteration cap: %.4f rad\n", + current); + return current; + }; + + const double opened = step_to_limit(false, "open"); + std::printf("[litegrip] backing off\n"); + bus_->control_mit_stream(opened - 0.15, 80.0, kd, 0.5, 0.0, 0.0, 0.005); + sleep_s(0.1); + const double closed = step_to_limit(true, "closed"); + + const double travel = closed - opened; + if (travel <= 0.0) { + throw CommError("calibration failed: the travel range is not positive"); + } + + CalibrationData result; + result.zero_position = closed; + result.max_position = opened; + result.travel_range = travel; + result.rad_to_mm = config_.max_stroke_mm / travel; + result.motor_type = motor_type_name(GripperParams::kMotorType); + result.can_id = config_.can_id; + result.mst_id = mst_id_.value_or(0); + + config_.pos_closed_rad = result.zero_position; + config_.pos_open_rad = result.max_position; + config_.rad_to_mm = result.rad_to_mm; + return result; +} + +CalibrationData LiteGrip::calibrate_manual(double duration, double settle_time, + double sample_interval) { + check_connected(); + check_enabled(); + + // Zero-torque streaming so the jaws can be moved by hand. + auto scope = safety_->zero_gravity_scope("calibrate_manual"); + + std::printf( + "[litegrip] hand-push calibration: the gripper is limp. Push the jaws " + "fully closed, then fully open, a few times. Recording for %.0f s.\n", + duration); + + double open_rad = std::numeric_limits::infinity(); + double close_rad = -std::numeric_limits::infinity(); + int samples = 0; + + const auto sample = [&]() { + safety_->guard_zero_torque_frame(0.0, 0.0, 0.0, 0.0, "calibrate_manual"); + bus_->control_mit(0.0, 0.0, 0.0, 0.0, 0.0); + bus_->poll(0.0); + const double position = bus_->get_position(); + // Ignore readings that cannot be real feedback. + if (std::fabs(position) < 50.0) { + ++samples; + open_rad = std::min(open_rad, position); + close_rad = std::max(close_rad, position); + } + }; + + const double deadline = monotonic_now() + duration; + while (monotonic_now() < deadline) { + sample(); + sleep_s(sample_interval); + } + + std::printf("[litegrip] settling for %.0f s\n", settle_time); + const double settle_deadline = monotonic_now() + settle_time; + while (monotonic_now() < settle_deadline) { + sample(); + sleep_s(sample_interval); + } + + // Leave the gripper holding wherever it is, with the configured gains. + bus_->control_mit_stream(bus_->get_position(), config_.kp, config_.kd, 0.05); + sleep_s(0.1); + + if (!std::isfinite(open_rad) || open_rad >= close_rad) { + throw CommError( + "calibration failed: no usable position range was captured — check " + "that the motor is enabled and producing feedback"); + } + + const double travel = close_rad - open_rad; + const double rad_to_mm = travel > 0.0 ? config_.max_stroke_mm / travel + : UnitConversion::kRadToMm; + + CalibrationData result; + result.zero_position = close_rad; + result.max_position = open_rad; + result.travel_range = travel; + result.rad_to_mm = rad_to_mm; + result.motor_type = motor_type_name(GripperParams::kMotorType); + result.can_id = config_.can_id; + result.mst_id = mst_id_.value_or(0); + + config_.pos_closed_rad = result.zero_position; + config_.pos_open_rad = result.max_position; + config_.rad_to_mm = result.rad_to_mm; + + std::printf( + "[litegrip] calibrate_manual: %d samples, closed=%.6f rad open=%.6f rad " + "travel=%.6f rad (%.1f mm) scale=%.1f mm/rad\n", + samples, result.zero_position, result.max_position, result.travel_range, + result.travel_mm(), result.rad_to_mm); + return result; +} + +std::string LiteGrip::save_calibration(std::optional path) { + const std::string destination = + path.has_value() ? *path : default_calibration_path(); + const int mst = mst_id_.value_or(0); + write_calibration_file(destination, config_, config_.can_id, mst, + motor_type_name(GripperParams::kMotorType)); + return destination; +} + +bool LiteGrip::load_calibration(std::optional path) { + const std::string user_path = path.has_value() ? *path : default_calibration_path(); + + // User file first, then the packaged read-only factory fallback. + std::optional calibration = + read_calibration_file(user_path); + if (!calibration.has_value()) { + calibration = read_calibration_file(factory_calibration_path()); + } + if (!calibration.has_value()) { + std::fprintf(stderr, + "[litegrip] no calibration found in %s or the factory " + "fallback; run a calibration first\n", + user_path.c_str()); + return false; + } + + config_.pos_closed_rad = calibration->zero_position_rad; + config_.pos_open_rad = calibration->max_position_rad; + config_.rad_to_mm = calibration->rad_to_mm; + + if (calibration->can_id.has_value()) { + config_.can_id = *calibration->can_id; + } + if (calibration->mst_id.has_value()) { + config_.mst_id = *calibration->mst_id; + mst_id_ = calibration->mst_id; + } + if (calibration->channel.has_value()) { + config_.can_channel = *calibration->channel; + } + if (calibration->canfd_mode.has_value()) { + config_.canfd_mode = *calibration->canfd_mode; + } + if (calibration->kp.has_value()) { + config_.kp = *calibration->kp; + } + if (calibration->kd.has_value()) { + config_.kd = *calibration->kd; + } + if (calibration->grasp_torque_threshold.has_value()) { + config_.grasp_torque_threshold = *calibration->grasp_torque_threshold; + } + return true; +} + +// ── state ───────────────────────────────────────────────────────────────── + +GripperState LiteGrip::get_state(bool wait) { + check_connected(); + + GripperState state; + if (bus_ == nullptr) { + return state; + } + + if (wait) { + bus_->update_state(0.05); + } else { + bus_->poll(0.0); + } + + can::MotorState* motor = bus_->motor(); + const double data_age = + motor != nullptr ? motor->data_age_s() + : std::numeric_limits::infinity(); + + if (wait && data_age > kStaleAfterS) { + std::fprintf(stderr, + "[litegrip] get_state(): no fresh status frame — the returned " + "values are cached/placeholder, not a measurement (a disabled " + "motor does not stream status frames)\n"); + } + + const double position_rad = bus_->get_position(); + state.position_rad = position_rad; + state.velocity_rad_s = bus_->get_velocity(); + state.torque_nm = bus_->get_torque(); + state.temperature_mos = bus_->get_temperature_mos(); + state.temperature_coil = bus_->get_temperature_coil(); + state.error_code = bus_->get_error(); + state.timestamp = wall_clock_now(); + state.data_age_s = data_age; + // The motor angle decreases as the gripper opens. + state.position_mm = + (config_.pos_closed_rad - position_rad) * config_.rad_to_mm; + state.force_n = state.torque_nm * config_.nm_to_n; + return state; +} + +bool LiteGrip::refresh_status(double timeout_s) { + check_connected(); + return bus_ != nullptr && bus_->refresh_status(timeout_s); +} + +double LiteGrip::get_position_mm() { return get_state().position_mm; } + +double LiteGrip::get_position_rad() { + check_connected(); + if (bus_ == nullptr) { + return 0.0; + } + bus_->update_state(0.05); + return bus_->get_position(); +} + +double LiteGrip::get_force() { return get_state().force_n; } + +double LiteGrip::get_torque() { + check_connected(); + if (bus_ == nullptr) { + return 0.0; + } + bus_->update_state(0.05); + return bus_->get_torque(); +} + +int LiteGrip::get_error() { + check_connected(); + if (bus_ == nullptr) { + return -1; + } + bus_->update_state(0.05); + return bus_->get_error(); +} + +std::pair LiteGrip::get_temperature() { + check_connected(); + if (bus_ == nullptr) { + return {0, 0}; + } + bus_->update_state(0.05); + return {bus_->get_temperature_mos(), bus_->get_temperature_coil()}; +} + +GripperInfo LiteGrip::get_info() const { + GripperInfo info; + info.motor_type = motor_type_name(GripperParams::kMotorType); + info.can_id = config_.can_id; + info.mst_id = mst_id_.value_or(0); + return info; +} + +bool LiteGrip::is_moving() { return get_state().is_moving(); } + +bool LiteGrip::is_grasped() { + const GripperState state = get_state(); + return std::fabs(state.torque_nm) > config_.grasp_torque_threshold; +} + +bool LiteGrip::wait_for_ready(double timeout) { + if (bus_ == nullptr) { + return false; + } + const double start = monotonic_now(); + while (monotonic_now() - start < timeout) { + bus_->update_state(0.05); + if (bus_->get_error() == 1 && !is_moving()) { + return true; + } + sleep_s(0.05); + } + return false; +} + +// ── parameter access ────────────────────────────────────────────────────── + +double LiteGrip::read_param(int rid, double timeout_s) { + check_connected(); + if (bus_ == nullptr) { + throw NotInitializedError("not connected"); + } + return bus_->read_param(rid, timeout_s); +} + +void LiteGrip::write_param(int rid, double value) { + check_connected(); + if (bus_ == nullptr) { + throw NotInitializedError("not connected"); + } + bus_->write_param(rid, value); +} + +// ── internal ────────────────────────────────────────────────────────────── + +void LiteGrip::check_connected() const { + if (!connected_) { + throw NotInitializedError( + "not connected — call connect() or use connect_raii()"); + } +} + +void LiteGrip::check_enabled() const { + if (!enabled_) { + throw NotInitializedError("not enabled — call init() or enable() first"); + } +} + +} // namespace litegrip diff --git a/src/hold_policy.cpp b/src/hold_policy.cpp new file mode 100644 index 0000000..739b994 --- /dev/null +++ b/src/hold_policy.cpp @@ -0,0 +1,88 @@ +// hold_policy.cpp — DefaultHoldPolicy: enable-and-hold, ported from +// LiteGripCAN._enable_and_hold / _hold_at_current. +// +// This is the behaviour the user intends to rewrite, so it is deliberately +// confined to this one file behind the HoldPolicy interface. + +#include "litegrip/hold_policy.hpp" + +#include +#include +#include + +#include "litegrip/bus.hpp" +#include "litegrip/constants.hpp" +#include "litegrip/exceptions.hpp" + +namespace litegrip { +namespace { + +double monotonic_now() noexcept { + return std::chrono::duration( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +void sleep_s(double seconds) { + if (seconds > 0.0) { + std::this_thread::sleep_for(std::chrono::duration(seconds)); + } +} + +} // namespace + +void DefaultHoldPolicy::init(GripperBus& bus, const GripperConfig& config) { + can::MotorState* motor = bus.motor(); + if (motor == nullptr) { + throw NotInitializedError("no gripper motor registered"); + } + + const std::uint64_t prev_rx = motor->rx_count(); + + // Zero-gain cover. Streamed rather than a single frame: everything else in + // this SDK streams for the same reason, and here one lost frame means the + // motor keeps running the stale target indefinitely rather than for ~10 ms. + bus.control_mit_stream(0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.005); + + const double deadline = monotonic_now() + DefaultParams::kInitTimeoutS; + bool fresh = false; + while (monotonic_now() < deadline) { + // Keep feeding the motor while we wait. It is ENABLED at this point, and an + // enabled motor that hears nothing for ~900 ms latches the 0xD comm-loss + // fault — which is exactly the fault this wait is supposed to detect, so + // silence here manufactures the failure it is looking for. The 0.05 s burst + // above is not long enough to cover a 2 s wait. + bus.control_mit(0.0, 0.0, 0.0, 0.0, 0.0); + bus.poll(0.01); + if (motor->rx_count() > prev_rx) { + fresh = true; + break; + } + sleep_s(0.005); + } + + if (!fresh) { + throw HardwareError("no motor feedback within the init timeout"); + } + + // rx_count only advances on a decoded status frame, so position() is a real + // reading from here on. + hold(bus, config); +} + +void DefaultHoldPolicy::hold(GripperBus& bus, const GripperConfig& config) { + can::MotorState* motor = bus.motor(); + if (motor == nullptr) { + return; + } + if (motor->rx_count() == 0) { + // Holding the constructor default (0.0) with a real gain would drive the + // gripper to a bogus target. Refuse instead. + std::fprintf(stderr, + "[litegrip] refusing to hold: no status frame decoded yet\n"); + return; + } + bus.control_mit_stream(motor->position(), config.kp, config.kd, 0.05); +} + +} // namespace litegrip diff --git a/src/json.cpp b/src/json.cpp new file mode 100644 index 0000000..e596d38 --- /dev/null +++ b/src/json.cpp @@ -0,0 +1,602 @@ +// json.cpp — minimal JSON parser/serializer. +// +// Scope is deliberately the SDK's own needs: the flat calibration object and +// the flat safety-limits baseline. Numbers are written with the shortest +// round-trip representation (std::to_chars, same idea as Python's repr) so a +// file written here reads back identically in the Python SDK and vice versa. + +#include "litegrip/json.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace litegrip::json { +namespace { + +constexpr double kIntegralPrintLimit = 1e15; + +void append_escaped(std::string& out, const std::string& text) { + out.push_back('"'); + for (const char raw : text) { + const auto ch = static_cast(raw); + switch (ch) { + case '"': + out += "\\\""; + break; + case '\\': + out += "\\\\"; + break; + case '\b': + out += "\\b"; + break; + case '\f': + out += "\\f"; + break; + case '\n': + out += "\\n"; + break; + case '\r': + out += "\\r"; + break; + case '\t': + out += "\\t"; + break; + default: + if (ch < 0x20) { + char buf[8]; + std::snprintf(buf, sizeof(buf), "\\u%04x", ch); + out += buf; + } else { + out.push_back(raw); + } + break; + } + } + out.push_back('"'); +} + +void append_number(std::string& out, double value) { + if (!std::isfinite(value)) { + // JSON has no NaN/Infinity. Emitting 0 is the least surprising choice for a + // value that should never occur; callers validate their own fields. + out += '0'; + return; + } + if (std::floor(value) == value && std::fabs(value) < kIntegralPrintLimit) { + char buf[32]; + std::snprintf(buf, sizeof(buf), "%.0f", value); + out += buf; + return; + } + char buf[40]; + const auto result = + std::to_chars(buf, buf + sizeof(buf), value); // shortest round-trip + if (result.ec == std::errc()) { + out.append(buf, result.ptr); + } else { + std::snprintf(buf, sizeof(buf), "%.17g", value); + out += buf; + } +} + +class Parser { + public: + Parser(const char* begin, const char* end) : cursor_(begin), end_(end) {} + + bool ok() const { return ok_; } + + Value parse_document() { + skip_ws(); + Value value = parse_value(); + skip_ws(); + if (cursor_ != end_) { + ok_ = false; // trailing garbage + } + return value; + } + + private: + void skip_ws() { + while (cursor_ != end_ && + (*cursor_ == ' ' || *cursor_ == '\t' || *cursor_ == '\n' || + *cursor_ == '\r')) { + ++cursor_; + } + } + + bool consume(char expected) { + if (cursor_ != end_ && *cursor_ == expected) { + ++cursor_; + return true; + } + return false; + } + + bool looking_at(const char* literal) { + const std::size_t length = std::strlen(literal); + if (static_cast(end_ - cursor_) < length) { + return false; + } + return std::strncmp(cursor_, literal, length) == 0; + } + + Value parse_value() { + if (cursor_ == end_) { + ok_ = false; + return Value{}; + } + switch (*cursor_) { + case '{': + return parse_object(); + case '[': + return parse_array(); + case '"': + return parse_string(); + case 't': + if (looking_at("true")) { + cursor_ += 4; + return Value(true); + } + break; + case 'f': + if (looking_at("false")) { + cursor_ += 5; + return Value(false); + } + break; + case 'n': + if (looking_at("null")) { + cursor_ += 4; + return Value(nullptr); + } + break; + default: + break; + } + if (*cursor_ == '-' || (*cursor_ >= '0' && *cursor_ <= '9')) { + return parse_number(); + } + ok_ = false; + return Value{}; + } + + Value parse_number() { + const char* start = cursor_; + if (cursor_ != end_ && *cursor_ == '-') { + ++cursor_; + } + while (cursor_ != end_ && *cursor_ >= '0' && *cursor_ <= '9') { + ++cursor_; + } + if (cursor_ != end_ && *cursor_ == '.') { + ++cursor_; + while (cursor_ != end_ && *cursor_ >= '0' && *cursor_ <= '9') { + ++cursor_; + } + } + if (cursor_ != end_ && (*cursor_ == 'e' || *cursor_ == 'E')) { + ++cursor_; + if (cursor_ != end_ && (*cursor_ == '+' || *cursor_ == '-')) { + ++cursor_; + } + while (cursor_ != end_ && *cursor_ >= '0' && *cursor_ <= '9') { + ++cursor_; + } + } + const std::string text(start, cursor_); + char* parse_end = nullptr; + const double value = std::strtod(text.c_str(), &parse_end); + if (parse_end == text.c_str()) { + ok_ = false; + return Value{}; + } + return Value(value); + } + + Value parse_string() { + if (!consume('"')) { + ok_ = false; + return Value{}; + } + std::string out; + while (cursor_ != end_) { + const char ch = *cursor_++; + if (ch == '"') { + return Value(std::move(out)); + } + if (ch != '\\') { + out.push_back(ch); + continue; + } + if (cursor_ == end_) { + break; + } + const char escape = *cursor_++; + switch (escape) { + case '"': + out.push_back('"'); + break; + case '\\': + out.push_back('\\'); + break; + case '/': + out.push_back('/'); + break; + case 'b': + out.push_back('\b'); + break; + case 'f': + out.push_back('\f'); + break; + case 'n': + out.push_back('\n'); + break; + case 'r': + out.push_back('\r'); + break; + case 't': + out.push_back('\t'); + break; + case 'u': { + if (end_ - cursor_ < 4) { + ok_ = false; + return Value{}; + } + unsigned int code = 0; + for (int i = 0; i < 4; ++i) { + const char digit = *cursor_++; + code <<= 4; + if (digit >= '0' && digit <= '9') { + code |= static_cast(digit - '0'); + } else if (digit >= 'a' && digit <= 'f') { + code |= static_cast(digit - 'a' + 10); + } else if (digit >= 'A' && digit <= 'F') { + code |= static_cast(digit - 'A' + 10); + } else { + ok_ = false; + return Value{}; + } + } + // Encode as UTF-8 (surrogate pairs are passed through unpaired; the + // SDK never emits them). + if (code < 0x80) { + out.push_back(static_cast(code)); + } else if (code < 0x800) { + out.push_back(static_cast(0xC0 | (code >> 6))); + out.push_back(static_cast(0x80 | (code & 0x3F))); + } else { + out.push_back(static_cast(0xE0 | (code >> 12))); + out.push_back(static_cast(0x80 | ((code >> 6) & 0x3F))); + out.push_back(static_cast(0x80 | (code & 0x3F))); + } + break; + } + default: + ok_ = false; + return Value{}; + } + } + ok_ = false; + return Value{}; + } + + Value parse_array() { + Value array = Value::make_array(); + if (!consume('[')) { + ok_ = false; + return array; + } + skip_ws(); + if (consume(']')) { + return array; + } + while (true) { + skip_ws(); + array.push_back(parse_value()); + if (!ok_) { + return array; + } + skip_ws(); + if (consume(',')) { + continue; + } + if (consume(']')) { + return array; + } + ok_ = false; + return array; + } + } + + Value parse_object() { + Value object = Value::make_object(); + if (!consume('{')) { + ok_ = false; + return object; + } + skip_ws(); + if (consume('}')) { + return object; + } + while (true) { + skip_ws(); + if (cursor_ == end_ || *cursor_ != '"') { + ok_ = false; + return object; + } + Value key = parse_string(); + if (!ok_) { + return object; + } + skip_ws(); + if (!consume(':')) { + ok_ = false; + return object; + } + skip_ws(); + Value value = parse_value(); + if (!ok_) { + return object; + } + object.set(key.as_string(), std::move(value)); + skip_ws(); + if (consume(',')) { + continue; + } + if (consume('}')) { + return object; + } + ok_ = false; + return object; + } + } + + const char* cursor_; + const char* end_; + bool ok_ = true; +}; + +} // namespace + +Value Value::make_array() { + Value value; + value.type_ = Type::kArray; + return value; +} + +Value Value::make_object() { + Value value; + value.type_ = Type::kObject; + return value; +} + +bool Value::as_bool(bool fallback) const noexcept { + return type_ == Type::kBool ? bool_ : fallback; +} + +double Value::as_number(double fallback) const noexcept { + return type_ == Type::kNumber ? number_ : fallback; +} + +std::string Value::as_string(const std::string& fallback) const { + return type_ == Type::kString ? string_ : fallback; +} + +const Value* Value::find(const std::string& key) const noexcept { + if (type_ != Type::kObject) { + return nullptr; + } + for (const auto& entry : object_) { + if (entry.first == key) { + return &entry.second; + } + } + return nullptr; +} + +bool Value::contains(const std::string& key) const noexcept { + return find(key) != nullptr; +} + +double Value::get_number(const std::string& key, double fallback) const noexcept { + const Value* value = find(key); + return value != nullptr ? value->as_number(fallback) : fallback; +} + +int Value::get_int(const std::string& key, int fallback) const noexcept { + const Value* value = find(key); + if (value == nullptr || value->type() != Type::kNumber) { + return fallback; + } + return static_cast(value->as_number(static_cast(fallback))); +} + +bool Value::get_bool(const std::string& key, bool fallback) const noexcept { + const Value* value = find(key); + return value != nullptr ? value->as_bool(fallback) : fallback; +} + +std::string Value::get_string(const std::string& key, + const std::string& fallback) const { + const Value* value = find(key); + return value != nullptr ? value->as_string(fallback) : fallback; +} + +void Value::set(const std::string& key, Value value) { + if (type_ != Type::kObject) { + type_ = Type::kObject; + object_.clear(); + } + for (auto& entry : object_) { + if (entry.first == key) { + entry.second = std::move(value); + return; + } + } + object_.emplace_back(key, std::move(value)); +} + +std::size_t Value::size() const noexcept { + if (type_ == Type::kArray) { + return array_.size(); + } + if (type_ == Type::kObject) { + return object_.size(); + } + return 0; +} + +void Value::push_back(Value value) { + if (type_ != Type::kArray) { + type_ = Type::kArray; + array_.clear(); + } + array_.push_back(std::move(value)); +} + +const Value& Value::operator[](std::size_t index) const { + return array_.at(index); +} + +void Value::dump_to(std::string& out, int indent, int depth) const { + const bool pretty = indent > 0; + const std::string pad = + pretty ? std::string(static_cast(indent * (depth + 1)), ' ') + : std::string(); + const std::string close_pad = + pretty ? std::string(static_cast(indent * depth), ' ') + : std::string(); + + switch (type_) { + case Type::kNull: + out += "null"; + return; + case Type::kBool: + out += bool_ ? "true" : "false"; + return; + case Type::kNumber: + append_number(out, number_); + return; + case Type::kString: + append_escaped(out, string_); + return; + case Type::kArray: { + if (array_.empty()) { + out += "[]"; + return; + } + out.push_back('['); + for (std::size_t i = 0; i < array_.size(); ++i) { + if (i != 0) { + out.push_back(','); + } + if (pretty) { + out.push_back('\n'); + out += pad; + } + array_[i].dump_to(out, indent, depth + 1); + } + if (pretty) { + out.push_back('\n'); + out += close_pad; + } + out.push_back(']'); + return; + } + case Type::kObject: { + if (object_.empty()) { + out += "{}"; + return; + } + out.push_back('{'); + for (std::size_t i = 0; i < object_.size(); ++i) { + if (i != 0) { + out.push_back(','); + } + if (pretty) { + out.push_back('\n'); + out += pad; + } + append_escaped(out, object_[i].first); + out.push_back(':'); + if (pretty) { + out.push_back(' '); + } + object_[i].second.dump_to(out, indent, depth + 1); + } + if (pretty) { + out.push_back('\n'); + out += close_pad; + } + out.push_back('}'); + return; + } + } +} + +std::string Value::dump(int indent) const { + std::string out; + dump_to(out, indent, 0); + return out; +} + +std::optional Value::parse(const std::string& text) { + Parser parser(text.data(), text.data() + text.size()); + Value value = parser.parse_document(); + if (!parser.ok()) { + return std::nullopt; + } + return value; +} + +std::optional Value::parse_file(const std::string& path) { + std::ifstream input(path, std::ios::binary); + if (!input) { + return std::nullopt; + } + std::ostringstream buffer; + buffer << input.rdbuf(); + return parse(buffer.str()); +} + +bool Value::write_file(const std::string& path, int indent) const { + // Create parent directories, mirroring the Python SDK (which used + // os.makedirs(parent, exist_ok=True)). + const std::size_t slash = path.find_last_of('/'); + if (slash != std::string::npos && slash > 0) { + const std::string parent = path.substr(0, slash); + std::string accumulated; + std::size_t start = 0; + while (start <= parent.size()) { + const std::size_t next = parent.find('/', start); + const std::string component = + parent.substr(start, next == std::string::npos ? std::string::npos + : next - start); + if (!component.empty()) { + accumulated += "/" + component; + ::mkdir(accumulated.c_str(), 0755); + } + if (next == std::string::npos) { + break; + } + start = next + 1; + } + } + + std::ofstream output(path, std::ios::binary | std::ios::trunc); + if (!output) { + return false; + } + output << dump(indent); + return output.good(); +} + +} // namespace litegrip::json diff --git a/src/safety.cpp b/src/safety.cpp new file mode 100644 index 0000000..d8f66d1 --- /dev/null +++ b/src/safety.cpp @@ -0,0 +1,762 @@ +// safety.cpp — the safety core: limits, guards, modes, latch, watchdog, +// baseline loading. +// +// Ported from the safety-aware SDK variant's safety_limits.py (core only — the +// contact/force/stall physics models are out of scope, see the plan D5). +// +// The criteria ORDER in guard_motion_frame is part of the safety argument and +// is reproduced deliberately: measured position first (because "already outside +// the red lines" must reject regardless of where the target points), then the +// mechanical range, then the red lines, then quantisation, then the command +// ceilings, then the deceleration zone, then the measured-torque check. + +#include "litegrip/safety.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "litegrip/can/protocol.hpp" +#include "litegrip/exceptions.hpp" +#include "litegrip/json.hpp" + +namespace litegrip { +namespace { + +bool file_exists(const std::string& path) { + struct stat info {}; + return ::stat(path.c_str(), &info) == 0 && S_ISREG(info.st_mode); +} + +const char* env_or_null(const char* name) { + const char* value = std::getenv(name); + return (value != nullptr && value[0] != '\0') ? value : nullptr; +} + +bool finite(double value) { return std::isfinite(value); } + +/// "wider when larger" direction per TemporaryParams field. +/// +/// Every ceiling field is "larger = more permissive" except t_comm_s and +/// safety_reserve_rad, which feed the stopping-distance model and are therefore +/// "smaller = more permissive". Getting this backwards would reject a genuine +/// tightening (e.g. raising t_comm) and, worse, accept a loosening. +struct ParamField { + const char* name; + bool wider_when_larger; + double TemporaryParams::*member; +}; + +const ParamField kParamFields[] = { + {"a_max_rad_s2", true, &TemporaryParams::a_max_rad_s2}, + {"t_comm_s", false, &TemporaryParams::t_comm_s}, + {"safety_reserve_rad", false, &TemporaryParams::safety_reserve_rad}, + {"kp_max", true, &TemporaryParams::kp_max}, + {"kd_max", true, &TemporaryParams::kd_max}, + {"tau_max_nm", true, &TemporaryParams::tau_max_nm}, + {"max_motion_duration_s", true, &TemporaryParams::max_motion_duration_s}, + {"recovery_kp_max", true, &TemporaryParams::recovery_kp_max}, + {"recovery_dq_max", true, &TemporaryParams::recovery_dq_max}, + {"recovery_tau_max", true, &TemporaryParams::recovery_tau_max}, +}; + +/// Require a finite, strictly positive field. +void require_positive(double value, const std::string& name, + const std::string& source) { + if (!finite(value)) { + throw SafetyConfigError(source + ": " + name + " is not a finite number"); + } + if (value <= 0.0) { + throw SafetyConfigError(source + ": " + name + " must be positive"); + } +} + +/// Candidate locations for a data file shipped with the calibration/baseline +/// data. Mirrors factory_calibration_path(): no machine-dependent path is baked +/// into the source. +std::string find_data_file(const std::string& name) { + std::vector candidates; + if (const char* data_dir = env_or_null("LITEGRIP_DATA_DIR")) { + candidates.push_back(std::string(data_dir) + "/calibration/" + name); + candidates.push_back(std::string(data_dir) + "/" + name); + } + candidates.push_back("calibration/" + name); + candidates.push_back("../calibration/" + name); + candidates.push_back(name); + candidates.push_back("../" + name); + + for (const std::string& candidate : candidates) { + if (file_exists(candidate)) { + return candidate; + } + } + return candidates.front(); +} + +} // namespace + +// ── TemporaryParams ─────────────────────────────────────────────────────── + +void TemporaryParams::validate() const { + for (const ParamField& field : kParamFields) { + require_positive(this->*(field.member), field.name, "temporary parameter"); + } +} + +// ── SafetyLimits ────────────────────────────────────────────────────────── + +void SafetyLimits::validate() const { + if (!enabled) { + // There is no legitimate scenario that needs the red lines switched off: + // hand-push calibration needs zero-torque frames, and maintenance mode only + // lowers the ceilings. An instance with enabled == false has *no* bounds, + // so it is refused at construction rather than relied upon downstream. + throw SafetyConfigError( + "red-line master switch enabled=false — this is not a narrower limit, " + "it is no limit at all"); + } + + for (const double value : + {mech_min_rad, mech_max_rad, red_min_rad, red_max_rad}) { + if (!finite(value)) { + throw SafetyConfigError("safety limit is not a finite number"); + } + } + + if (!(mech_min_rad < red_min_rad && red_min_rad < red_max_rad && + red_max_rad < mech_max_rad)) { + throw SafetyConfigError( + "safety limit ordering must be mech_min < red_min < red_max < mech_max"); + } + + params.validate(); +} + +void SafetyLimits::assert_no_wider_than(const SafetyLimits& baseline) const { + if (!enabled) { + throw SafetyConfigError( + "enabled=false is not a narrower limit — refuse"); + } + + if (mech_min_rad < baseline.mech_min_rad) { + throw SafetyConfigError( + "mechanical lower bound is wider than the packaged baseline — limits " + "may only be tightened"); + } + if (red_min_rad < baseline.red_min_rad) { + throw SafetyConfigError( + "red line (open side) is wider than the packaged baseline — limits may " + "only be tightened"); + } + if (mech_max_rad > baseline.mech_max_rad) { + throw SafetyConfigError( + "mechanical upper bound is wider than the packaged baseline — limits " + "may only be tightened"); + } + if (red_max_rad > baseline.red_max_rad) { + throw SafetyConfigError( + "red line (closed side) is wider than the packaged baseline — limits " + "may only be tightened"); + } + + for (const ParamField& field : kParamFields) { + const double mine = this->params.*(field.member); + const double base = baseline.params.*(field.member); + const bool too_wide = + field.wider_when_larger ? (mine > base) : (mine < base); + if (too_wide) { + throw SafetyConfigError(std::string("parameter ") + field.name + + " is wider than the packaged baseline — limits " + "may only be tightened"); + } + } +} + +SafetyLimits SafetyLimits::sanitized_copy(const std::string& source) const { + SafetyLimits copy{*this}; + for (const double value : + {copy.mech_min_rad, copy.mech_max_rad, copy.red_min_rad, + copy.red_max_rad}) { + if (!finite(value)) { + throw SafetyConfigError(source + ": safety limit is not finite"); + } + } + for (const ParamField& field : kParamFields) { + require_positive(copy.params.*(field.member), field.name, source); + } + return copy; +} + +double SafetyLimits::quantize_toward_interior(double q, double red_min, + double red_max) { + // The 16-bit MIT field cannot represent every value, and float_to_uint + // truncates toward zero, so the decoded value is always <= the commanded one. + // Step inward until the DECODED value is inside the interval. + std::uint32_t code = + can::float_to_uint(q, -kProtocolQMaxRad, kProtocolQMaxRad, 16); + for (int attempt = 0; attempt < 4; ++attempt) { + const double decoded = + can::uint_to_float(code, -kProtocolQMaxRad, kProtocolQMaxRad, 16); + if (decoded >= red_min && decoded <= red_max) { + return decoded; + } + if (decoded < red_min) { + ++code; // step toward the closed side (numerically larger) + } else { + --code; // step toward the open side (numerically smaller) + } + } + throw LimitViolation( + "target could not be quantized into the red lines — command not sent"); +} + +double SafetyLimits::v_allow(double q_act, double direction) const { + if (direction == 0.0 || !finite(q_act)) { + return params.recovery_dq_max; // no motion intent, no speed limit + } + + const double distance = + direction > 0.0 ? (red_max_rad - q_act) : (q_act - red_min_rad); + const double margin = direction > 0.0 ? (mech_max_rad - red_max_rad) + : (red_min_rad - mech_min_rad); + + const double limit = std::min(margin, distance); + const double available = limit - params.safety_reserve_rad; + if (available <= 0.0) { + return 0.0; // no margin left: do not move further in this direction + } + + const double at = params.a_max_rad_s2 * params.t_comm_s; + const double v = + -at + std::sqrt(at * at + 2.0 * params.a_max_rad_s2 * available); + + // Overflow guard: the closed form can produce inf/nan for absurd t_comm, and + // the caller's test is `|dq| > v_lim` — for inf and nan that is always false, + // which would silently short-circuit the deceleration gate entirely. Fail + // closed instead. + if (!finite(v)) { + return 0.0; + } + return v; +} + +bool SafetyLimits::contains_red(double q) const { + return finite(q) && q >= red_min_rad && q <= red_max_rad; +} + +bool SafetyLimits::contains_mech(double q) const { + return finite(q) && q >= mech_min_rad && q <= mech_max_rad; +} + +std::pair SafetyLimits::mm_range(double pos_closed_rad, + double rad_to_mm) const { + return {rad_to_mm * (pos_closed_rad - red_max_rad), + rad_to_mm * (pos_closed_rad - red_min_rad)}; +} + +// ── strict numeric boundary ─────────────────────────────────────────────── + +double normalize_scalar(double value, const std::string& name, + const std::string& source) { + if (!finite(value)) { + throw LimitViolation(source + ": " + name + + " is not a finite number — command not sent"); + } + return value; +} + +// ── SafetyGuard ─────────────────────────────────────────────────────────── + +SafetyGuard::SafetyGuard(const SafetyLimits& limits) + : limits_(checked_limits(limits, "SafetyGuard")) {} + +FrameMode SafetyGuard::mode() const noexcept { return mode_stack_.back(); } + +void SafetyGuard::push_mode(FrameMode mode, const std::string& reason) { + if (mode == FrameMode::kNormal) { + throw SafetyConfigError( + "kNormal cannot be pushed — it only exists as the stack bottom"); + } + mode_stack_.push_back(mode); + std::fprintf(stderr, "[litegrip] entering a safety mode: %s\n", + reason.empty() ? "(no reason given)" : reason.c_str()); +} + +FrameMode SafetyGuard::pop_mode() { + if (mode_stack_.size() <= 1) { + throw SafetyConfigError("safety mode stack is already at the bottom"); + } + const FrameMode popped = mode_stack_.back(); + mode_stack_.pop_back(); + return popped; +} + +SafetyGuard::ModeScope::ModeScope(SafetyGuard& owner, FrameMode mode, + const std::string& reason) + : owner_(&owner) { + owner_->push_mode(mode, reason); +} + +SafetyGuard::ModeScope::~ModeScope() { + if (owner_ == nullptr) { + return; + } + try { + owner_->pop_mode(); + } catch (const LiteGripError&) { + // A destructor must not throw. Reaching here means the stack was already + // unwound, which the push in the constructor makes impossible in practice. + } +} + +SafetyGuard::ModeScope SafetyGuard::zero_gravity_scope(const std::string& reason) { + return ModeScope(*this, FrameMode::kZeroGravity, reason); +} + +SafetyGuard::ModeScope SafetyGuard::maintenance_scope(const std::string& reason) { + return ModeScope(*this, FrameMode::kMaintenance, reason); +} + +SafetyGuard::ModeScope SafetyGuard::recovery_scope(const std::string& reason) { + return ModeScope(*this, FrameMode::kRecovery, reason); +} + +void SafetyGuard::latch_fault(const std::string& reason) { + if (!fault_latched_) { + fault_latched_ = true; + latch_reason_ = reason; + std::fprintf(stderr, + "[litegrip] SAFETY FAULT LATCHED: %s — all further drive " + "commands are refused until clear_safety_latch()\n", + reason.c_str()); + } +} + +void SafetyGuard::clear_safety_latch() { + fault_latched_ = false; + latch_reason_.clear(); + armed_ = false; // re-arms once the measured position is back inside +} + +void SafetyGuard::disarm_watchdog(const std::string& reason) { + if (armed_) { + std::fprintf(stderr, + "[litegrip] feedback watchdog disarmed: %s (re-arms inside " + "the red lines)\n", + reason.c_str()); + } + armed_ = false; +} + +void SafetyGuard::check_not_latched(const std::string& source) const { + if (fault_latched_) { + throw SafetyFault(source + + ": safety fault is latched (" + latch_reason_ + + ") — command refused; call clear_fault() after removing " + "the cause"); + } +} + +double SafetyGuard::require_act_within_red(std::optional q_act, + const std::string& source) { + if (!q_act.has_value()) { + // Distinguish "never received feedback" from "received a bad value": here + // we do not know where the gripper is, so no motion frame may be sent. + throw SafetyFault(source + + ": no valid feedback (measured position unknown) — " + "motion frames are refused"); + } + const double q = normalize_scalar(*q_act, "measured position", source); + if (q < limits_.red_min_rad || q > limits_.red_max_rad) { + // Even a target pointing inward is refused: the restricted recovery channel + // exists for that, and it enforces its own kp/dq/tau ceilings which an + // ordinary motion frame does not. + throw LimitViolation( + source + + ": measured position is already outside the software red lines — " + "ordinary motion is refused (only the recovery channel may move it " + "back inside)"); + } + return q; +} + +double SafetyGuard::guard_motion_frame(double q_target, double kp, double kd, + double dq_target, double tau_feedforward, + std::optional q_act, + std::optional dq_act, + std::optional tau_act, + const std::string& source) { + check_not_latched(source); + + // Strict numeric boundary first: every criterion below compares these + // values, so a non-finite one must never reach a comparison. + q_target = normalize_scalar(q_target, "target angle", source); + kp = normalize_scalar(kp, "kp", source); + kd = normalize_scalar(kd, "kd", source); + dq_target = normalize_scalar(dq_target, "target velocity", source); + tau_feedforward = normalize_scalar(tau_feedforward, "feed-forward torque", + source); + + if (!limits_.enabled) { + return q_target; + } + if (mode() == FrameMode::kZeroGravity) { + throw LimitViolation(source + + ": zero-gravity mode allows only zero-torque frames " + "— command not sent"); + } + + // Measured position is the precondition, and it comes BEFORE the target + // checks so that "the target points inward" cannot look permitted. + const double measured = require_act_within_red(q_act, source); + + if (q_target < limits_.mech_min_rad || q_target > limits_.mech_max_rad) { + throw SafetyFault(source + + ": target is outside the mechanical observed range — " + "command not sent"); + } + if (q_target < limits_.red_min_rad || q_target > limits_.red_max_rad) { + throw LimitViolation(source + + ": target is outside the software red lines — " + "command not sent"); + } + const double q_safe = SafetyLimits::quantize_toward_interior( + q_target, limits_.red_min_rad, limits_.red_max_rad); + + if (!dq_act.has_value() || !finite(*dq_act)) { + throw SafetyFault(source + + ": no valid velocity feedback — motion frames are " + "refused"); + } + + const TemporaryParams& p = limits_.params; + + if (kp < 0.0 || kp > p.kp_max) { + throw LimitViolation(source + ": kp is outside the allowed range"); + } + if (kd < 0.0 || kd > p.kd_max) { + throw LimitViolation(source + ": kd is outside the allowed range"); + } + if (std::fabs(tau_feedforward) > p.tau_max_nm) { + throw LimitViolation(source + ": feed-forward torque exceeds the ceiling"); + } + + if (dq_target != 0.0) { + const double v_limit = limits_.v_allow(measured, dq_target); + if (std::fabs(dq_target) > v_limit) { + throw LimitViolation(source + + ": target velocity exceeds what the deceleration " + "zone allows here — command not sent"); + } + } + + // Final total torque, judged from the MEASURED torque: the motor's reported + // tau already includes kp*dq + kd*ddq + tau_ff, whereas estimating kp*error + // would misjudge any large legitimate step from far away. Only a frame that + // keeps pushing in the same direction while already over the limit is + // refused; a frame that unloads is always allowed. + if (tau_act.has_value() && finite(*tau_act) && + std::fabs(*tau_act) > p.tau_max_nm) { + const bool pushing_further = + (*tau_act > 0.0 && (tau_feedforward > 0.0 || q_safe > measured)) || + (*tau_act < 0.0 && (tau_feedforward < 0.0 || q_safe < measured)); + if (pushing_further) { + throw LimitViolation( + source + + ": measured total torque already exceeds the ceiling and this frame " + "still pushes the same way — command not sent (probably at a " + "mechanical stop or gripping too hard)"); + } + } + + return q_safe; +} + +double SafetyGuard::guard_recovery_frame(double q_target, double kp, double kd, + double dq_target, double tau_feedforward, + std::optional q_act, + std::optional /*dq_act*/, + const std::string& source) { + check_not_latched(source); + + q_target = normalize_scalar(q_target, "target angle", source); + kp = normalize_scalar(kp, "kp", source); + kd = normalize_scalar(kd, "kd", source); + dq_target = normalize_scalar(dq_target, "target velocity", source); + tau_feedforward = normalize_scalar(tau_feedforward, "feed-forward torque", + source); + + if (!q_act.has_value()) { + throw SafetyFault(source + + ": no valid feedback — recovery motion is refused"); + } + const double measured = normalize_scalar(*q_act, "measured position", source); + + if (measured < limits_.mech_min_rad || measured > limits_.mech_max_rad) { + throw SafetyFault(source + + ": measured position is outside the mechanical observed " + "range — automatic recovery refused, inspect the " + "mechanism"); + } + if (limits_.red_min_rad <= measured && measured <= limits_.red_max_rad) { + throw LimitViolation(source + + ": measured position is already inside the red lines, " + "no recovery needed"); + } + + // Direction: only toward the inside. + if (measured > limits_.red_max_rad && dq_target > 0.0) { + throw LimitViolation(source + + ": position is past the closed red line, further " + "closing is refused"); + } + if (measured < limits_.red_min_rad && dq_target < 0.0) { + throw LimitViolation(source + + ": position is past the open red line, further " + "opening is refused"); + } + if (measured > limits_.red_max_rad && q_target > measured) { + throw LimitViolation(source + + ": target is further outside than the current " + "position — command not sent"); + } + if (measured < limits_.red_min_rad && q_target < measured) { + throw LimitViolation(source + + ": target is further outside than the current " + "position — command not sent"); + } + + const TemporaryParams& p = limits_.params; + if (kp < 0.0 || kp > p.recovery_kp_max) { + throw LimitViolation(source + ": recovery kp exceeds its ceiling"); + } + if (std::fabs(dq_target) > p.recovery_dq_max) { + throw LimitViolation(source + ": recovery velocity exceeds its ceiling"); + } + if (std::fabs(tau_feedforward) > p.recovery_tau_max) { + throw LimitViolation(source + ": recovery torque exceeds its ceiling"); + } + if (q_target < limits_.mech_min_rad || q_target > limits_.mech_max_rad) { + throw SafetyFault(source + + ": recovery target is outside the mechanical observed " + "range — command not sent"); + } + return q_target; +} + +void SafetyGuard::guard_zero_torque_frame(double kp, double kd, double dq, + double tau, + const std::string& source) { + // Bypassing the red lines is the entire point of a zero-torque frame, and it + // is only legitimate while all four fields are exactly zero — any non-zero + // value could produce motion and must go through the motion gate instead. + const std::pair fields[] = { + {"kp", kp}, {"kd", kd}, {"dq", dq}, {"tau", tau}}; + for (const auto& field : fields) { + const double value = normalize_scalar(field.second, field.first, source); + if (value != 0.0) { + throw LimitViolation( + source + ": a zero-torque frame requires kp=kd=dq=tau=0, but " + + field.first + " is non-zero — refused"); + } + } +} + +double SafetyGuard::zero_torque_q(std::optional q_act, + bool has_feedback) { + if (has_feedback && q_act.has_value()) { + double value = 0.0; + bool usable = true; + try { + value = normalize_scalar(*q_act, "measured position", "zero_torque_q"); + } catch (const LimitViolation&) { + usable = false; + } + if (usable && std::fabs(value) <= kProtocolQMaxRad) { + return value; + } + } + // The fallback is a constant that depends on no feedback at all, so it cannot + // block the stop/disable paths — which is exactly when it is needed. + return kZeroTorqueQFallback; +} + +void SafetyGuard::guard_feedback_position(double q, const std::string& source) { + if (!finite(q)) { + latch_fault(source + ": feedback angle is invalid"); + throw SafetyFault(source + ": feedback angle is invalid"); + } + // Once latched (or disabled) there is nothing more to report; reading state + // must stay possible, otherwise the recovery direction cannot be chosen. + if (!limits_.enabled || fault_latched_) { + return; + } + + if (limits_.red_min_rad <= q && q <= limits_.red_max_rad) { + arm_watchdog(); // back inside: recovery is complete + return; + } + + if (q < limits_.mech_min_rad || q > limits_.mech_max_rad) { + const std::string reason = + source + + ": measured position is outside the mechanical observed range — it may " + "have been forced past the stop or the encoder is faulty"; + latch_fault(reason); + throw SafetyFault(reason); + } + + const FrameMode current = mode(); + if (current == FrameMode::kZeroGravity || current == FrameMode::kRecovery) { + // Hand-pushing and recovery genuinely operate outside the red lines; + // latching here would make those flows impossible to complete. + std::fprintf(stderr, + "[litegrip] measured position crossed a red line during a " + "mode that permits it (%s) — warning only\n", + source.c_str()); + return; + } + + if (!armed_) { + return; // just cleared, or on the way back in + } + + const std::string reason = + source + ": measured position crossed the software red lines"; + latch_fault(reason); + throw SafetyFault(reason); +} + +// ── baseline loading ────────────────────────────────────────────────────── + +const SafetyBaselineVersion kSafetyBaselineVersions[] = { + {"3.5", "safety_limits_350.json"}, + {"0.25", "safety_limits_025.json"}, +}; +const std::size_t kSafetyBaselineVersionsCount = + sizeof(kSafetyBaselineVersions) / sizeof(kSafetyBaselineVersions[0]); + +std::string safety_baseline_path(const std::string& version) { + for (std::size_t i = 0; i < kSafetyBaselineVersionsCount; ++i) { + if (version == kSafetyBaselineVersions[i].version) { + return find_data_file(kSafetyBaselineVersions[i].file_name); + } + } + throw SafetyConfigError("unknown safety baseline version '" + version + "'"); +} + +std::optional load_safety_limits( + const std::optional& path) { + std::vector candidates; + if (path.has_value()) { + candidates.push_back(*path); + } else { + candidates.push_back("safety_limits.json"); + candidates.push_back("../safety_limits.json"); + candidates.push_back("calibration/safety_limits.json"); + candidates.push_back("../calibration/safety_limits.json"); + } + + std::string source; + std::optional document; + for (const std::string& candidate : candidates) { + if (!file_exists(candidate)) { + continue; + } + document = json::Value::parse_file(candidate); + if (!document.has_value()) { + throw SafetyConfigError("safety limits config is not valid JSON: " + + candidate); + } + source = candidate; + break; + } + if (!document.has_value()) { + // Contract: the caller must handle "no config file" explicitly rather than + // being handed some shared fallback object. + return std::nullopt; + } + + for (const char* required : {"mechanical_observed_min_rad", + "mechanical_observed_max_rad", + "red_open_limit_rad", "red_close_limit_rad"}) { + if (!document->contains(required)) { + throw SafetyConfigError("safety limits config " + source + + " is missing the required field " + required); + } + } + + if (document->contains("red_line_enabled") && + !document->get_bool("red_line_enabled", true)) { + throw SafetyConfigError( + "safety limits config " + source + + " sets red_line_enabled=false — the red lines cannot be disabled; a " + "config file may only NARROW the interval"); + } + + SafetyLimits candidate; + candidate.mech_min_rad = + document->get_number("mechanical_observed_min_rad", kPackageMechMin); + candidate.mech_max_rad = + document->get_number("mechanical_observed_max_rad", kPackageMechMax); + candidate.red_min_rad = + document->get_number("red_open_limit_rad", kPackageRedMin); + candidate.red_max_rad = + document->get_number("red_close_limit_rad", kPackageRedMax); + candidate.enabled = document->get_bool("red_line_enabled", true); + if (document->contains("hard_torque_limit_nm")) { + candidate.params.tau_max_nm = + document->get_number("hard_torque_limit_nm", kPackageTauMaxNm); + } + + return checked_limits(candidate, "safety limits config " + source); +} + +SafetyLimits load_safety_baseline(const std::optional& version) { + const std::string wanted = + version.has_value() ? *version : std::string(kDefaultSafetyBaseline); + + // A string containing a path separator is taken as an explicit file, not a + // version name. That lets a deployment pin an exact file (and lets tests point + // at one) without the version table having to know about it — while a bare + // version still resolves through the table, so the rollback stays auditable. + const std::string path = wanted.find('/') != std::string::npos + ? wanted + : safety_baseline_path(wanted); + if (!file_exists(path)) { + // Fail-closed: silently falling back would make "the config was lost" look + // identical to "the config is correct", and the node would run with a + // torque ceiling nobody confirmed. + throw SafetyConfigError("safety baseline '" + wanted + + "' file does not exist: " + path + + " — refusing to fall back to any default"); + } + const auto limits = load_safety_limits(path); + if (!limits.has_value()) { + throw SafetyConfigError("safety baseline '" + wanted + + "' could not be loaded: " + path); + } + return *limits; +} + +SafetyLimits canonical_baseline() { + // Freshly constructed per call, so no caller can widen the comparison + // reference by mutating a shared object. + return SafetyLimits{}; +} + +SafetyLimits checked_limits(const SafetyLimits& candidate, + const std::string& source) { + SafetyLimits copy = candidate.sanitized_copy(source); + copy.validate(); + copy.assert_no_wider_than(canonical_baseline()); + return copy; +} + +} // namespace litegrip diff --git a/src/version.cpp b/src/version.cpp new file mode 100644 index 0000000..1954735 --- /dev/null +++ b/src/version.cpp @@ -0,0 +1,13 @@ +// litegrip_cpp — ROS-agnostic C++ SDK for the LiteGrip adaptive two-finger gripper. + +#include "litegrip/version.hpp" + +namespace litegrip { + +const char* version() noexcept { return LITEGRIP_CPP_VERSION_STRING; } + +int version_major() noexcept { return LITEGRIP_CPP_VERSION_MAJOR; } +int version_minor() noexcept { return LITEGRIP_CPP_VERSION_MINOR; } +int version_patch() noexcept { return LITEGRIP_CPP_VERSION_PATCH; } + +} // namespace litegrip diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..2bf5709 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,42 @@ +# Tests use no third-party framework (the library is zero-dependency, and so is +# its test setup): each test file is a standalone executable that returns +# non-zero on failure and is registered with CTest. + +# T1: version smoke test. T2–T4 add the protocol / model / safety parity tests +# (golden vectors lifted from litegrip_driver/tests/test_protocol.py). +set(LITEGRIP_CPP_TESTS + test_version.cpp + test_headers.cpp + test_protocol.cpp + test_motor.cpp + test_golden.cpp + test_transport.cpp + test_json.cpp + test_calibration.cpp + test_bus.cpp + test_safety.cpp + test_gripper.cpp + test_control_loop.cpp +) + +foreach(test_src IN LISTS LITEGRIP_CPP_TESTS) + get_filename_component(test_name ${test_src} NAME_WE) + add_executable(${test_name} ${test_src}) + target_link_libraries(${test_name} PRIVATE litegrip_cpp::litegrip_cpp) + add_test(NAME ${test_name} COMMAND ${test_name}) +endforeach() + +# test_calibration verifies the SHIPPED factory_calibration.json, so it needs +# the source data directory. Test-only: the library itself resolves its data +# location at run time and never bakes in a machine-dependent path. +target_compile_definitions(test_calibration PRIVATE + LITEGRIP_TEST_DATA_DIR="${CMAKE_CURRENT_SOURCE_DIR}/../calibration") +target_compile_definitions(test_gripper PRIVATE + LITEGRIP_TEST_DATA_DIR="${CMAKE_CURRENT_SOURCE_DIR}/../calibration") + +# test_safety loads the shipped versioned baselines. The library discovers its +# data directory at run time (no baked-in path), so point it at the source tree. +set_tests_properties(test_safety PROPERTIES + ENVIRONMENT "LITEGRIP_DATA_DIR=${CMAKE_CURRENT_SOURCE_DIR}/..") +set_tests_properties(test_control_loop PROPERTIES + ENVIRONMENT "LITEGRIP_DATA_DIR=${CMAKE_CURRENT_SOURCE_DIR}/..") diff --git a/test/generate_golden.py b/test/generate_golden.py new file mode 100644 index 0000000..ddbf40d --- /dev/null +++ b/test/generate_golden.py @@ -0,0 +1,255 @@ +#!/usr/bin/env python3 +"""Generate test/test_golden_generated.cpp from the PYTHON SDK. + +This is the port's parity harness. Rather than hand-copying expected values +(which is how a port drifts), it drives the real Python implementation and +emits the exact bytes it produces; the C++ test then has to reproduce them. + +Deterministic: fixed seed, so regenerating without changing the Python SDK +produces a byte-identical file. + +Usage (from litegrip_cpp/): + + python3 test/generate_golden.py +""" + +from __future__ import annotations + +import os +import random +import struct +import sys + +HERE = os.path.dirname(os.path.abspath(__file__)) +SDK_ROOT = os.path.normpath(os.path.join(HERE, "..", "..", "litegrip_driver")) +sys.path.insert(0, SDK_ROOT) + +from litegrip.can import protocol as P # noqa: E402 + +SEED = 20260928 +N_QUANT = 64 +N_MIT = 64 +N_STATUS = 64 +N_WRITE = 48 +N_PARAM_RESP = 48 + +# Motor limits to exercise: DM4310 (default), DM4340, DM6248P. +LIMIT_SETS = [ + (1, P.get_motor_limits(1)), + (3, P.get_motor_limits(3)), + (6, P.get_motor_limits(6)), +] + + +def c_double(value: float) -> str: + """Emit a double literal that round-trips exactly.""" + return repr(float(value)) + + +def bytes_c(values) -> str: + return "{" + ", ".join(f"0x{b:02X}" for b in values) + "}" + + +def main() -> int: + rng = random.Random(SEED) + out: list[str] = [] + + out.append("// AUTO-GENERATED by test/generate_golden.py — DO NOT EDIT.") + out.append("//") + out.append("// Values are produced by the Python SDK (litegrip_driver) and must be") + out.append("// reproduced byte-for-byte by the C++ port. Regenerate with:") + out.append("// python3 test/generate_golden.py") + out.append("") + out.append('#include "litegrip/can/protocol.hpp"') + out.append("") + out.append("namespace litegrip::test::golden {") + out.append("") + + # ── quantization round-trips ────────────────────────────────────────── + out.append("struct QuantVector {") + out.append(" double value, value_min, value_max;") + out.append(" int bits;") + out.append(" unsigned int encoded;") + out.append(" double decoded;") + out.append("};") + out.append("") + out.append("constexpr QuantVector kQuantVectors[] = {") + probes = [(-999.0, -12.5, 12.5, 16), (999.0, -12.5, 12.5, 16), + (0.0, -12.5, 12.5, 16), (6.25, -12.5, 12.5, 16), + (-6.25, -12.5, 12.5, 16), (12.5, -12.5, 12.5, 16), + (-12.5, -12.5, 12.5, 16), (0.0, 0.0, 500.0, 12), + (250.0, 0.0, 500.0, 12), (500.0, 0.0, 500.0, 12), + (0.0, 0.0, 5.0, 12), (5.0, 0.0, 5.0, 12), + (0.0, -30.0, 30.0, 12), (30.0, -30.0, 30.0, 12), + (-30.0, -30.0, 30.0, 12), (0.0, -10.0, 10.0, 12)] + for _ in range(N_QUANT): + bits = rng.choice([12, 16]) + lo = rng.choice([-12.5, -30.0, -10.0, 0.0]) + hi = rng.choice([5.0, 10.0, 12.5, 30.0, 500.0]) + if hi <= lo: + hi = lo + 1.0 + probes.append((rng.uniform(lo * 1.2, hi * 1.2), lo, hi, bits)) + + for value, lo, hi, bits in probes: + encoded = P.float_to_uint(value, lo, hi, bits) + decoded = P.uint_to_float(encoded, lo, hi, bits) + out.append(f" {{{c_double(value)}, {c_double(lo)}, {c_double(hi)}, " + f"{bits}, {encoded}u, {c_double(decoded)}}},") + out.append("};") + out.append("") + + # ── MIT frames ──────────────────────────────────────────────────────── + out.append("struct MitVector {") + out.append(" double q, dq, kp, kd, tau;") + out.append(" double q_max, dq_max, tau_max;") + out.append(" unsigned char bytes[8];") + out.append("};") + out.append("") + out.append("constexpr MitVector kMitVectors[] = {") + for _ in range(N_MIT): + _, limits = rng.choice(LIMIT_SETS) + q = rng.uniform(-limits.q_max, limits.q_max) + dq = rng.uniform(-limits.dq_max, limits.dq_max) + kp = rng.uniform(0.0, 500.0) + kd = rng.uniform(0.0, 5.0) + tau = rng.uniform(-limits.tau_max, limits.tau_max) + data = P.pack_mit_frame(q, dq, kp, kd, tau, q_max=limits.q_max, + dq_max=limits.dq_max, tau_max=limits.tau_max) + out.append(f" {{{c_double(q)}, {c_double(dq)}, {c_double(kp)}, " + f"{c_double(kd)}, {c_double(tau)}, {c_double(limits.q_max)}, " + f"{c_double(limits.dq_max)}, {c_double(limits.tau_max)}, " + f"{bytes_c(data)}}},") + out.append("};") + out.append("") + + # ── status frames ───────────────────────────────────────────────────── + out.append("struct StatusVector {") + out.append(" unsigned char raw[8];") + out.append(" double q_max, dq_max, tau_max;") + out.append(" int err, can_id, t_mos, t_coil;") + out.append(" double q, dq, tau;") + out.append("};") + out.append("") + out.append("constexpr StatusVector kStatusVectors[] = {") + for _ in range(N_STATUS): + _, limits = rng.choice(LIMIT_SETS) + raw = bytes(rng.randrange(256) for _ in range(8)) + s = P.unpack_status_frame(raw, q_max=limits.q_max, dq_max=limits.dq_max, + tau_max=limits.tau_max) + out.append(f" {{{bytes_c(raw)}, {c_double(limits.q_max)}, " + f"{c_double(limits.dq_max)}, {c_double(limits.tau_max)}, " + f"{s.err}, {s.can_id}, {s.t_mos}, {s.t_coil}, " + f"{c_double(s.q)}, {c_double(s.dq)}, {c_double(s.tau)}}},") + out.append("};") + out.append("") + + # ── parameter writes (float + int registers) ────────────────────────── + out.append("struct WriteVector {") + out.append(" int can_id, rid;") + out.append(" double value;") + out.append(" unsigned char bytes[8];") + out.append("};") + out.append("") + out.append("constexpr WriteVector kWriteVectors[] = {") + int_regs = [7, 8, 9, 10, 13, 14, 15, 16, 35, 36] + float_regs = [0, 1, 2, 3, 4, 5, 6, 11, 12, 17, 21, 22, 23, 25, 26] + for _ in range(N_WRITE): + can_id = rng.choice([0x08, 0x01, 0x0F]) + if rng.random() < 0.5: + rid = rng.choice(int_regs) + value = float(rng.randrange(0, 256)) + else: + rid = rng.choice(float_regs) + value = rng.uniform(-100.0, 500.0) + data = P.pack_write_param_frame(can_id, rid, value) + out.append(f" {{{can_id}, {rid}, {c_double(value)}, {bytes_c(data)}}},") + out.append("};") + out.append("") + + # ── parameter responses ─────────────────────────────────────────────── + out.append("struct ParamRespVector {") + out.append(" unsigned char raw[8];") + out.append(" bool has_value;") + out.append(" int can_id, opcode, rid;") + out.append(" double value;") + out.append("};") + out.append("") + out.append("constexpr ParamRespVector kParamRespVectors[] = {") + for _ in range(N_PARAM_RESP): + can_id = rng.choice([0x08, 0x01, 0x0E]) + opcode = rng.choice([0x33, 0x55, 0xAA]) + if rng.random() < 0.5: + rid = rng.choice(int_regs) + payload = rng.randrange(0, 1 << 24).to_bytes(4, "little") + else: + rid = rng.choice(float_regs) + payload = struct.pack(" +#include +#include + +#include "litegrip/bus.hpp" +#include "litegrip/exceptions.hpp" + +namespace { + +int g_failures = 0; + +void check(bool condition, const char* what) { + if (!condition) { + std::cerr << "FAIL: " << what << "\n"; + ++g_failures; + } +} + +// A stand-in for the user's rewrite of the hold behaviour: proves the seam is +// usable without touching anything else. +class TrackingPolicy : public litegrip::HoldPolicy { + public: + void init(litegrip::GripperBus&, const litegrip::GripperConfig&) override { + ++init_calls; + } + void hold(litegrip::GripperBus&, const litegrip::GripperConfig&) override { + ++hold_calls; + } + const char* name() const noexcept override { return "TrackingPolicy"; } + + int init_calls = 0; + int hold_calls = 0; +}; + +} // namespace + +int main() { + // ── construction must not touch any hardware ────────────────────────── + { + litegrip::GripperBus bus; + check(!bus.is_connected(), "a fresh bus is not connected"); + check(!bus.is_initialized(), "a fresh bus is not initialised"); + check(bus.motor() == nullptr, "a fresh bus has no motor"); + check(bus.config().can_id == litegrip::GripperParams::kCanId, + "config defaults carried through"); + } + + // ── every bus operation refuses while disconnected ──────────────────── + { + litegrip::GripperBus bus; + check(!bus.control_mit(0.0, 10.0, 1.0), "control_mit refuses when closed"); + check(!bus.control_mit_stream(0.0, 10.0, 1.0, 0.01), + "control_mit_stream refuses when closed"); + check(!bus.poll(0.0), "poll refuses when closed"); + check(!bus.update_state(0.01), "update_state refuses when closed"); + check(!bus.refresh_status(0.01), "refresh_status refuses when closed"); + check(!bus.enable(), "enable refuses when closed"); + check(!bus.disable(), "disable refuses when closed"); + check(!bus.clear_fault(), "clear_fault refuses when closed"); + check(bus.get_position() == 0.0, "position defaults to 0"); + check(bus.get_velocity() == 0.0, "velocity defaults to 0"); + check(bus.get_torque() == 0.0, "torque defaults to 0"); + check(bus.get_error() == -1, "error defaults to -1 when there is no motor"); + + bool threw = false; + try { + bus.read_param(7); + } catch (const litegrip::NotInitializedError&) { + threw = true; + } catch (const litegrip::LiteGripError&) { + threw = true; + } + check(threw, "read_param throws while disconnected"); + } + + // ── init on a closed bus is an explicit error, not a silent false ───── + { + litegrip::GripperBus bus; + bool threw = false; + try { + bus.init(); + } catch (const litegrip::NotInitializedError&) { + threw = true; + } catch (const litegrip::LiteGripError&) { + threw = true; + } + check(threw, "init on a closed bus throws"); + } + + // ── connecting to a missing interface surfaces ConnectError ─────────── + { + litegrip::GripperConfig config; + config.can_channel = "lg_no_such_iface"; + litegrip::GripperBus bus(config); + bool threw = false; + try { + bus.connect(); + } catch (const litegrip::ConnectError&) { + threw = true; + } catch (const litegrip::LiteGripError&) { + threw = true; + } + check(threw, "connect to a missing interface throws"); + check(!bus.is_connected(), "bus stays disconnected after a failed connect"); + // A failed connect must leave it usable for a retry. + check(!bus.control_mit(0.0, 0.0, 0.0), "still refuses after a failed connect"); + } + + // ── config plumbing ─────────────────────────────────────────────────── + { + litegrip::GripperConfig config; + config.can_channel = "can1"; + config.can_id = 0x09; + config.mst_id = 0x19; + config.canfd_mode = true; + config.kp = 123.0; + litegrip::GripperBus bus(config); + check(bus.config().can_channel == "can1", "channel carried into the bus"); + check(bus.config().can_id == 0x09, "can_id carried into the bus"); + check(bus.config().mst_id.has_value() && *bus.config().mst_id == 0x19, + "mst_id carried into the bus"); + check(bus.config().canfd_mode, "canfd_mode carried into the bus"); + + // The accessor is mutable, matching the Python SDK's model. + bus.config().kp = 200.0; + check(bus.config().kp == 200.0, "config is mutable through the accessor"); + } + + // ── the R2 hold seam ────────────────────────────────────────────────── + { + litegrip::GripperBus bus; + check(std::string(bus.hold_policy().name()) == "DefaultHoldPolicy", + "default hold policy installed"); + + auto policy = std::make_unique(); + TrackingPolicy* observer = policy.get(); + bus.set_hold_policy(std::move(policy)); + check(std::string(bus.hold_policy().name()) == "TrackingPolicy", + "injected hold policy is in effect"); + check(observer->init_calls == 0 && observer->hold_calls == 0, + "injected policy is not called until init/enable"); + + // A null policy must not replace a working one. + bus.set_hold_policy(nullptr); + check(std::string(bus.hold_policy().name()) == "TrackingPolicy", + "null policy is ignored"); + } + + if (g_failures != 0) { + std::cerr << g_failures << " bus check(s) failed\n"; + return 1; + } + std::cout << "bus checks OK (no hardware touched)\n"; + return 0; +} diff --git a/test/test_calibration.cpp b/test/test_calibration.cpp new file mode 100644 index 0000000..18a5afd --- /dev/null +++ b/test/test_calibration.cpp @@ -0,0 +1,173 @@ +// test_calibration.cpp — calibration file load/save and path resolution. +// +// Uses the REAL packaged factory_calibration.json (its directory is passed in +// by CMake as LITEGRIP_TEST_DATA_DIR), so the shipped data file is verified +// rather than a fixture invented for the test. + +#include +#include +#include +#include +#include +#include + +#include "litegrip/calibration.hpp" +#include "litegrip/json.hpp" +#include "litegrip/models.hpp" + +#ifndef LITEGRIP_TEST_DATA_DIR +#error "LITEGRIP_TEST_DATA_DIR must be defined by CMake" +#endif + +namespace { + +int g_failures = 0; + +void check(bool condition, const char* what) { + if (!condition) { + std::cerr << "FAIL: " << what << "\n"; + ++g_failures; + } +} + +void check_near(double got, double want, double tol, const char* what) { + if (!(std::fabs(got - want) < tol)) { + std::cerr << "FAIL: " << what << " (got " << got << ", want " << want + << ")\n"; + ++g_failures; + } +} + +const std::string kFactoryPath = + std::string(LITEGRIP_TEST_DATA_DIR) + "/factory_calibration.json"; + +// The fixtures below are written with std::fopen, which creates the file but +// not its directory. Create it here: on a clean machine nothing has yet, and +// the checks must not depend on an earlier run having left it behind. +const char* const kFixtureDir = "/tmp/litegrip_calib_test"; + +} // namespace + +int main() { + ::mkdir(kFixtureDir, 0755); + + // ── the shipped factory calibration ─────────────────────────────────── + { + const auto factory = litegrip::read_calibration_file(kFactoryPath); + check(factory.has_value(), "factory_calibration.json parses"); + if (factory.has_value()) { + check(factory->has_closed && factory->has_open && factory->has_rad_to_mm, + "required fields present"); + // Values from the file, unchanged from the Python SDK's copy. + check_near(factory->zero_position_rad, 0.114, 1e-12, "zero_position_rad"); + check_near(factory->max_position_rad, -1.491, 1e-12, "max_position_rad"); + check_near(factory->rad_to_mm, 74.8, 1e-12, "rad_to_mm"); + check(factory->can_id.has_value() && *factory->can_id == 8, "can_id"); + check(factory->mst_id.has_value() && *factory->mst_id == 24, "mst_id"); + check(factory->canfd_mode.has_value() && !*factory->canfd_mode, + "canfd_mode"); + check(factory->motor_type.has_value() && *factory->motor_type == "DM4310", + "motor_type"); + check(factory->kp.has_value() && std::fabs(*factory->kp - 100.0) < 1e-12, + "kp"); + check(factory->grasp_torque_threshold.has_value() && + std::fabs(*factory->grasp_torque_threshold - 0.5) < 1e-12, + "grasp_torque_threshold"); + } + } + + // ── malformed / missing ─────────────────────────────────────────────── + check(!litegrip::read_calibration_file(kFactoryPath + ".nope").has_value(), + "missing file yields no value"); + { + const std::string path = std::string(kFixtureDir) + "/broken.json"; + const std::string text = "{\"can_id\": 8}"; // required keys absent + FILE* file = std::fopen(path.c_str(), "w"); + check(file != nullptr, "write fixture"); + if (file != nullptr) { + std::fwrite(text.data(), 1, text.size(), file); + std::fclose(file); + } + check(!litegrip::read_calibration_file(path).has_value(), + "file without required keys is rejected"); + + const std::string garbage = std::string(kFixtureDir) + "/garbage.json"; + file = std::fopen(garbage.c_str(), "w"); + if (file != nullptr) { + const std::string bad = "{not json"; + std::fwrite(bad.data(), 1, bad.size(), file); + std::fclose(file); + } + check(!litegrip::read_calibration_file(garbage).has_value(), + "malformed JSON is rejected"); + } + + // ── write then read back ────────────────────────────────────────────── + { + litegrip::GripperConfig config; + config.can_channel = "can0"; + config.pos_closed_rad = 1.775959; + config.pos_open_rad = -0.064279; + config.rad_to_mm = 65.21; + config.kp = 5.0; + config.kd = 2.0; + config.grasp_torque_threshold = 0.5; + + const std::string path = std::string(kFixtureDir) + "/roundtrip.json"; + // The explicit can_id/mst_id/motor_type arguments are authoritative for + // those three fields; the rest comes from the config. + litegrip::write_calibration_file(path, config, 8, 18, "DM4310"); + + const auto reloaded = litegrip::read_calibration_file(path); + check(reloaded.has_value(), "written calibration reads back"); + if (reloaded.has_value()) { + check_near(reloaded->zero_position_rad, 1.775959, 1e-6, "closed round-trip"); + check_near(reloaded->max_position_rad, -0.064279, 1e-6, "open round-trip"); + check_near(reloaded->rad_to_mm, 65.21, 1e-6, "rad_to_mm round-trip"); + check(reloaded->mst_id.has_value() && *reloaded->mst_id == 18, + "mst_id round-trip"); + check(reloaded->kp.has_value() && std::fabs(*reloaded->kp - 5.0) < 1e-9, + "kp round-trip"); + check(reloaded->channel.has_value() && *reloaded->channel == "can0", + "channel round-trip"); + } + + // travel_range_rad is written as the absolute difference. + const auto document = litegrip::json::Value::parse_file(path); + check(document.has_value(), "written file is valid JSON"); + if (document.has_value()) { + check_near(document->get_number("travel_range_rad", 0.0), + std::fabs(-0.064279 - 1.775959), 1e-6, + "travel_range_rad is absolute"); + } + } + + // ── path resolution / overrides ─────────────────────────────────────── + { + const std::string explicit_calib = + std::string(kFixtureDir) + "/env_calib.json"; + ::setenv("LITEGRIP_CALIB", explicit_calib.c_str(), 1); + check(litegrip::default_calibration_path() == explicit_calib, + "LITEGRIP_CALIB overrides the default calib path"); + ::unsetenv("LITEGRIP_CALIB"); + + // Without the override: $HOME/.litegrip/litegrip_calibration.json + const std::string fake_home = std::string(kFixtureDir) + "/home"; + ::setenv("HOME", fake_home.c_str(), 1); + check(litegrip::default_calibration_path() == + fake_home + "/.litegrip/litegrip_calibration.json", + "default calib path is under $HOME/.litegrip"); + + ::setenv("LITEGRIP_FACTORY_CALIB", kFactoryPath.c_str(), 1); + check(litegrip::factory_calibration_path() == kFactoryPath, + "LITEGRIP_FACTORY_CALIB overrides the factory path"); + ::unsetenv("LITEGRIP_FACTORY_CALIB"); + } + + if (g_failures != 0) { + std::cerr << g_failures << " calibration check(s) failed\n"; + return 1; + } + std::cout << "calibration checks OK\n"; + return 0; +} diff --git a/test/test_control_loop.cpp b/test/test_control_loop.cpp new file mode 100644 index 0000000..2a15fab --- /dev/null +++ b/test/test_control_loop.cpp @@ -0,0 +1,259 @@ +// test_control_loop.cpp — the control loop, driven in dry-run mode. +// +// dry_run runs the whole control path (rate limiting, torque-budget allocation, +// the safety gate, the watchdogs) against a simulated plant while skipping CAN. +// That makes the loop, and every deploy-config fail-closed rule, testable +// without hardware. + +#include +#include +#include +#include + +#include "litegrip/control_loop.hpp" +#include "litegrip/exceptions.hpp" + +namespace { + +int g_failures = 0; + +void check(bool condition, const char* what) { + if (!condition) { + std::cerr << "FAIL: " << what << "\n"; + ++g_failures; + } +} + +void check_near(double got, double want, double tol, const char* what) { + if (!(std::fabs(got - want) < tol)) { + std::cerr << "FAIL: " << what << " (got " << got << ", want " << want + << ")\n"; + ++g_failures; + } +} + +template +void check_throws(const char* what, Fn&& fn) { + try { + fn(); + std::cerr << "FAIL: " << what << " (did not throw)\n"; + ++g_failures; + } catch (const litegrip::LiteGripError&) { + // expected + } +} + +void sleep_ms(int ms) { + std::this_thread::sleep_for(std::chrono::milliseconds(ms)); +} + +/// A dry-run config whose calibration is CONSISTENT with the packaged red lines +/// ([-1.24, -0.01] rad), with a synthetic 1 rad == 100 mm scale so the +/// arithmetic in the assertions is obvious. +/// +/// The closed end has to sit inside the red lines for the gate to let any +/// motion through. Note what that implies for a real unit: the packaged red +/// lines are the REFERENCE unit's measurement (plan R3), so a deployment whose +/// calibration puts the closed end outside them will — correctly — be refused +/// all motion until the red lines are re-derived. +litegrip::ControlLoopConfig dry_config() { + litegrip::ControlLoopConfig cfg; + cfg.dry_run = true; + cfg.control_rate_hz = 200.0; + cfg.max_velocity_rad_s = 0.4; + cfg.torque_limit_nm = 3.5; + // A worst-case velocity bound MUST be given, otherwise the loop refuses to + // send motion at all (see the fail-closed test below). + cfg.max_feedback_velocity_rad_s = 1.0; + cfg.command_timeout_s = 10.0; + cfg.pos_closed_rad = litegrip::kPackageRedMax; // -0.01, inside the red lines + cfg.pos_open_rad = litegrip::kPackageRedMin; // -1.24 + cfg.rad_to_mm = 100.0; + return cfg; +} + +} // namespace + +int main() { + // ── lifecycle ───────────────────────────────────────────────────────── + { + litegrip::ControlLoop loop(dry_config()); + check(!loop.is_running(), "a fresh loop is not running"); + check(loop.config().control_rate_hz == 200.0, "config is readable"); + check(loop.fault_code() == 0, "a fresh loop has no fault"); + + check(loop.start(), "dry-run start succeeds"); + check(loop.is_running(), "the loop is running after start"); + loop.stop(); + check(!loop.is_running(), "the loop stops"); + loop.stop(); // idempotent + check(!loop.is_running(), "stop is idempotent"); + } + + // ── the safety limits come from the shipped baseline ────────────────── + { + litegrip::ControlLoop loop(dry_config()); + loop.start(); + const litegrip::SafetyLimits& limits = loop.safety_limits(); + check_near(limits.red_min_rad, litegrip::kPackageRedMin, 1e-12, + "loop uses the packaged open red line"); + check_near(limits.red_max_rad, litegrip::kPackageRedMax, 1e-12, + "loop uses the packaged closed red line"); + loop.stop(); + } + + // ── a target in mm is tracked ───────────────────────────────────────── + { + litegrip::ControlLoop loop(dry_config()); + loop.start(); + loop.set_enable(true); + loop.set_target_mm(10.0); // 10 mm == 0.1 rad at 100 mm/rad + + sleep_ms(600); + const double mm = loop.state().position_mm; + check_near(mm, 10.0, 0.5, "dry-run tracks a 10 mm target"); + check(loop.fault_code() == 0, "no fault while tracking"); + loop.stop(); + } + + // ── the rate ceiling actually limits the trajectory ─────────────────── + { + litegrip::ControlLoopConfig cfg = dry_config(); + cfg.max_velocity_rad_s = 0.05; // 5 mm/s at 100 mm/rad + litegrip::ControlLoop loop(cfg); + loop.start(); + loop.set_enable(true); + loop.set_target_mm(40.0); + + sleep_ms(200); + const double early = loop.state().position_mm; + check(early > 0.0, "the trajectory has started moving"); + // 0.2 s at 5 mm/s can cover at most ~1 mm; certainly not the whole 40 mm. + check(early < 5.0, "the rate ceiling prevented a jump to the target"); + + sleep_ms(400); + const double later = loop.state().position_mm; + check(later > early, "the trajectory keeps advancing toward the target"); + check(later < 40.0, "it is still rate limited"); + loop.stop(); + } + + // ── a stale command makes the loop hold position ────────────────────── + { + litegrip::ControlLoopConfig cfg = dry_config(); + cfg.command_timeout_s = 0.05; + cfg.max_velocity_rad_s = 0.4; + litegrip::ControlLoop loop(cfg); + loop.start(); + loop.set_enable(true); + loop.set_target_mm(80.0); + + // Only 50 ms of freshness at 40 mm/s => a few mm, then it must hold. + sleep_ms(400); + const double held = loop.state().position_mm; + check(held < 80.0, "a stale command did not complete the motion"); + + sleep_ms(200); + check_near(loop.state().position_mm, held, 0.5, + "the position is held once the command goes stale"); + loop.stop(); + } + + // ── emergency stop latches a fault and stops motion ─────────────────── + { + litegrip::ControlLoop loop(dry_config()); + loop.start(); + loop.set_enable(true); + loop.set_target_mm(30.0); + sleep_ms(150); + + loop.emergency_stop(); + sleep_ms(100); + check(loop.fault_code() == + static_cast(litegrip::FaultCode::kHardwareSafeStop), + "emergency stop latches a hardware safe stop"); + + const double after_stop = loop.state().position_mm; + sleep_ms(200); + check_near(loop.state().position_mm, after_stop, 0.5, + "no further motion after the emergency stop"); + loop.stop(); + } + + // ── fail-closed: no worst-case velocity bound => no motion ──────────── + { + litegrip::ControlLoopConfig cfg = dry_config(); + cfg.max_feedback_velocity_rad_s = -1.0; // "not given" + litegrip::ControlLoop loop(cfg); + loop.start(); + loop.set_enable(true); + loop.set_target_mm(20.0); + + sleep_ms(300); + check(loop.fault_code() == + static_cast(litegrip::FaultCode::kCommandRejected), + "motion is refused when the velocity bound is not given"); + check_near(loop.state().position_mm, 0.0, 0.5, + "the gripper never moved without a bounded torque budget"); + loop.stop(); + } + + // ── deploy configuration may only be tightened ──────────────────────── + { + litegrip::ControlLoopConfig cfg = dry_config(); + cfg.max_velocity_rad_s = 10.0; // above the hard ceiling + litegrip::ControlLoop loop(cfg); + check_throws("a velocity above the hard ceiling is refused", + [&] { loop.start(); }); + } + { + litegrip::ControlLoopConfig cfg = dry_config(); + cfg.torque_limit_nm = 9.0; // above the hard ceiling + litegrip::ControlLoop loop(cfg); + check_throws("a torque above the hard ceiling is refused", + [&] { loop.start(); }); + } + { + litegrip::ControlLoopConfig cfg = dry_config(); + cfg.control_rate_hz = 0.0; + litegrip::ControlLoop loop(cfg); + check_throws("a non-positive control rate is refused", + [&] { loop.start(); }); + } + { + litegrip::ControlLoopConfig cfg = dry_config(); + cfg.safety_baseline = "9.9"; // no such version + litegrip::ControlLoop loop(cfg); + check_throws("an unknown baseline version is refused", + [&] { loop.start(); }); + } + + // ── the dual switch ─────────────────────────────────────────────────── + { + litegrip::ControlLoopConfig cfg = dry_config(); + cfg.dry_run = false; + cfg.hardware_enable = false; + litegrip::ControlLoop loop(cfg); + check_throws("dry_run=false without hardware_enable is refused", + [&] { loop.start(); }); + check(!loop.is_running(), "the loop did not start"); + } + { + // hardware_enable=true with no gripper attached must fail at connect, not + // silently pretend to work. + litegrip::ControlLoopConfig cfg = dry_config(); + cfg.dry_run = false; + cfg.hardware_enable = true; + cfg.channel = "lg_no_such_iface"; + litegrip::ControlLoop loop(cfg); + check_throws("the real path fails loudly when the interface is missing", + [&] { loop.start(); }); + } + + if (g_failures != 0) { + std::cerr << g_failures << " control-loop check(s) failed\n"; + return 1; + } + std::cout << "control loop checks OK (dry run, no hardware touched)\n"; + return 0; +} diff --git a/test/test_golden.cpp b/test/test_golden.cpp new file mode 100644 index 0000000..1080596 --- /dev/null +++ b/test/test_golden.cpp @@ -0,0 +1,171 @@ +// test_golden.cpp — reproduce, byte for byte, what the PYTHON SDK emits. +// +// The vectors in golden_generated.hpp are produced by test/generate_golden.py +// driving the real litegrip_driver implementation. Unlike test_protocol.cpp +// (hand-lifted cases), this file scales: any divergence between the C++ port +// and the Python original in quantization, bit packing, endianness or float +// decoding shows up here. + +#include +#include +#include + +#include "golden_generated.hpp" +#include "litegrip/can/protocol.hpp" + +using namespace litegrip::test::golden; + +namespace { + +int g_failures = 0; +int g_checks = 0; + +void fail(const char* group, std::size_t index, const char* what) { + std::cerr << "FAIL: " << group << "[" << index << "] " << what << "\n"; + ++g_failures; +} + +void check_bytes(const char* group, std::size_t index, const std::uint8_t* got, + const std::uint8_t* want, std::size_t size) { + for (std::size_t i = 0; i < size; ++i) { + ++g_checks; + if (got[i] != want[i]) { + std::cerr << "FAIL: " << group << "[" << index << "] byte " << i + << ": got 0x" << std::hex << static_cast(got[i]) + << ", want 0x" << static_cast(want[i]) << std::dec << "\n"; + ++g_failures; + return; + } + } +} + +} // namespace + +int main() { + litegrip::can::MotorLimits limits; + + // ── quantization ────────────────────────────────────────────────────── + for (std::size_t i = 0; i < sizeof(kQuantVectors) / sizeof(kQuantVectors[0]); + ++i) { + const auto& v = kQuantVectors[i]; + const std::uint32_t encoded = + litegrip::can::float_to_uint(v.value, v.value_min, v.value_max, v.bits); + ++g_checks; + if (encoded != v.encoded) { + std::cerr << "FAIL: quant[" << i << "] encoded: got " << encoded + << ", want " << v.encoded << "\n"; + ++g_failures; + } + const double decoded = + litegrip::can::uint_to_float(encoded, v.value_min, v.value_max, v.bits); + ++g_checks; + if (std::fabs(decoded - v.decoded) > 1e-12) { + std::cerr << "FAIL: quant[" << i << "] decoded: got " << decoded + << ", want " << v.decoded << "\n"; + ++g_failures; + } + } + + // ── MIT frames ──────────────────────────────────────────────────────── + for (std::size_t i = 0; i < sizeof(kMitVectors) / sizeof(kMitVectors[0]); + ++i) { + const auto& v = kMitVectors[i]; + limits.q_max = v.q_max; + limits.dq_max = v.dq_max; + limits.tau_max = v.tau_max; + const auto got = litegrip::can::pack_mit_frame(v.q, v.dq, v.kp, v.kd, v.tau, + limits); + check_bytes("mit", i, got.data(), v.bytes, 8); + } + + // ── status frames ───────────────────────────────────────────────────── + for (std::size_t i = 0; i < sizeof(kStatusVectors) / sizeof(kStatusVectors[0]); + ++i) { + const auto& v = kStatusVectors[i]; + limits.q_max = v.q_max; + limits.dq_max = v.dq_max; + limits.tau_max = v.tau_max; + const auto got = litegrip::can::unpack_status_frame(v.raw, 8, limits); + ++g_checks; + if (!got.has_value()) { + fail("status", i, "did not parse"); + continue; + } + if (got->err != v.err || got->can_id != v.can_id || got->t_mos != v.t_mos || + got->t_coil != v.t_coil) { + fail("status", i, "integer fields differ"); + continue; + } + if (std::fabs(got->q - v.q) > 1e-12 || std::fabs(got->dq - v.dq) > 1e-12 || + std::fabs(got->tau - v.tau) > 1e-12) { + fail("status", i, "decoded floats differ"); + } + } + + // ── parameter writes ────────────────────────────────────────────────── + for (std::size_t i = 0; i < sizeof(kWriteVectors) / sizeof(kWriteVectors[0]); + ++i) { + const auto& v = kWriteVectors[i]; + const auto got = + litegrip::can::pack_write_param_frame(v.can_id, v.rid, v.value); + check_bytes("write", i, got.data(), v.bytes, 8); + } + + // ── parameter responses ─────────────────────────────────────────────── + for (std::size_t i = 0; + i < sizeof(kParamRespVectors) / sizeof(kParamRespVectors[0]); ++i) { + const auto& v = kParamRespVectors[i]; + const auto got = litegrip::can::unpack_param_response(v.raw, 8); + ++g_checks; + if (got.has_value() != v.has_value) { + fail("param_resp", i, "has_value differs"); + continue; + } + if (!v.has_value) { + continue; + } + if (got->can_id != v.can_id || got->opcode != v.opcode || got->rid != v.rid) { + fail("param_resp", i, "integer fields differ"); + continue; + } + if (std::fabs(got->value - v.value) > 1e-6) { + std::cerr << "FAIL: param_resp[" << i << "] value: got " << got->value + << ", want " << v.value << "\n"; + ++g_failures; + } + } + + // ── fixed frames ────────────────────────────────────────────────────── + for (std::size_t i = 0; i < sizeof(kReadVectors) / sizeof(kReadVectors[0]); + ++i) { + const auto& v = kReadVectors[i]; + // Recover the rid from the reference bytes (payload byte 3). + const auto got = litegrip::can::pack_read_param_frame(v.can_id, v.bytes[3]); + check_bytes("read", i, got.data(), v.bytes, 8); + } + for (std::size_t i = 0; + i < sizeof(kRefreshVectors) / sizeof(kRefreshVectors[0]); ++i) { + const auto& v = kRefreshVectors[i]; + const auto got = litegrip::can::pack_refresh_frame(v.can_id); + check_bytes("refresh", i, got.data(), v.bytes, 4); + } + for (std::size_t i = 0; i < sizeof(kSaveVectors) / sizeof(kSaveVectors[0]); + ++i) { + const auto& v = kSaveVectors[i]; + const auto got = litegrip::can::pack_save_param_frame(v.can_id); + check_bytes("save", i, got.data(), v.bytes, 8); + } + for (std::size_t i = 0; + i < sizeof(kCommandVectors) / sizeof(kCommandVectors[0]); ++i) { + const auto& v = kCommandVectors[i]; + const auto got = litegrip::can::pack_command_frame(v.cmd); + check_bytes("command", i, got.data(), v.bytes, 8); + } + + if (g_failures != 0) { + std::cerr << g_failures << " of " << g_checks << " parity check(s) failed\n"; + return 1; + } + std::cout << "python parity OK (" << g_checks << " checks)\n"; + return 0; +} diff --git a/test/test_gripper.cpp b/test/test_gripper.cpp new file mode 100644 index 0000000..8de55ff --- /dev/null +++ b/test/test_gripper.cpp @@ -0,0 +1,205 @@ +// test_gripper.cpp — LiteGrip's behaviour that does NOT need hardware. +// +// Construction, the "not connected" refusals, calibration file plumbing, the +// move semantics connect_raii() needs, and the fact that the default safety +// limits are the packaged baseline. Motion and enable paths need the real +// device and are covered there. + +#include +#include +#include +#include +#include + +#include "litegrip/exceptions.hpp" +#include "litegrip/gripper.hpp" +#include "litegrip/safety.hpp" + +#ifndef LITEGRIP_TEST_DATA_DIR +#error "LITEGRIP_TEST_DATA_DIR must be defined by CMake" +#endif + +namespace { + +int g_failures = 0; + +void check(bool condition, const char* what) { + if (!condition) { + std::cerr << "FAIL: " << what << "\n"; + ++g_failures; + } +} + +template +void check_throws(const char* what, Fn&& fn) { + try { + fn(); + std::cerr << "FAIL: " << what << " (did not throw)\n"; + ++g_failures; + } catch (const litegrip::LiteGripError&) { + // expected + } +} + +const std::string kFactoryPath = + std::string(LITEGRIP_TEST_DATA_DIR) + "/factory_calibration.json"; + +} // namespace + +int main() { + // ── construction must not touch the bus ─────────────────────────────── + { + litegrip::LiteGrip gripper; + check(!gripper.is_connected(), "a fresh gripper is not connected"); + check(!gripper.is_enabled(), "a fresh gripper is not enabled"); + check(gripper.channel() == litegrip::DefaultParams::kCanChannel, + "default channel"); + check(gripper.can_id() == litegrip::GripperParams::kCanId, "default can_id"); + check(!gripper.mst_id().has_value(), "mst_id is unset before connect"); + } + + // ── the default safety limits are the packaged baseline ─────────────── + { + litegrip::LiteGrip gripper; + const litegrip::SafetyLimits& limits = gripper.safety().limits(); + check(std::fabs(limits.red_min_rad - litegrip::kPackageRedMin) < 1e-12, + "the default guard uses the packaged open red line"); + check(std::fabs(limits.red_max_rad - litegrip::kPackageRedMax) < 1e-12, + "the default guard uses the packaged closed red line"); + check(limits.enabled, "the default guard has the red lines enabled"); + check(gripper.safety().mode() == litegrip::FrameMode::kNormal, + "the default guard starts in normal mode"); + } + + // ── everything that needs the bus refuses while disconnected ────────── + { + litegrip::LiteGrip gripper; + check_throws("init refuses when not connected", + [&] { gripper.init(); }); + check_throws("enable refuses when not connected", + [&] { gripper.enable(); }); + check_throws("disable refuses when not connected", + [&] { gripper.disable(); }); + check_throws("clear_fault refuses when not connected", + [&] { gripper.clear_fault(); }); + check_throws("get_state refuses when not connected", + [&] { gripper.get_state(); }); + check_throws("get_position_rad refuses when not connected", + [&] { gripper.get_position_rad(); }); + check_throws("goto_mm refuses when not connected", + [&] { gripper.goto_mm(40.0); }); + check_throws("goto_rad refuses when not connected", + [&] { gripper.goto_rad(-0.5); }); + check_throws("move_to refuses when not connected", + [&] { gripper.move_to(-0.5); }); + check_throws("read_param refuses when not connected", + [&] { gripper.read_param(7); }); + check_throws("write_param refuses when not connected", + [&] { gripper.write_param(7, 1.0); }); + + // These report rather than throw. + check(!gripper.send_mit_frame(0.0, 0.0, 0.0), "send_mit_frame reports false"); + check(!gripper.poll(0.0), "poll reports false"); + gripper.stop(); // a no-op, must not throw + } + + // ── connecting to a missing interface surfaces ConnectError ─────────── + { + litegrip::GripperConfig config; + config.can_channel = "lg_no_such_iface"; + litegrip::LiteGrip gripper(config); + check_throws("connect to a missing interface throws", + [&] { gripper.connect(); }); + check(!gripper.is_connected(), "stays disconnected after a failed connect"); + } + + // ── config plumbing ─────────────────────────────────────────────────── + { + litegrip::GripperConfig config; + config.can_channel = "can1"; + config.can_id = 0x0A; + config.kp = 150.0; + litegrip::LiteGrip gripper(config); + check(gripper.config().can_channel == "can1", "config carried through"); + check(gripper.config().can_id == 0x0A, "can_id carried through"); + check(gripper.config().kp == 150.0, "kp carried through"); + gripper.config().kd = 3.0; + check(gripper.config().kd == 3.0, "config is mutable"); + } + + // ── calibration file plumbing (no hardware involved) ────────────────── + { + litegrip::LiteGrip gripper; + // An explicit path loads even without a connection, matching the Python + // SDK's documented order (load after connect, before enable). + check(gripper.load_calibration(kFactoryPath), "explicit calibration loads"); + check(std::fabs(gripper.config().pos_closed_rad - 0.114) < 1e-12, + "closed limit loaded from the factory file"); + check(std::fabs(gripper.config().pos_open_rad - (-1.491)) < 1e-12, + "open limit loaded from the factory file"); + check(std::fabs(gripper.config().rad_to_mm - 74.8) < 1e-12, + "rad_to_mm loaded from the factory file"); + check(gripper.config().can_id == 8, "can_id loaded from the factory file"); + check(gripper.mst_id().has_value() && *gripper.mst_id() == 24, + "mst_id loaded from the factory file"); + + check(!gripper.load_calibration("/tmp/litegrip_gripper_test/none.json"), + "a missing calibration reports false and keeps the fallback order"); + } + { + // Saving writes the current config back out. + litegrip::GripperConfig config; + config.pos_closed_rad = 1.2; + config.pos_open_rad = -0.1; + config.rad_to_mm = 65.0; + litegrip::LiteGrip gripper(config); + const std::string path = "/tmp/litegrip_gripper_test/saved.json"; + check(gripper.save_calibration(path) == path, + "save_calibration returns the path written"); + + litegrip::LiteGrip reloaded; + check(reloaded.load_calibration(path), "the saved calibration reloads"); + check(std::fabs(reloaded.config().pos_closed_rad - 1.2) < 1e-6, + "closed limit round-trips through the file"); + check(std::fabs(reloaded.config().rad_to_mm - 65.0) < 1e-6, + "rad_to_mm round-trips through the file"); + } + { + // Without an explicit path the default user path is used, and a missing file + // falls through to the packaged factory calibration. + ::setenv("LITEGRIP_CALIB", "/tmp/litegrip_gripper_test/absent.json", 1); + ::setenv("LITEGRIP_FACTORY_CALIB", kFactoryPath.c_str(), 1); + litegrip::LiteGrip gripper; + check(gripper.load_calibration(), "falls back to the factory calibration"); + check(std::fabs(gripper.config().rad_to_mm - 74.8) < 1e-12, + "factory values applied via the fallback"); + ::unsetenv("LITEGRIP_CALIB"); + ::unsetenv("LITEGRIP_FACTORY_CALIB"); + } + + // ── info and the move semantics connect_raii needs ──────────────────── + { + litegrip::GripperConfig config; + config.can_id = 0x08; + litegrip::LiteGrip gripper(config); + const litegrip::GripperInfo info = gripper.get_info(); + check(info.can_id == 0x08, "get_info reports can_id"); + check(info.motor_type == "DM4310", "get_info reports the motor type"); + check(info.model == "LiteGrip", "get_info reports the model"); + } + { + litegrip::LiteGrip original; + litegrip::LiteGrip moved(std::move(original)); + check(!moved.is_connected(), "a moved-to gripper is not connected"); + // The moved-from object must be inert: its destructor must not try to close + // a bus it no longer owns. + check(!original.is_connected(), "a moved-from gripper reports disconnected"); + } + + if (g_failures != 0) { + std::cerr << g_failures << " gripper check(s) failed\n"; + return 1; + } + std::cout << "gripper checks OK (no hardware touched)\n"; + return 0; +} diff --git a/test/test_headers.cpp b/test/test_headers.cpp new file mode 100644 index 0000000..0c54d94 --- /dev/null +++ b/test/test_headers.cpp @@ -0,0 +1,57 @@ +// test_headers.cpp — T1 gate: the frozen public API surface must compile, and +// the value types must stay aggregate-simple. +// +// This is a compile-time test first and a runtime test second: if any header +// fails to stand on its own, the build breaks here rather than in a consumer. + +#include + +#include "litegrip/litegrip.hpp" + +// The value types must be default-constructible and copyable — they cross +// layer boundaries as plain values. +static_assert(std::is_default_constructible_v); +static_assert(std::is_copy_constructible_v); +static_assert(std::is_default_constructible_v); +static_assert(std::is_default_constructible_v); +static_assert(std::is_default_constructible_v); + +// The safety limits must be trivially copyable: they are snapshot-diagnostics +// as well as configuration. +static_assert(std::is_copy_constructible_v); + +// Buses and loops own resources; copying them must be impossible. +static_assert(!std::is_copy_constructible_v); +static_assert(!std::is_copy_constructible_v); +static_assert(!std::is_copy_constructible_v); + +// The hold seam must be polymorphic (R2). +static_assert(std::has_virtual_destructor_v); +static_assert(std::is_base_of_v); + +// Exception hierarchy: catch-by-base must work for every specific error. +static_assert(std::is_base_of_v); +static_assert(std::is_base_of_v); +static_assert(std::is_base_of_v == false); +static_assert(std::is_base_of_v); +static_assert(std::is_base_of_v); + +// Protocol frame sizes are part of the wire contract, and are asserted with +// real golden vectors in test_protocol (T2) — not here, since these packers are +// declarations only at T1. + +int main() { + // Constructing a config must not touch any hardware. + const litegrip::GripperConfig config; + if (config.can_id != 0x08 || config.rad_to_mm <= 0.0) { + return 1; + } + // The packaged baseline must satisfy its own invariant. + const litegrip::SafetyLimits limits; + if (!(limits.mech_min_rad < limits.red_min_rad && + limits.red_min_rad < limits.red_max_rad && + limits.red_max_rad < limits.mech_max_rad)) { + return 1; + } + return 0; +} diff --git a/test/test_json.cpp b/test/test_json.cpp new file mode 100644 index 0000000..03f3611 --- /dev/null +++ b/test/test_json.cpp @@ -0,0 +1,198 @@ +// test_json.cpp — the minimal JSON parser/serializer. +// +// The SDK's files are read back by the Python SDK (and vice versa) during +// parallel validation, so both structure and number formatting matter. + +#include +#include +#include +#include +#include + +#include "litegrip/json.hpp" + +using litegrip::json::Value; + +namespace { + +int g_failures = 0; + +void check(bool condition, const char* what) { + if (!condition) { + std::cerr << "FAIL: " << what << "\n"; + ++g_failures; + } +} + +void check_str(const std::string& got, const std::string& want, + const char* what) { + if (got != want) { + std::cerr << "FAIL: " << what << "\n got: " << got << "\n want: " << want + << "\n"; + ++g_failures; + } +} + +} // namespace + +int main() { + // ── scalars ─────────────────────────────────────────────────────────── + { + const auto value = Value::parse("42"); + check(value.has_value() && value->is_number(), "parse integer"); + check(value.has_value() && value->as_number() == 42.0, "integer value"); + } + { + const auto value = Value::parse("-1.5e2"); + check(value.has_value() && value->is_number(), "parse exponent"); + if (value.has_value()) { + check(std::fabs(value->as_number() - (-150.0)) < 1e-9, "exponent value"); + } + } + { + const auto value = Value::parse("true"); + check(value.has_value() && value->is_bool() && value->as_bool(), + "parse true"); + const auto other = Value::parse("false"); + check(other.has_value() && other->is_bool() && !other->as_bool(), + "parse false"); + const auto nil = Value::parse("null"); + check(nil.has_value() && nil->is_null(), "parse null"); + } + + // ── strings and escapes ─────────────────────────────────────────────── + { + const auto value = Value::parse("\"hello\""); + check(value.has_value() && value->is_string(), "parse string"); + check(value.has_value() && value->as_string() == "hello", "string value"); + } + { + const auto value = + Value::parse("\"a\\\"b\\\\c\\nd\\te\\u0041\""); + check(value.has_value(), "parse escaped string"); + if (value.has_value()) { + check_str(value->as_string(), "a\"b\\c\nd\teA", "escape decoding"); + } + } + { + const auto value = Value::parse("\"\\u00e9\""); + check(value.has_value(), "parse \\u with 2-byte UTF-8"); + if (value.has_value()) { + check_str(value->as_string(), "\xc3\xa9", "2-byte UTF-8 encoding"); + } + } + + // ── objects and arrays ──────────────────────────────────────────────── + { + const auto value = Value::parse("{\"a\": 1, \"b\": \"two\", \"c\": true}"); + check(value.has_value() && value->is_object(), "parse object"); + if (value.has_value()) { + check(value->contains("a") && value->contains("b") && value->contains("c"), + "object keys present"); + check(!value->contains("zzz"), "absent key reports false"); + check(value->find("zzz") == nullptr, "absent key yields no value"); + check(value->get_int("a", 0) == 1, "get_int"); + check_str(value->get_string("b", ""), "two", "get_string"); + check(value->get_bool("c", false), "get_bool"); + // Missing keys fall back rather than throwing. + check(value->get_int("zzz", 7) == 7, "get_int default"); + check(value->get_number("zzz", 2.5) == 2.5, "get_number default"); + check(value->get_bool("zzz", true), "get_bool default"); + } + } + { + const auto value = Value::parse("[1, 2, [3, 4], {\"k\": 5}]"); + check(value.has_value() && value->is_array(), "parse nested array"); + if (value.has_value()) { + check(value->size() == 4, "array size"); + check(value->get_number("", 0.0) == 0.0, "get_* on an array defaults"); + check((*value)[0].as_number() == 1.0, "array index 0"); + check((*value)[2].is_array() && (*value)[2].size() == 2, "nested array"); + check((*value)[3].is_object(), "object inside array"); + } + } + + // ── malformed input is rejected, not guessed at ─────────────────────── + for (const char* bad : {"", "{", "}", "[1,]", "{\"a\"}", "{\"a\":}", "tru", + "nul", "123abc", "\"unterminated", "[", "{\"a\":1,}"}) { + check(!Value::parse(bad).has_value(), "malformed input rejected"); + } + + // ── dump ────────────────────────────────────────────────────────────── + { + Value object = Value::make_object(); + object.set("channel", "can0"); + object.set("can_id", 8); + object.set("rad_to_mm", 0.114); + object.set("canfd_mode", false); + check_str(object.dump(0), + "{\"channel\":\"can0\",\"can_id\":8,\"rad_to_mm\":0.114," + "\"canfd_mode\":false}", + "compact dump"); + } + { + // Shortest round-trip number formatting, so files stay diffable and match + // what Python's json.dump writes. + check_str(Value(0.114).dump(0), "0.114", "0.114 formats short"); + check_str(Value(65.0231).dump(0), "65.0231", "65.0231 formats short"); + check_str(Value(8.0).dump(0), "8", "integral double prints without .0"); + check_str(Value(-0.01).dump(0), "-0.01", "small negative formats short"); + } + { + Value array = Value::make_array(); + array.push_back(Value(1)); + array.push_back(Value("x")); + array.push_back(Value(true)); + check_str(array.dump(0), "[1,\"x\",true]", "array dump"); + } + { + Value empty_object = Value::make_object(); + Value empty_array = Value::make_array(); + check_str(empty_object.dump(0), "{}", "empty object dump"); + check_str(empty_array.dump(0), "[]", "empty array dump"); + } + + // ── round-trip ──────────────────────────────────────────────────────── + { + const std::string text = + "{\"a\":[1,2.5,\"s\"],\"b\":{\"c\":null},\"d\":true}"; + const auto parsed = Value::parse(text); + check(parsed.has_value(), "round-trip parse"); + if (parsed.has_value()) { + check_str(parsed->dump(0), text, "round-trip is byte-identical"); + } + } + + // ── file I/O (creates parent directories, like the Python SDK) ──────── + { + const std::string path = + "/tmp/litegrip_json_test/nested/dir/sample.json"; + ::unlink(path.c_str()); + + Value document = Value::make_object(); + document.set("can_id", 8); + document.set("rad_to_mm", 74.8); + document.set("motor_type", "DM4310"); + check(document.write_file(path), "write_file creates parents"); + + const auto reloaded = Value::parse_file(path); + check(reloaded.has_value(), "parse_file reads it back"); + if (reloaded.has_value()) { + check(reloaded->get_int("can_id", 0) == 8, "reloaded can_id"); + check(std::fabs(reloaded->get_number("rad_to_mm", 0.0) - 74.8) < 1e-12, + "reloaded rad_to_mm"); + check_str(reloaded->get_string("motor_type", ""), "DM4310", + "reloaded motor_type"); + } + } + check(!Value::parse_file("/tmp/litegrip_json_test/does/not/exist.json") + .has_value(), + "missing file yields no value"); + + if (g_failures != 0) { + std::cerr << g_failures << " json check(s) failed\n"; + return 1; + } + std::cout << "json checks OK\n"; + return 0; +} diff --git a/test/test_motor.cpp b/test/test_motor.cpp new file mode 100644 index 0000000..d05b2a0 --- /dev/null +++ b/test/test_motor.cpp @@ -0,0 +1,211 @@ +// test_motor.cpp — golden vectors lifted from +// litegrip_driver/tests/test_models.py, plus the MotorState cases from the same +// suite. +// +// Divergence from the Python suite: describe_error() returns ENGLISH text in +// the C++ SDK (a deliberate, confirmed choice), so the assertions match English +// substrings instead of the Python messages' Chinese ones. + +#include +#include +#include +#include +#include + +#include "litegrip/can/motor.hpp" +#include "litegrip/constants.hpp" +#include "litegrip/models.hpp" + +using litegrip::CalibrationData; +using litegrip::describe_error; +using litegrip::GripperConfig; +using litegrip::GripperParams; +using litegrip::GripperState; +using litegrip::GripperStatus; +using litegrip::has_flag; +using litegrip::UnitConversion; + +namespace { + +int g_failures = 0; + +void check(bool condition, const char* what) { + if (!condition) { + std::cerr << "FAIL: " << what << "\n"; + ++g_failures; + } +} + +void check_near(double got, double want, double tol, const char* what) { + if (!(std::fabs(got - want) < tol)) { + std::cerr << "FAIL: " << what << " (got " << got << ", want " << want + << ")\n"; + ++g_failures; + } +} + +bool contains(const std::string& haystack, const char* needle) { + return haystack.find(needle) != std::string::npos; +} + +double monotonic_now() { + return std::chrono::duration( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +} // namespace + +int main() { + // ── GripperState predicates ─────────────────────────────────────────── + { + GripperState enabled; + enabled.error_code = 1; + check(enabled.is_enabled(), "error_code 1 => is_enabled"); + check(!enabled.is_error(), "error_code 1 => not is_error"); + + GripperState disabled; + disabled.error_code = 0; + check(!disabled.is_enabled(), "error_code 0 => not is_enabled"); + check(!disabled.is_error(), "error_code 0 => not is_error"); + + GripperState uv; + uv.error_code = 0x9; + check(!uv.is_enabled(), "error_code 0x9 => not is_enabled"); + check(uv.is_error(), "error_code 0x9 => is_error"); + } + { + GripperState moving; + moving.velocity_rad_s = 1.0; + check(moving.is_moving(), "velocity 1.0 => is_moving"); + + GripperState still; + still.velocity_rad_s = 0.001; + check(!still.is_moving(), "velocity 0.001 => not is_moving"); + } + + // ── GripperConfig defaults and rad <-> mm conversion ────────────────── + { + const GripperConfig cfg; + check(cfg.rad_to_mm > 0.0, "default rad_to_mm positive"); + check(cfg.pos_closed_rad >= 0.0, "default pos_closed_rad non-negative"); + check(cfg.pos_open_rad >= 0.0, "default pos_open_rad non-negative"); + } + { + GripperConfig cfg; + cfg.pos_closed_rad = 0.114; // closed => numerically larger + cfg.pos_open_rad = -1.491; // open => numerically smaller + cfg.rad_to_mm = 74.8; + + const double rad_at_0 = cfg.pos_closed_rad - 0.0 / cfg.rad_to_mm; + check_near(rad_at_0, cfg.pos_closed_rad, 0.001, "conversion at 0 mm"); + + const double rad_at_60 = cfg.pos_closed_rad - 60.0 / cfg.rad_to_mm; + const double mm = (cfg.pos_closed_rad - rad_at_60) * cfg.rad_to_mm; + check_near(mm, 60.0, 0.01, "conversion at 60 mm"); + + const double travel_rad = cfg.pos_closed_rad - cfg.pos_open_rad; + const double travel_mm = travel_rad * cfg.rad_to_mm; + check_near(travel_mm, 120.0, 1.0, "full stroke ~120 mm"); + } + + // ── CalibrationData ─────────────────────────────────────────────────── + { + CalibrationData calib; + calib.zero_position = 0.114; + calib.max_position = -1.491; + calib.travel_range = 1.605; + calib.rad_to_mm = 74.8; + check_near(calib.travel_mm(), 1.605 * 74.8, 0.01, "travel_mm"); + } + + // ── GripperStatus flags ─────────────────────────────────────────────── + { + const GripperStatus flags = + GripperStatus::kEnabled | GripperStatus::kGrasped; + check(has_flag(flags, GripperStatus::kEnabled), "ENABLED flag set"); + check(has_flag(flags, GripperStatus::kGrasped), "GRASPED flag set"); + check(!has_flag(flags, GripperStatus::kMoving), "MOVING flag not set"); + } + + // ── error descriptions (English) ────────────────────────────────────── + check(contains(describe_error(0x0), "disabled"), "describe 0x0"); + check(contains(describe_error(0x1), "enabled"), "describe 0x1"); + check(contains(describe_error(0x8), "overvoltage"), "describe 0x8"); + check(contains(describe_error(0x9), "undervoltage"), "describe 0x9"); + check(contains(describe_error(0xA), "overcurrent"), "describe 0xA"); + check(contains(describe_error(0xB), "over-temperature"), "describe 0xB"); + check(contains(describe_error(0xC), "over-temperature"), "describe 0xC"); + check(contains(describe_error(0xD), "communication loss"), "describe 0xD"); + check(contains(describe_error(0xE), "overload"), "describe 0xE"); + + // Every code the motor can actually report must have a real description: + // 0xD in particular trips after ~0.9 s without a frame. + for (const int code : {0x0, 0x1, 0x8, 0x9, 0xA, 0xB, 0xC, 0xD, 0xE}) { + check(!contains(describe_error(code), "unknown"), + "real codes have no 'unknown' description"); + } + check(contains(describe_error(0xFF), "unknown"), "describe unknown code"); + + // ── staleness ───────────────────────────────────────────────────────── + { + const GripperState fresh; + check(!fresh.has_data(), "default state has no data"); + check(fresh.is_stale(), "default state is stale"); + check(std::isinf(fresh.data_age_s), "default data_age_s is infinite"); + + GripperState live; + live.data_age_s = 0.0; + check(live.has_data(), "age 0 => has_data"); + check(!live.is_stale(), "age 0 => not stale"); + + GripperState old; + old.data_age_s = litegrip::kStaleAfterS + 0.1; + check(old.has_data(), "old state still has data"); + check(old.is_stale(), "age past threshold => stale"); + } + + // ── MotorState ──────────────────────────────────────────────────────── + { + litegrip::can::MotorState motor{}; + check(!motor.has_data(), "new motor has no data"); + check(std::isinf(motor.data_age_s()), "new motor data age is infinite"); + check(motor.rx_count() == 0, "new motor rx_count is 0"); + // Limits are derived from the motor type (DM4310 default). + check_near(motor.limits().q_max, 12.5, 1e-9, "default q_max"); + check_near(motor.limits().dq_max, 30.0, 1e-9, "default dq_max"); + check_near(motor.limits().tau_max, 10.0, 1e-9, "default tau_max"); + } + { + litegrip::can::MotorState motor{}; + motor.update_from_status(1.0, 0.0, 0.0, 1, 30, 28, monotonic_now() - 0.25); + check(motor.has_data(), "motor has data after a status frame"); + check(motor.data_age_s() >= 0.25, "motor data age grows"); + check(motor.rx_count() == 1, "rx_count incremented"); + check(motor.is_enabled(), "error 1 => is_enabled"); + } + { + // Per-type limits: DM6248P has different q/dq/tau maxima. + litegrip::can::MotorParams params; + params.motor_type = litegrip::can::MotorType::kDM6248P; + const litegrip::can::MotorState motor{params}; + check_near(motor.limits().tau_max, 120.0, 1e-9, "DM6248P tau_max"); + } + + // ── UnitConversion ──────────────────────────────────────────────────── + check_near(UnitConversion::kNmToN * UnitConversion::kNToNm, 1.0, 0.01, + "NM_TO_N * N_TO_NM ~ 1"); + check_near(UnitConversion::kRadToMm * UnitConversion::kMmToRad, 1.0, 0.01, + "RAD_TO_MM * MM_TO_RAD ~ 1"); + + // ── GripperParams ───────────────────────────────────────────────────── + check(GripperParams::kMstId == 0x18, "GripperParams MST_ID == 0x18"); + check(GripperParams::kCanId == 0x08, "GripperParams CAN_ID == 0x08"); + + if (g_failures != 0) { + std::cerr << g_failures << " model/motor check(s) failed\n"; + return 1; + } + std::cout << "model/motor golden vectors OK\n"; + return 0; +} diff --git a/test/test_protocol.cpp b/test/test_protocol.cpp new file mode 100644 index 0000000..5b35055 --- /dev/null +++ b/test/test_protocol.cpp @@ -0,0 +1,257 @@ +// test_protocol.cpp — golden vectors lifted from +// litegrip_driver/tests/test_protocol.py. +// +// The point of this file is PARITY: every case below exists in the Python +// suite with the same expected value, so a divergence in the port shows up +// here rather than on hardware. + +#include +#include +#include + +#include "litegrip/can/protocol.hpp" + +using litegrip::can::DmReg; +using litegrip::can::MotorLimits; +using litegrip::can::pack_command_frame; +using litegrip::can::pack_mit_frame; +using litegrip::can::pack_read_param_frame; +using litegrip::can::pack_refresh_frame; +using litegrip::can::pack_save_param_frame; +using litegrip::can::pack_write_param_frame; +using litegrip::can::unpack_param_response; +using litegrip::can::unpack_status_frame; + +namespace { + +int g_failures = 0; + +void check(bool condition, const char* what) { + if (!condition) { + std::cerr << "FAIL: " << what << "\n"; + ++g_failures; + } +} + +void check_eq_int(long long got, long long want, const char* what) { + if (got != want) { + std::cerr << "FAIL: " << what << " (got " << got << ", want " << want + << ")\n"; + ++g_failures; + } +} + +void check_near(double got, double want, double tol, const char* what) { + if (!(std::fabs(got - want) < tol)) { + std::cerr << "FAIL: " << what << " (got " << got << ", want " << want + << ", tol " << tol << ")\n"; + ++g_failures; + } +} + +} // namespace + +int main() { + const MotorLimits dm4310 = litegrip::can::get_motor_limits(1); + + // ── float_to_uint / uint_to_float round-trip ────────────────────────── + { + const auto u = litegrip::can::float_to_uint(0.0, -12.5, 12.5, 16); + const double f = litegrip::can::uint_to_float(u, -12.5, 12.5, 16); + check_near(f, 0.0, 0.001, "roundtrip zero"); + + const auto up = litegrip::can::float_to_uint(6.25, -12.5, 12.5, 16); + check_near(litegrip::can::uint_to_float(up, -12.5, 12.5, 16), 6.25, 0.01, + "roundtrip +6.25"); + + const auto un = litegrip::can::float_to_uint(-6.25, -12.5, 12.5, 16); + check_near(litegrip::can::uint_to_float(un, -12.5, 12.5, 16), -6.25, 0.01, + "roundtrip -6.25"); + } + + // Values beyond the range are clamped. + check_eq_int(litegrip::can::float_to_uint(999.0, -12.5, 12.5, 16), 65535, + "clamp high"); + check_eq_int(litegrip::can::float_to_uint(-999.0, -12.5, 12.5, 16), 0, + "clamp low"); + + // ── pack_mit_frame ──────────────────────────────────────────────────── + { + const auto data = pack_mit_frame(0.0, 0.0, 100.0, 2.0, 0.0, dm4310); + check_eq_int(static_cast(data.size()), 8, "mit frame length"); + } + { + // q = 0 -> 32767 = 0x7FFF, so data[0] = 0x7F, data[1] = 0xFF. + const auto data = pack_mit_frame(0.0, 0.0, 0.0, 0.0, 0.0, dm4310); + check_eq_int(data[0], 0x7F, "zero position byte 0"); + check_eq_int(data[1], 0xFF, "zero position byte 1"); + } + { + const auto pos = pack_mit_frame(12.5, 0.0, 100.0, 2.0, 0.0, dm4310); + check_eq_int(pos[0], 0xFF, "q=+12.5 byte 0"); + check_eq_int(pos[1], 0xFF, "q=+12.5 byte 1"); + + const auto neg = pack_mit_frame(-12.5, 0.0, 100.0, 2.0, 0.0, dm4310); + check_eq_int(neg[0], 0x00, "q=-12.5 byte 0"); + check_eq_int(neg[1], 0x00, "q=-12.5 byte 1"); + } + { + // tau = +10 with tau_max = 10 -> 0xFFF, and kd bits are 0. + const auto data = pack_mit_frame(0.0, 0.0, 0.0, 0.0, 10.0, dm4310); + check_eq_int(data[6] & 0x0F, 0x0F, "tau high nibble"); + check_eq_int(data[7], 0xFF, "tau low byte"); + } + + // ── unpack_status_frame ─────────────────────────────────────────────── + { + const std::uint8_t raw[8] = {0x18, 0x80, 0x00, 0x08, + 0x00, 0x00, 25, 30}; + const auto status = unpack_status_frame(raw, 8, dm4310); + check(status.has_value(), "status frame parses"); + if (status.has_value()) { + check_eq_int(status->err, 1, "status err == 1 (enabled)"); + check_eq_int(status->can_id, 8, "status can_id == 8"); + check_eq_int(status->t_mos, 25, "status t_mos"); + check_eq_int(status->t_coil, 30, "status t_coil"); + } + } + { + const std::uint8_t raw[8] = {0x98, 0x80, 0x00, 0x08, + 0x00, 0x00, 30, 35}; + const auto status = unpack_status_frame(raw, 8, dm4310); + check(status.has_value(), "uv fault frame parses"); + if (status.has_value()) { + check_eq_int(status->err, 0x9, "status err == 0x9 (UV)"); + check_eq_int(status->can_id, 8, "uv frame can_id"); + } + } + { + const std::uint8_t raw[3] = {0x00, 0x00, 0x00}; + check(!unpack_status_frame(raw, 3, dm4310).has_value(), + "short status frame rejected"); + } + + // ── command frames ──────────────────────────────────────────────────── + { + const auto enable = pack_command_frame(litegrip::can::kCmdEnable); + check_eq_int(static_cast(enable.size()), 8, "command frame length"); + check_eq_int(enable[7], litegrip::can::kCmdEnable, "enable byte 7"); + bool all_ff = true; + for (int i = 0; i < 7; ++i) { + all_ff = all_ff && enable[static_cast(i)] == 0xFF; + } + check(all_ff, "enable bytes 0..6 are 0xFF"); + + check_eq_int(pack_command_frame(litegrip::can::kCmdDisable)[7], + litegrip::can::kCmdDisable, "disable byte 7"); + check_eq_int(pack_command_frame(litegrip::can::kCmdClearFault)[7], + litegrip::can::kCmdClearFault, "clear-fault byte 7"); + } + + // ── refresh / read / save frames ────────────────────────────────────── + { + const auto refresh = pack_refresh_frame(0x08); + check_eq_int(static_cast(refresh.size()), 4, "refresh frame length"); + check_eq_int(refresh[0], 0x08, "refresh can_id low"); + check_eq_int(refresh[2], 0xCC, "refresh opcode"); + + const auto read = pack_read_param_frame(0x08, static_cast(DmReg::kMstId)); + check_eq_int(static_cast(read.size()), 8, "read frame length"); + check_eq_int(read[0], 0x08, "read can_id low"); + check_eq_int(read[1], 0x00, "read can_id high"); + check_eq_int(read[2], 0x33, "read opcode"); + check_eq_int(read[3], static_cast(DmReg::kMstId), "read rid"); + + const auto save = pack_save_param_frame(0x08); + check_eq_int(static_cast(save.size()), 8, "save frame length"); + check_eq_int(save[2], 0xAA, "save opcode"); + check_eq_int(save[3], 0x01, "save flag"); + } + + // ── parameter write/read round-trip ─────────────────────────────────── + { + // Float register (KP_ASR = 25). + const auto write = pack_write_param_frame( + 0x08, static_cast(DmReg::kKpAsr), 85.5); + check_eq_int(write[2], 0x55, "write opcode (float reg)"); + const std::uint8_t resp_raw[8] = {0x08, + 0x00, + 0x55, + static_cast(DmReg::kKpAsr), + write[4], + write[5], + write[6], + write[7]}; + const auto resp = unpack_param_response(resp_raw, 8); + check(resp.has_value(), "float param response parses"); + if (resp.has_value()) { + check_eq_int(resp->opcode, 0x55, "float resp opcode"); + check_eq_int(resp->rid, static_cast(DmReg::kKpAsr), "float resp rid"); + check_near(resp->value, 85.5, 0.01, "float resp value"); + } + } + { + // Integer register (MST_ID = 7). + const auto write = pack_write_param_frame( + 0x08, static_cast(DmReg::kMstId), 0x18); + const std::uint8_t resp_raw[8] = {0x08, + 0x00, + 0x33, + static_cast(DmReg::kMstId), + write[4], + write[5], + write[6], + write[7]}; + const auto resp = unpack_param_response(resp_raw, 8); + check(resp.has_value(), "int param response parses"); + if (resp.has_value()) { + check_eq_int(resp->opcode, 0x33, "int resp opcode"); + check_eq_int(resp->rid, static_cast(DmReg::kMstId), "int resp rid"); + check_eq_int(static_cast(resp->value), 0x18, "int resp value"); + } + } + + // ── integer register detection ──────────────────────────────────────── + check(litegrip::can::is_int_register(static_cast(DmReg::kMstId)), + "MST_ID (7) is an int register"); + check(litegrip::can::is_int_register(static_cast(DmReg::kCtrlMode)), + "CTRL_MODE (10) is an int register"); + check(litegrip::can::is_int_register(static_cast(DmReg::kHwVer)), + "hw_ver (13) is an int register"); + check(litegrip::can::is_int_register(static_cast(DmReg::kSn)), + "SN (15) is an int register"); + check(!litegrip::can::is_int_register(static_cast(DmReg::kKpAsr)), + "KP_ASR (25) is a float register"); + check(!litegrip::can::is_int_register(static_cast(DmReg::kKiAsr)), + "KI_ASR (26) is a float register"); + check(litegrip::can::is_int_register(static_cast(DmReg::kCanBr)), + "can_br (35) is an int register"); + check(litegrip::can::is_int_register(static_cast(DmReg::kSubVer)), + "sub_ver (36) is an int register"); + + // ── unpack_param_response edge cases ───────────────────────────────── + { + const std::uint8_t short_raw[3] = {0x08, 0x00, 0x33}; + check(!unpack_param_response(short_raw, 3).has_value(), + "short param response rejected"); + + const std::uint8_t bad_opcode[8] = {0x08, 0x00, 0xFF, 0x07, + 0x00, 0x00, 0x00, 0x00}; + check(!unpack_param_response(bad_opcode, 8).has_value(), + "unknown opcode rejected"); + } + + // ── constants ───────────────────────────────────────────────────────── + check_eq_int(litegrip::can::kBroadcastId, 0x7FF, "BROADCAST_ID"); + check_eq_int(litegrip::can::kCmdEnable, 0xFC, "CMD_ENABLE"); + check_eq_int(litegrip::can::kCmdDisable, 0xFD, "CMD_DISABLE"); + check_eq_int(litegrip::can::kCmdClearFault, 0xFB, "CMD_CLEAR_FAULT"); + check_eq_int(litegrip::can::kCmdSetZero, 0xFE, "CMD_SET_ZERO"); + + if (g_failures != 0) { + std::cerr << g_failures << " protocol check(s) failed\n"; + return 1; + } + std::cout << "protocol golden vectors OK\n"; + return 0; +} diff --git a/test/test_safety.cpp b/test/test_safety.cpp new file mode 100644 index 0000000..973700d --- /dev/null +++ b/test/test_safety.cpp @@ -0,0 +1,541 @@ +// test_safety.cpp — the safety core: limits, guards, modes, latch, baselines. +// +// These are the criteria that decide whether a frame may reach the motor, so +// they are tested directly and adversarially: every "must reject" case below is +// a case where accepting it would move hardware. + +#include +#include +#include +#include +#include +#include +#include + +#include "litegrip/exceptions.hpp" +#include "litegrip/safety.hpp" + +namespace { + +int g_failures = 0; + +void check(bool condition, const char* what) { + if (!condition) { + std::cerr << "FAIL: " << what << "\n"; + ++g_failures; + } +} + +void check_near(double got, double want, double tol, const char* what) { + if (!(std::fabs(got - want) < tol)) { + std::cerr << "FAIL: " << what << " (got " << got << ", want " << want + << ")\n"; + ++g_failures; + } +} + +template +void check_throws(const char* what, Fn&& fn) { + try { + fn(); + std::cerr << "FAIL: " << what << " (did not throw)\n"; + ++g_failures; + } catch (const litegrip::LiteGripError&) { + // expected + } +} + +const double kInf = std::numeric_limits::infinity(); + +std::string write_temp_json(const std::string& name, const std::string& body) { + ::mkdir("/tmp/litegrip_safety_test", 0755); + const std::string path = "/tmp/litegrip_safety_test/" + name; + FILE* file = std::fopen(path.c_str(), "w"); + if (file == nullptr) { + std::cerr << "FAIL: could not write fixture " << path << "\n"; + ++g_failures; + return path; + } + std::fwrite(body.data(), 1, body.size(), file); + std::fclose(file); + return path; +} + +} // namespace + +int main() { + const litegrip::SafetyLimits defaults; + + // ── SafetyLimits self-consistency ───────────────────────────────────── + defaults.validate(); // must not throw + check(defaults.enabled, "defaults have the red lines enabled"); + check(defaults.mech_min_rad < defaults.red_min_rad && + defaults.red_min_rad < defaults.red_max_rad && + defaults.red_max_rad < defaults.mech_max_rad, + "packaged baseline ordering holds"); + { + litegrip::SafetyLimits disabled; + disabled.enabled = false; + check_throws("enabled=false is refused", [&] { disabled.validate(); }); + check_throws("enabled=false is not narrower", + [&] { disabled.assert_no_wider_than(defaults); }); + } + { + litegrip::SafetyLimits reversed; + reversed.red_min_rad = 0.5; // inside-out interval + check_throws("reversed interval is refused", [&] { reversed.validate(); }); + } + + // ── tighten only ────────────────────────────────────────────────────── + defaults.assert_no_wider_than(defaults); // equal is not wider + { + litegrip::SafetyLimits narrowed = defaults; + narrowed.red_min_rad = -1.2; // higher than -1.24 => narrower + narrowed.red_max_rad = -0.05; // lower than -0.01 => narrower + narrowed.assert_no_wider_than(defaults); + narrowed.validate(); + } + { + litegrip::SafetyLimits wider = defaults; + wider.red_min_rad = -1.5; + check_throws("wider red line (open side) is refused", + [&] { wider.assert_no_wider_than(defaults); }); + } + { + litegrip::SafetyLimits wider = defaults; + wider.mech_max_rad = 1.0; + check_throws("wider mechanical bound is refused", + [&] { wider.assert_no_wider_than(defaults); }); + } + { + litegrip::SafetyLimits wider = defaults; + wider.params.kp_max = 900.0; + check_throws("wider kp_max is refused", + [&] { wider.assert_no_wider_than(defaults); }); + } + { + // Direction table: t_comm_s is "smaller = wider", so RAISING it is a + // tightening and must be accepted; lowering it must be refused. + litegrip::SafetyLimits tighter = defaults; + tighter.params.t_comm_s = 0.05; + tighter.assert_no_wider_than(defaults); + + litegrip::SafetyLimits wider = defaults; + wider.params.t_comm_s = 0.005; // below the baseline, i.e. wider + check_throws("lowering t_comm_s is a loosening and is refused", + [&] { wider.assert_no_wider_than(defaults); }); + } + { + litegrip::SafetyLimits tighter = defaults; + tighter.params.safety_reserve_rad = 0.02; + tighter.assert_no_wider_than(defaults); + + litegrip::SafetyLimits wider = defaults; + wider.params.safety_reserve_rad = 0.001; + check_throws("lowering the safety reserve is a loosening and is refused", + [&] { wider.assert_no_wider_than(defaults); }); + } + + // ── strict numeric boundary ─────────────────────────────────────────── + { + const std::string source = "test"; + check_near(litegrip::normalize_scalar(1.5, "x", source), 1.5, 1e-12, + "finite value passes"); + check_throws("NaN is refused", + [&] { litegrip::normalize_scalar(NAN, "x", source); }); + check_throws("+inf is refused", + [&] { litegrip::normalize_scalar(kInf, "x", source); }); + check_throws("-inf is refused", + [&] { litegrip::normalize_scalar(-kInf, "x", source); }); + } + + // ── quantization into the red lines ─────────────────────────────────── + { + const double decoded = litegrip::SafetyLimits::quantize_toward_interior( + defaults.red_min_rad, defaults.red_min_rad, defaults.red_max_rad); + check(decoded >= defaults.red_min_rad && decoded <= defaults.red_max_rad, + "quantized open endpoint lands inside the red lines"); + // The open endpoint is not exactly representable and truncation pushes it + // further out, so the helper steps inward — the result may sit slightly + // ABOVE the commanded value. "Inside the interval" is the guarantee; it is + // not a one-LSB error bound in either direction. + const double one_lsb = 2.0 * litegrip::kProtocolQMaxRad / 65535.0; + check(std::fabs(decoded - defaults.red_min_rad) <= 3.0 * one_lsb, + "quantization stays within a few LSB of the requested endpoint"); + check(decoded > defaults.red_min_rad, + "the open endpoint is nudged inward, not left outside"); + } + { + const double decoded = litegrip::SafetyLimits::quantize_toward_interior( + defaults.red_max_rad, defaults.red_min_rad, defaults.red_max_rad); + check(decoded >= defaults.red_min_rad && decoded <= defaults.red_max_rad, + "quantized closed endpoint lands inside the red lines"); + } + { + const double decoded = litegrip::SafetyLimits::quantize_toward_interior( + -0.5, defaults.red_min_rad, defaults.red_max_rad); + check(std::fabs(decoded - (-0.5)) < 0.001, "mid-range quantization is close"); + } + + // ── v_allow: the deceleration zone ──────────────────────────────────── + { + // At the open red line there is no margin left in that direction. + check_near(defaults.v_allow(defaults.red_min_rad, -1.0), 0.0, 1e-12, + "no margin at the red line => v_allow is 0"); + // Far from the red line, the model's full-margin value emerges. This is the + // number the hard ceiling is supposed to be derived from, and it is checked + // against the ceiling below — so raising one without the other turns red. + check_near(defaults.v_allow(-1.0, -1.0), 1.500330, 1e-5, + "v_allow reproduces the model's open-side value"); + // ★ The ceiling must be REACHABLE by the model. If the ceiling were higher + // than what the stopping-distance model can ever allow, the configuration + // would advertise a speed the gate must refuse — and this is not + // hypothetical: the model SATURATES at (margin - reserve) / t_comm, so at + // the original t_comm = 0.02 the asymptote was 1.3897 rad/s and the 1.5 + // ceiling would have been unreachable at any a_max whatsoever. + check(litegrip::kMaxCommandVelocityCeilingRadS <= + defaults.v_allow(-1.0, -1.0) + 1e-9, + "the hard ceiling exceeds what the stopping-distance model can allow; " + "raise a_max_rad_s2 / lower t_comm_s first, or lower the ceiling"); + // Closing side has more margin, so a higher allowance. + check(defaults.v_allow(-0.5, 1.0) > defaults.v_allow(-1.0, -1.0), + "closing side allows more speed than the open side"); + // No motion intent => no speed limit. + check_near(defaults.v_allow(-1.0, 0.0), defaults.params.recovery_dq_max, + 1e-12, "direction 0 yields the recovery ceiling"); + check_near(defaults.v_allow(NAN, -1.0), defaults.params.recovery_dq_max, + 1e-12, "non-finite position yields the recovery ceiling"); + } + { + // Overflow guard: an absurd comms delay must fail closed, not emit inf/nan + // (for which `|dq| > v_lim` is always false and the gate would vanish). + litegrip::SafetyLimits absurd; + absurd.params.t_comm_s = 1e160; + const double v = absurd.v_allow(-1.0, -1.0); + check(std::isfinite(v), "v_allow stays finite under an absurd t_comm_s"); + check_near(v, 0.0, 1e-12, "v_allow fails closed under an absurd t_comm_s"); + } + + // ── containment / mm range ──────────────────────────────────────────── + check(defaults.contains_red(-0.5), "red contains the midpoint"); + check(defaults.contains_red(defaults.red_min_rad), "red endpoints are closed"); + check(defaults.contains_red(defaults.red_max_rad), "red endpoints are closed"); + check(!defaults.contains_red(-1.3), "red excludes values beyond the open line"); + check(!defaults.contains_red(NAN), "red excludes NaN"); + check(defaults.contains_mech(-1.27), "mech contains near its bound"); + check(!defaults.contains_mech(0.1), "mech excludes beyond its bound"); + { + const auto range = defaults.mm_range(0.114, 74.8); + check(range.second > range.first, "mm range is ordered"); + check_near(range.first, 74.8 * (0.114 - defaults.red_max_rad), 1e-9, + "mm range lower edge"); + } + + // ── SafetyGuard: modes ──────────────────────────────────────────────── + { + litegrip::SafetyGuard guard{defaults}; + check(guard.mode() == litegrip::FrameMode::kNormal, "starts in normal mode"); + check_throws("kNormal cannot be pushed", + [&] { guard.push_mode(litegrip::FrameMode::kNormal); }); + check_throws("cannot pop the bottom of the stack", + [&] { guard.pop_mode(); }); + + guard.push_mode(litegrip::FrameMode::kMaintenance); + check(guard.mode() == litegrip::FrameMode::kMaintenance, "mode pushed"); + guard.push_mode(litegrip::FrameMode::kZeroGravity); + check(guard.mode() == litegrip::FrameMode::kZeroGravity, "modes nest"); + check(guard.pop_mode() == litegrip::FrameMode::kZeroGravity, "pop returns"); + check(guard.mode() == litegrip::FrameMode::kMaintenance, "nesting unwinds"); + check(guard.pop_mode() == litegrip::FrameMode::kMaintenance, "pop returns"); + check(guard.mode() == litegrip::FrameMode::kNormal, "back to normal"); + } + { + // The RAII scope must restore the mode even when unwound by an exception. + litegrip::SafetyGuard guard{defaults}; + bool restored = false; + try { + auto scope = guard.zero_gravity_scope("test"); + check(guard.mode() == litegrip::FrameMode::kZeroGravity, "scope entered"); + throw litegrip::CommandError("boom"); + } catch (const litegrip::LiteGripError&) { + restored = guard.mode() == litegrip::FrameMode::kNormal; + } + check(restored, "mode scope unwinds on an exception"); + } + + // ── guard_motion_frame ──────────────────────────────────────────────── + { + litegrip::SafetyGuard guard{defaults}; + // Inside: measured inside, target inside, sane gains, no velocity intent. + const double safe = guard.guard_motion_frame(-0.5, 50.0, 1.0, 0.0, 0.0, -0.5, + 0.0, 0.0); + check(std::fabs(safe - (-0.5)) < 0.001, "a legal motion frame passes"); + + // Measured outside the red lines: refused even though the target points in. + check_throws("measured outside the red lines is refused", + [&] { + guard.guard_motion_frame(-0.5, 50.0, 1.0, 0.0, 0.0, -1.3, 0.0, + 0.0); + }); + // No feedback at all: refused. + check_throws("missing measured position is refused", + [&] { + guard.guard_motion_frame(-0.5, 50.0, 1.0, 0.0, 0.0, + std::nullopt, 0.0, 0.0); + }); + // Target beyond the red lines: refused. + check_throws("target beyond the red lines is refused", + [&] { + guard.guard_motion_frame(-0.005, 50.0, 1.0, 0.0, 0.0, -0.5, 0.0, + 0.0); + }); + // Target beyond the mechanical range: refused (different severity). + check_throws("target beyond the mechanical range is refused", + [&] { + guard.guard_motion_frame(-1.5, 50.0, 1.0, 0.0, 0.0, -0.5, 0.0, + 0.0); + }); + // Gains over their ceilings: refused. + check_throws("kp above the ceiling is refused", + [&] { + guard.guard_motion_frame(-0.5, 300.0, 1.0, 0.0, 0.0, -0.5, 0.0, + 0.0); + }); + check_throws("kd above the ceiling is refused", + [&] { + guard.guard_motion_frame(-0.5, 50.0, 9.0, 0.0, 0.0, -0.5, 0.0, + 0.0); + }); + // Feed-forward above the torque ceiling: refused. + check_throws("feed-forward torque above the ceiling is refused", + [&] { + guard.guard_motion_frame(-0.5, 50.0, 1.0, 0.0, 5.0, -0.5, 0.0, + 0.0); + }); + // Velocity beyond the deceleration zone: refused. + check_throws("velocity beyond the deceleration zone is refused", + [&] { + guard.guard_motion_frame(-0.5, 50.0, 1.0, 5.0, 0.0, -0.5, 0.0, + 0.0); + }); + // Missing velocity feedback: refused. + check_throws("missing velocity feedback is refused", + [&] { + guard.guard_motion_frame(-0.5, 50.0, 1.0, 0.0, 0.0, -0.5, + std::nullopt, 0.0); + }); + // Non-finite inputs never reach a comparison. + check_throws("NaN target is refused", + [&] { + guard.guard_motion_frame(NAN, 50.0, 1.0, 0.0, 0.0, -0.5, 0.0, + 0.0); + }); + + // Measured torque already over the ceiling: only a frame that pushes the + // SAME way is refused; one that unloads is allowed. + check_throws("over-torque while still pushing is refused", + [&] { + guard.guard_motion_frame(-0.4, 50.0, 1.0, 0.0, 0.0, -0.5, -0.5, + 5.0); + }); + const double unloading = guard.guard_motion_frame(-0.6, 50.0, 1.0, 0.0, 0.0, + -0.5, 0.5, 5.0); + check(std::isfinite(unloading), "over-torque while unloading is allowed"); + } + { + // Zero-gravity mode refuses motion frames outright. + litegrip::SafetyGuard guard{defaults}; + auto scope = guard.zero_gravity_scope("test"); + check_throws("zero-gravity refuses motion frames", + [&] { + guard.guard_motion_frame(-0.5, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0, + 0.0); + }); + } + + // ── guard_zero_torque_frame / zero_torque_q ─────────────────────────── + { + litegrip::SafetyGuard guard{defaults}; + guard.guard_zero_torque_frame(0.0, 0.0, 0.0, 0.0); // must not throw + check_throws("non-zero kp in a zero-torque frame is refused", + [&] { guard.guard_zero_torque_frame(1.0, 0.0, 0.0, 0.0); }); + check_throws("non-zero tau in a zero-torque frame is refused", + [&] { guard.guard_zero_torque_frame(0.0, 0.0, 0.0, 0.1); }); + check_throws("NaN in a zero-torque frame is refused", + [&] { guard.guard_zero_torque_frame(0.0, NAN, 0.0, 0.0); }); + } + { + using litegrip::SafetyGuard; + check_near(SafetyGuard::zero_torque_q(std::nullopt, false), + litegrip::kZeroTorqueQFallback, 1e-12, "no feedback => fallback"); + check_near(SafetyGuard::zero_torque_q(0.5, true), 0.5, 1e-12, + "usable feedback is used"); + check_near(SafetyGuard::zero_torque_q(NAN, true), + litegrip::kZeroTorqueQFallback, 1e-12, "NaN => fallback"); + check_near(SafetyGuard::zero_torque_q(20.0, true), + litegrip::kZeroTorqueQFallback, 1e-12, + "beyond the protocol range => fallback"); + // Outside the red lines is still a valid placeholder: that is exactly the + // situation an emergency stop has to work in. + check_near(SafetyGuard::zero_torque_q(-1.6, true), -1.6, 1e-12, + "position outside the red lines is still used"); + } + + // ── guard_recovery_frame ────────────────────────────────────────────── + { + litegrip::SafetyGuard guard{defaults}; + // Already inside: nothing to recover. + check_throws("recovery from inside the red lines is refused", + [&] { + guard.guard_recovery_frame(-0.5, 10.0, 1.0, 0.0, 0.0, -0.5, 0.0); + }); + // Outside and moving further out: refused. + check_throws("recovery that moves further out is refused", + [&] { + guard.guard_recovery_frame(-1.3, 10.0, 1.0, -0.1, 0.0, -1.25, + 0.0); + }); + // Outside and moving back in, within the recovery ceilings: allowed. + const double target = + guard.guard_recovery_frame(-1.2, 10.0, 1.0, 0.1, 0.0, -1.25, 0.0); + check_near(target, -1.2, 1e-12, "inward recovery is allowed"); + // Recovery gains / speeds / torques have their own much tighter ceilings. + check_throws("recovery kp above its ceiling is refused", + [&] { + guard.guard_recovery_frame(-1.2, 100.0, 1.0, 0.0, 0.0, -1.25, + 0.0); + }); + check_throws("recovery velocity above its ceiling is refused", + [&] { + guard.guard_recovery_frame(-1.2, 10.0, 1.0, 1.0, 0.0, -1.25, + 0.0); + }); + check_throws("recovery torque above its ceiling is refused", + [&] { + guard.guard_recovery_frame(-1.2, 10.0, 1.0, 0.0, 1.0, -1.25, + 0.0); + }); + // Beyond the mechanical range: no automatic recovery. + check_throws("recovery from beyond the mechanical range is refused", + [&] { + guard.guard_recovery_frame(-1.2, 10.0, 1.0, 0.0, 0.0, -1.35, + 0.0); + }); + } + + // ── latch and watchdog ──────────────────────────────────────────────── + { + litegrip::SafetyGuard guard{defaults}; + check(!guard.is_fault_latched(), "starts unlatched"); + check(guard.is_watchdog_armed(), "starts armed"); + + guard.latch_fault("first reason"); + guard.latch_fault("second reason"); + check(guard.is_fault_latched(), "latched"); + check(guard.latch_reason() == "first reason", + "the first latch reason is kept"); + check_throws("a latched guard refuses motion frames", + [&] { + guard.guard_motion_frame(-0.5, 50.0, 1.0, 0.0, 0.0, -0.5, 0.0, + 0.0); + }); + + guard.clear_safety_latch(); + check(!guard.is_fault_latched(), "clear_safety_latch unlatches"); + check(!guard.is_watchdog_armed(), + "clearing the latch disarms the watchdog (re-arms inside the red " + "lines)"); + guard.guard_feedback_position(-0.5); // back inside + check(guard.is_watchdog_armed(), "watchdog re-arms once back inside"); + } + { + // Feedback outside the red lines latches in normal mode... + litegrip::SafetyGuard guard{defaults}; + check_throws("feedback outside the red lines latches", + [&] { guard.guard_feedback_position(-1.25); }); + check(guard.is_fault_latched(), "the crossing latched the fault"); + } + { + // ...but only warns in zero-gravity mode (hand-pushing crosses red lines by + // design), while still latching beyond the mechanical range. + litegrip::SafetyGuard guard{defaults}; + { + auto scope = guard.zero_gravity_scope("test"); + guard.guard_feedback_position(-1.25); // must not throw + check(!guard.is_fault_latched(), "zero-gravity does not latch a crossing"); + check_throws("beyond the mechanical range latches in every mode", + [&] { guard.guard_feedback_position(-1.35); }); + } + } + + // ── baseline loading ────────────────────────────────────────────────── + { + const litegrip::SafetyLimits baseline = litegrip::load_safety_baseline(); + check_near(baseline.red_min_rad, litegrip::kPackageRedMin, 1e-12, + "3.5 baseline open red line"); + check_near(baseline.red_max_rad, litegrip::kPackageRedMax, 1e-12, + "3.5 baseline closed red line"); + check_near(baseline.params.tau_max_nm, 3.5, 1e-12, "3.5 baseline torque"); + + const litegrip::SafetyLimits rollback = litegrip::load_safety_baseline("0.25"); + check_near(rollback.params.tau_max_nm, 0.25, 1e-12, + "0.25 rollback baseline torque"); + check_near(rollback.red_min_rad, baseline.red_min_rad, 1e-12, + "the two baselines share the same red lines"); + + check_throws("an unknown baseline version is refused", + [] { litegrip::load_safety_baseline("9.9"); }); + check_throws("an explicit missing baseline file is refused", + [] { litegrip::load_safety_baseline("/tmp/nope/none.json"); }); + } + { + // A config file may narrow... + const std::string narrowed = write_temp_json( + "narrow.json", + "{\"mechanical_observed_min_rad\":-1.27," + "\"mechanical_observed_max_rad\":0.05," + "\"red_open_limit_rad\":-1.2,\"red_close_limit_rad\":-0.05}"); + const auto loaded = litegrip::load_safety_limits(narrowed); + check(loaded.has_value(), "a narrowing config loads"); + if (loaded.has_value()) { + check_near(loaded->red_min_rad, -1.2, 1e-12, "narrowed open red line"); + } + + // ...but never widen. + const std::string widened = write_temp_json( + "widen.json", + "{\"mechanical_observed_min_rad\":-1.27," + "\"mechanical_observed_max_rad\":0.05," + "\"red_open_limit_rad\":-1.5,\"red_close_limit_rad\":0.5}"); + check_throws("a widening config is refused", + [&] { litegrip::load_safety_limits(widened); }); + + // Disabling the red lines is refused with an actionable message. + const std::string disabled = write_temp_json( + "disabled.json", + "{\"mechanical_observed_min_rad\":-1.27," + "\"mechanical_observed_max_rad\":0.05," + "\"red_open_limit_rad\":-1.24,\"red_close_limit_rad\":-0.01," + "\"red_line_enabled\":false}"); + check_throws("red_line_enabled=false is refused", + [&] { litegrip::load_safety_limits(disabled); }); + + // Missing required fields. + const std::string incomplete = + write_temp_json("incomplete.json", "{\"red_open_limit_rad\":-1.24}"); + check_throws("a config missing required fields is refused", + [&] { litegrip::load_safety_limits(incomplete); }); + + // A path that does not exist yields nullopt (the contract the caller must + // handle), not a silent fallback. + check(!litegrip::load_safety_limits("/tmp/nope/none.json").has_value(), + "a missing config yields nullopt"); + } + + if (g_failures != 0) { + std::cerr << g_failures << " safety check(s) failed\n"; + return 1; + } + std::cout << "safety checks OK\n"; + return 0; +} diff --git a/test/test_transport.cpp b/test/test_transport.cpp new file mode 100644 index 0000000..604f894 --- /dev/null +++ b/test/test_transport.cpp @@ -0,0 +1,110 @@ +// test_transport.cpp — SocketCAN transport checks that do NOT transmit. +// +// Deliberately non-intrusive: a live gripper (can0 up, carrier present) may be +// attached to this machine, so this test only ever +// * reads an interface MTU via ioctl, +// * exercises the error path for a missing interface, +// * opens and closes a socket on a real CAN interface (bind only). +// It never calls send(), so it cannot disturb a running stack. +// +// The interface-dependent part is skipped (not failed) when can0 is absent, so +// the suite still works on machines without CAN hardware. + +#include +#include +#include + +#include +#include + +#include "litegrip/can/transport.hpp" +#include "litegrip/exceptions.hpp" + +namespace { + +int g_failures = 0; + +void check(bool condition, const char* what) { + if (!condition) { + std::cerr << "FAIL: " << what << "\n"; + ++g_failures; + } +} + +} // namespace + +int main() { + // ── iface_mtu on a known interface ──────────────────────────────────── + { + const int fd = ::socket(AF_INET, SOCK_DGRAM, 0); + check(fd >= 0, "throwaway socket created"); + if (fd >= 0) { + const auto mtu = litegrip::can::iface_mtu(fd, "lo"); + check(mtu.has_value(), "loopback MTU readable"); + if (mtu.has_value()) { + check(*mtu > 0, "loopback MTU positive"); + } + const auto bogus = litegrip::can::iface_mtu(fd, "lg_no_such_iface"); + check(!bogus.has_value(), "missing interface yields no MTU"); + ::close(fd); + } + } + + // ── opening a missing interface must throw ConnectError ─────────────── + { + litegrip::can::CanTransport transport("lg_no_such_iface"); + bool threw = false; + try { + transport.open(); + } catch (const litegrip::ConnectError&) { + threw = true; + } catch (const litegrip::LiteGripError&) { + threw = true; + } + check(threw, "opening a missing interface throws"); + check(!transport.is_open(), "transport stays closed after a failed open"); + } + + // ── send before open must throw, not corrupt anything ───────────────── + { + litegrip::can::CanTransport transport("can0"); + bool threw = false; + try { + const std::uint8_t payload[8] = {0, 0, 0, 0, 0, 0, 0, 0}; + transport.send(0x08, payload, sizeof(payload)); + } catch (const litegrip::LiteGripError&) { + threw = true; + } + check(threw, "send on a closed transport throws"); + } + + // ── open/close a real CAN interface (bind only, no traffic) ─────────── + const bool can0_present = ::if_nametoindex("can0") != 0; + if (!can0_present) { + std::cout << "can0 not present; interface open/close check skipped\n"; + } else { + litegrip::can::CanTransport transport("can0"); + transport.open(); + check(transport.is_open(), "can0 opens"); + check(transport.channel() == "can0", "channel name preserved"); + // can0 is classic CAN (MTU 16) here, so the requested mode must survive + // reconciliation. + check(transport.mode() == litegrip::can::CanMode::kCan, + "classic CAN interface keeps classic mode"); + transport.close(); + check(!transport.is_open(), "can0 closes"); + + // Re-opening must be idempotent rather than leaking a socket. + transport.open(); + transport.open(); + check(transport.is_open(), "re-open is idempotent"); + transport.close(); + } + + if (g_failures != 0) { + std::cerr << g_failures << " transport check(s) failed\n"; + return 1; + } + std::cout << "transport checks OK (no frames transmitted)\n"; + return 0; +} diff --git a/test/test_version.cpp b/test/test_version.cpp new file mode 100644 index 0000000..1d3d8f5 --- /dev/null +++ b/test/test_version.cpp @@ -0,0 +1,20 @@ +// test_version.cpp — smoke test that the header/library wiring works at all. +// Deliberately dependency-free (no gtest): returns 1 on failure. + +#include +#include + +#include "litegrip/version.hpp" + +int main() { + if (std::strlen(litegrip::version()) == 0) { + std::cerr << "FAIL: version() returned an empty string\n"; + return 1; + } + if (litegrip::version_major() != LITEGRIP_CPP_VERSION_MAJOR) { + std::cerr << "FAIL: version_major() disagrees with the macro\n"; + return 1; + } + std::cout << "litegrip_cpp " << litegrip::version() << " OK\n"; + return 0; +}