From dd3fdf724076b86ff48c6665e7b2cb14c6dc60f3 Mon Sep 17 00:00:00 2001 From: Stephen Lin Date: Mon, 27 Apr 2026 08:56:43 -0700 Subject: [PATCH] feat(fairino): add fr10 actuatorbackend skeleton (milestone 1, read-only); register fairino + fr10 robotconfig; option b frame guard; 40 unit tests - src/gradient_os/arm_controller/backends/fairino/: backend, rpc_client (sdk quarantine), state_stream (50hz daemon poller), frame_guard (refuses non-identity end_effector_offset), config (env overrides) - src/gradient_os/arm_controller/robots/fr10/: fr10config, default_servo_backend=fairino, 6 joints - backends/__init__.py + robots/__init__.py: register fairino backend and fr10 robot - pyproject.toml: [fairino] optional extra - tests/test_fairino_backend.py: 40 tests with fakefairinosdk; covers frame guard, rpc lifecycle, state stream timing/error resilience, motion-stub raises, registry wiring, env-var config - docs/readme.md: brass-tacks notes on supported robots and fairino bring-up --- docs/README.md | 36 + pyproject.toml | 7 + .../arm_controller/backends/__init__.py | 22 +- .../backends/fairino/__init__.py | 30 + .../backends/fairino/backend.py | 253 +++++++ .../arm_controller/backends/fairino/config.py | 158 +++++ .../backends/fairino/frame_guard.py | 79 +++ .../backends/fairino/rpc_client.py | 220 ++++++ .../backends/fairino/state_stream.py | 151 +++++ .../arm_controller/robots/__init__.py | 3 + .../arm_controller/robots/fr10/__init__.py | 9 + .../arm_controller/robots/fr10/config.py | 179 +++++ tests/test_fairino_backend.py | 634 ++++++++++++++++++ 13 files changed, 1780 insertions(+), 1 deletion(-) create mode 100644 src/gradient_os/arm_controller/backends/fairino/__init__.py create mode 100644 src/gradient_os/arm_controller/backends/fairino/backend.py create mode 100644 src/gradient_os/arm_controller/backends/fairino/config.py create mode 100644 src/gradient_os/arm_controller/backends/fairino/frame_guard.py create mode 100644 src/gradient_os/arm_controller/backends/fairino/rpc_client.py create mode 100644 src/gradient_os/arm_controller/backends/fairino/state_stream.py create mode 100644 src/gradient_os/arm_controller/robots/fr10/__init__.py create mode 100644 src/gradient_os/arm_controller/robots/fr10/config.py create mode 100644 tests/test_fairino_backend.py diff --git a/docs/README.md b/docs/README.md index d38e4cd2..3651bcf5 100644 --- a/docs/README.md +++ b/docs/README.md @@ -142,11 +142,47 @@ If scripts are not on PATH, run through modules: ## Project Layout - `src/gradient_os/arm_controller/` - controller runtime and command handlers +- `src/gradient_os/arm_controller/backends/` - actuator backends (`feetech`, `ethercat_rtcore`, `fairino`, `simulation`) +- `src/gradient_os/arm_controller/robots/` - robot configs (`gradient0`, `fr10`) - `src/gradient_os/api/` - HTTP/SSE API - `src/gradient_os/cad/` - topology extraction and STEP-driven planning helpers - `src/gradient_os/vision/` - vision pipeline - `web-ui/` - React operator interface - `docs/` - subsystem docs and references + +## Supported Robots + +Pick a robot at startup with `--robot `. Each robot config picks its own +default backend, so `--robot fr10` just works. + +| Robot | Backend | What it talks to | +|---|---|---| +| `gradient0` | `feetech` | Feetech serial-bus servos over USB | +| `gradient0` | `ethercat_rtcore` | `gradient-rt-motion` daemon (PREEMPT_RT) | +| `fr10` | `fairino` | Fairino FR10 controller at `192.168.58.2` (network client) | +| (any) | `simulation` | In-memory mock | + +### Fairino FR10 (Milestone 1: read-only) + +The FR10 backend is a **network client** — the FR10 owns its own real-time +loop, calibration, and PID. GradientOS is just a planner and UI on top. + +- Set `END_EFFECTOR_OFFSET` to identity in `src/gradient_os/ik_solver.py` + before starting Fairino. The FR10 owns the tool transform via + `toolcoord1`; doubling up causes a fixture crash. The backend refuses to + start otherwise. +- Install the SDK extra: `uv pip install -e '.[fairino]'`. +- Walk the three `TODO(SDK):` markers in + `arm_controller/backends/fairino/rpc_client.py` against the installed + Fairino wheel. +- Bring-up runbook lives in the `Fairino-FR10` repo at + `Docs/runbooks/milestone_1_readonly_bringup.md`. + +Env overrides for the Fairino backend (all optional): +`GRADIENT_FAIRINO_IP`, `GRADIENT_FAIRINO_RPC_PORT`, +`GRADIENT_FAIRINO_STATE_PORT`, `GRADIENT_FAIRINO_POLL_HZ`, +`GRADIENT_FAIRINO_CONNECT_TIMEOUT_S`, +`GRADIENT_FAIRINO_STALE_SNAPSHOT_S`. - `tests/` - automated tests ## Documentation Map diff --git a/pyproject.toml b/pyproject.toml index 53a0f5bb..3d062936 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -66,6 +66,13 @@ vision = [ picamera = [ "picamera2==0.3.30 ; platform_system == \"Linux\"", ] +fairino = [ + # Fairino FR-series Python SDK. Pin pending verification of the exact PyPI + # package name and version once the SDK is installed locally — see + # src/gradient_os/arm_controller/backends/fairino/rpc_client.py for the + # quarantined call sites. + "fairino", +] ai = [ "ultralytics", "torch", diff --git a/src/gradient_os/arm_controller/backends/__init__.py b/src/gradient_os/arm_controller/backends/__init__.py index fa622ed5..6276b6e3 100644 --- a/src/gradient_os/arm_controller/backends/__init__.py +++ b/src/gradient_os/arm_controller/backends/__init__.py @@ -35,6 +35,7 @@ # Import backend classes from .feetech import FeetechBackend from .ethercat_rtcore import EthercatRTCoreBackend +from .fairino import FairinoBackend from .simulation import SimulationBackend # ============================================================================= @@ -79,6 +80,18 @@ def _create_simulation_backend(robot_config: dict, **kwargs) -> SimulationBacken robot_config=robot_config, # Pass full config for servo ID mapping ) +def _create_fairino_backend(robot_config: dict, **kwargs) -> FairinoBackend: + """ + Factory for FairinoBackend (network client targeting an FR10 controller). + + The FR10 owns its own real-time loop, calibration, and PID, so the bulk + of `robot_config` is unused here — only `num_logical_joints` is checked. + All network endpoints come from FairinoBackendConfig.from_env() (see + backends/fairino/config.py for the list of GRADIENT_FAIRINO_* env vars). + """ + return FairinoBackend(robot_config=robot_config) + + def _create_ethercat_rtcore_backend(robot_config: dict, **kwargs) -> EthercatRTCoreBackend: """ Factory for EthercatRTCoreBackend (RTCore proxy). @@ -119,4 +132,11 @@ def _create_ethercat_rtcore_backend(robot_config: dict, **kwargs) -> EthercatRTC config_module_path="gradient_os.arm_controller.backends.ethercat_rtcore.config", ) -__all__ = ['FeetechBackend', 'SimulationBackend', 'EthercatRTCoreBackend', 'registry'] +# Fairino FR-series network client (FR10) +registry.register_backend_class( + name="fairino", + factory=_create_fairino_backend, + config_module_path="gradient_os.arm_controller.backends.fairino.config", +) + +__all__ = ['FeetechBackend', 'SimulationBackend', 'EthercatRTCoreBackend', 'FairinoBackend', 'registry'] diff --git a/src/gradient_os/arm_controller/backends/fairino/__init__.py b/src/gradient_os/arm_controller/backends/fairino/__init__.py new file mode 100644 index 00000000..c9d29c92 --- /dev/null +++ b/src/gradient_os/arm_controller/backends/fairino/__init__.py @@ -0,0 +1,30 @@ +# backends/fairino/__init__.py +# +# Fairino FR-series ActuatorBackend (FR10 in particular). +# Read-only at Milestone 1: connects to the FR10 over its Python SDK and +# streams joint state into GradientOS for UI display. Motion commands land +# in Milestone 2. +# +# Layout: +# - backend.py : FairinoBackend(ActuatorBackend) — the integration point +# - rpc_client.py : quarantined Fairino SDK calls +# - state_stream.py : daemon-thread state poller + thread-safe snapshot cache +# - frame_guard.py : Option B coordinate-strategy enforcement +# - config.py : network endpoints, poll rates, env-var overrides + +from .backend import FairinoBackend +from .config import FairinoBackendConfig +from .frame_guard import FrameStrategyViolation +from .rpc_client import FairinoRPCClient, FairinoSDKAdapter, FairinoStatusFlags +from .state_stream import FairinoStateStream, StateSnapshot + +__all__ = [ + "FairinoBackend", + "FairinoBackendConfig", + "FairinoRPCClient", + "FairinoSDKAdapter", + "FairinoStateStream", + "FairinoStatusFlags", + "FrameStrategyViolation", + "StateSnapshot", +] diff --git a/src/gradient_os/arm_controller/backends/fairino/backend.py b/src/gradient_os/arm_controller/backends/fairino/backend.py new file mode 100644 index 00000000..0928474c --- /dev/null +++ b/src/gradient_os/arm_controller/backends/fairino/backend.py @@ -0,0 +1,253 @@ +# backends/fairino/backend.py +# +# Read-only ActuatorBackend for the Fairino FR-series (FR10 in particular). +# Milestone 1 scope: connect to the FR10 controller over the network, stream +# joint state into GradientOS so the web UI can display it. NO motion yet — +# every method that would command the robot raises NotImplementedError with +# "Milestone N" pointing at when it lands. +# +# Architecture sketch: +# +# FairinoBackend +# ├── FairinoRPCClient (rpc_client.py — quarantined SDK calls) +# ├── FairinoStateStream (state_stream.py — daemon poller + snapshot) +# └── frame_guard (frame_guard.py — Option B enforcement) +# +# This backend is much thinner than feetech (no register layouts, no +# encoder math) because the FR10 owns its own real-time loop, calibration, +# joint limits, PID gains, and safety stack. We are a network client. + +from __future__ import annotations + +from typing import Optional + +from ...actuator_interface import ActuatorBackend +from .config import FR10_NUM_JOINTS, FairinoBackendConfig +from .frame_guard import assert_identity_end_effector_offset +from .rpc_client import FairinoRPCClient +from .state_stream import FairinoStateStream + + +class FairinoBackend(ActuatorBackend): + """ActuatorBackend implementation for the Fairino FR-series (read-only). + + Milestone 1: state streaming only. Initialize() runs the Option B frame + guard, then connects to the FR10 over its Python SDK and starts a 50 Hz + state poller. `get_joint_positions()` reads from the latest snapshot. + All motion-command methods raise NotImplementedError — wired up in + Milestone 2 (`set_joint_positions` → `MoveJ`) and beyond. + + Thread safety: every method on this class can be called concurrently + with the poller thread; the underlying RPCClient and StateStream + serialize their own internal state. + """ + + def __init__( + self, + robot_config: dict, + backend_config: Optional[FairinoBackendConfig] = None, + rpc_client: Optional[FairinoRPCClient] = None, + state_stream: Optional[FairinoStateStream] = None, + ) -> None: + # `robot_config` mirrors the dict produced by RobotConfig.get_config_dict(). + # Most fields don't apply to the FR10 (servo IDs, encoder mappings, PID + # gains all live inside the FR10 controller itself); we only touch the + # joint-count field for sanity checking. + configured_joints = int(robot_config.get("num_logical_joints", FR10_NUM_JOINTS)) + if configured_joints != FR10_NUM_JOINTS: + raise ValueError( + f"FairinoBackend expects num_logical_joints={FR10_NUM_JOINTS} " + f"(FR10 is a 6-DOF arm), got {configured_joints}. Check the " + "active RobotConfig — you probably want robots/fr10." + ) + + self._robot_config = robot_config + self._backend_config = backend_config or FairinoBackendConfig.from_env() + + self._rpc = rpc_client or FairinoRPCClient( + ip=self._backend_config.controller_ip, + connect_timeout_s=self._backend_config.connect_timeout_s, + ) + self._stream = state_stream or FairinoStateStream( + rpc=self._rpc, + poll_hz=self._backend_config.state_poll_hz, + ) + + self._initialized = False + + # ========================================================================= + # Lifecycle + # ========================================================================= + + def initialize(self) -> bool: + # Frame guard FIRST — before any network I/O. If the user has a + # non-identity END_EFFECTOR_OFFSET, we should fail before anything + # touches the robot. + try: + assert_identity_end_effector_offset() + except Exception as e: + # Print loudly and re-raise — this is operator-actionable, not + # a "degraded mode" we recover from. Letting it propagate makes + # the failure visible in run_controller.py's startup log. + print(f"[FairinoBackend] FRAME STRATEGY VIOLATION: {e}") + raise + + try: + self._rpc.connect() + except Exception as e: + print( + f"[FairinoBackend] WARNING: connect to FR10 at " + f"{self._backend_config.controller_ip} failed: {e}" + ) + self._initialized = False + return False + + self._stream.start() + self._initialized = True + print( + f"[FairinoBackend] Connected to FR10 at " + f"{self._backend_config.controller_ip} " + f"(state poll {self._backend_config.state_poll_hz} Hz)" + ) + return True + + def shutdown(self) -> None: + try: + self._stream.stop() + except Exception as e: + print(f"[FairinoBackend] state stream stop raised: {e}") + try: + self._rpc.disconnect() + except Exception as e: + print(f"[FairinoBackend] rpc disconnect raised: {e}") + self._initialized = False + + @property + def num_joints(self) -> int: + return FR10_NUM_JOINTS + + @property + def is_initialized(self) -> bool: + return self._initialized and self._rpc.is_connected + + @property + def encoder_resolution(self) -> int: + # The FR10 controller doesn't expose servo encoder counts to the + # network client — we work in radians end-to-end. Returning 0 (same + # as ethercat_rtcore) tells any legacy raw-encoder code to back off. + return 0 + + # ========================================================================= + # State (Milestone 1 surface) + # ========================================================================= + + def get_joint_positions(self, verbose: bool = False) -> list[float]: + """Latest 6 joint angles in radians. Never blocks.""" + snapshot = self._stream.latest() + if verbose: + age = snapshot.age_s() + state = "fresh" if age <= self._backend_config.stale_snapshot_s else "STALE" + print( + f"[FairinoBackend] get_joint_positions -> {snapshot.joint_positions_rad} " + f"({state}, age={age:.3f}s, seq={snapshot.sequence})" + ) + + if not snapshot.has_reading: + # State hasn't arrived yet (we're between connect and the first + # poll). Return zeros rather than raising — the UI must keep + # rendering during the bring-up window. + return [0.0] * FR10_NUM_JOINTS + + return list(snapshot.joint_positions_rad) + + def get_present_actuator_ids(self) -> set[int]: + # The FR10 doesn't have user-visible actuator IDs the way Feetech + # does. We expose 1..6 as synthetic IDs so the rest of GradientOS + # has something stable to key on (e.g. for telemetry rows). + if not self.is_initialized: + return set() + return set(range(1, FR10_NUM_JOINTS + 1)) + + def ping_actuator(self, actuator_id: int) -> bool: + return actuator_id in self.get_present_actuator_ids() + + # ========================================================================= + # Motion commands — Milestone 2+ + # ========================================================================= + + def set_joint_positions( + self, + positions_rad: list[float], + speed: float, + acceleration: float, + ) -> None: + raise NotImplementedError( + "FairinoBackend.set_joint_positions is Milestone 2 — read-only " + "state streaming only at Milestone 1." + ) + + def prepare_sync_write_commands( + self, + positions_rad: list[float], + speed: int = 4095, + accel: int = 0, + ) -> list[tuple]: + raise NotImplementedError( + "FairinoBackend has no SYNC_WRITE concept — the FR10 owns its " + "own control loop. Use set_joint_positions (Milestone 2)." + ) + + def sync_write(self, commands: list[tuple]) -> None: + raise NotImplementedError( + "FairinoBackend has no SYNC_WRITE concept — the FR10 owns its " + "own control loop. Use set_joint_positions (Milestone 2)." + ) + + def sync_read_positions( + self, + timeout_s: Optional[float] = None, + ) -> dict[int, int]: + # Return the "raw" positions as round-tripped joint-id -> rad*1000 + # int. We have no raw encoder counts to surface; returning {} makes + # any legacy consumer no-op until they migrate to get_joint_positions. + return {} + + def raw_to_joint_positions(self, raw_positions: dict[int, int]) -> list[float]: + # Same rationale as sync_read_positions — there is no raw layer. + # Return the latest cached joint vector so callers that combine + # sync_read + raw_to_joint still get a reading. + return self.get_joint_positions() + + def set_single_actuator_position( + self, + actuator_id: int, + position_rad: float, + speed: int, + accel: int, + ) -> None: + raise NotImplementedError( + "FairinoBackend.set_single_actuator_position is Milestone 2." + ) + + def read_single_actuator_position(self, actuator_id: int) -> Optional[int]: + # No raw encoder values — return None to signal "not supported". + return None + + # ========================================================================= + # Calibration / configuration — owned by the FR10 controller, not us + # ========================================================================= + + def set_current_position_as_zero(self, actuator_id: int) -> bool: + # Joint zero is set on the FR10 controller via its WebApp / pendant + # calibration UI. Not exposed via the network SDK on purpose. + return False + + def set_pid_gains(self, actuator_id: int, kp: int, ki: int, kd: int) -> bool: + # PID lives inside the FR10 servo drivers. Not user-tunable from here. + return False + + def apply_joint_limits(self) -> bool: + # Soft limits are configured on the FR10 (WebApp → Settings). + # GradientOS clamps planner output against `logical_joint_limits_rad` + # in robot config; the FR10 enforces hardware limits in firmware. + return False diff --git a/src/gradient_os/arm_controller/backends/fairino/config.py b/src/gradient_os/arm_controller/backends/fairino/config.py new file mode 100644 index 00000000..2d3c90b6 --- /dev/null +++ b/src/gradient_os/arm_controller/backends/fairino/config.py @@ -0,0 +1,158 @@ +# backends/fairino/config.py +# +# Configuration constants and environment-variable overrides for the Fairino +# FR-series ActuatorBackend. The Fairino backend does not own servo register +# layouts the way feetech/config.py does — the FR10 controller owns its own +# servos. What we configure here is the *network client* targeting the FR10: +# IP address, ports, poll rates, lifecycle defaults. +# +# MILESTONE 3 PROMOTION: when END_EFFECTOR_OFFSET is moved off the global in +# `gradient_os.ik_solver` and onto RobotConfig, the active tool/wobj indices +# we read from the FR10 should be plumbed through this config too — they are +# session-state, not constants. + +from __future__ import annotations + +import os +from dataclasses import dataclass + + +# Default Fairino controller endpoint. The FR10 ships at this IP and these +# ports out of the box; both are documented in FR10_BRASS_TACKS_TOUR.md. +DEFAULT_CONTROLLER_IP = "192.168.58.2" +DEFAULT_RPC_PORT = 20003 # commands (XML-RPC over TCP) +DEFAULT_STATE_PORT = 20004 # state stream + +# State-stream poll rate. The FR10 publishes at 1–125 Hz; 50 Hz is plenty for +# UI display and well within the controller's capability. Lower if CPU on the +# control PC is constrained; higher only if you need it (Milestone 4+). +DEFAULT_STATE_POLL_HZ = 50.0 + +# Connect timeout for the initial RPC handshake. Beyond this, initialize() +# returns False and the controller stays up in degraded mode (matches the +# ethercat_rtcore pattern). +DEFAULT_CONNECT_TIMEOUT_S = 3.0 + +# Maximum age of a state snapshot we'll trust when callers ask for joint +# positions. If the stream stalls for longer than this, get_joint_positions() +# logs a warning and returns the last known value (rather than blocking or +# raising — the UI must keep rendering). +DEFAULT_STALE_SNAPSHOT_S = 1.0 + +# FR10 has 6 joints. This is hardcoded because every FR-series arm shares it, +# and the value is also enforced via robots/fr10/config.py::num_logical_joints. +FR10_NUM_JOINTS = 6 + + +# ============================================================================= +# Legacy registry shim constants +# ============================================================================= +# The registry.get_*() helpers (encoder_resolution, default_pid_gains, +# default_baud_rate, telemetry blocks, …) expect every backend's config +# module to expose a fixed set of module-level names. They were designed for +# serial-servo protocols (Feetech). The Fairino backend has none of that — +# the FR10 controller owns its own servos, encoders, PID, telemetry. We +# expose zero placeholders so the helpers don't AttributeError, mirroring +# ethercat_rtcore/config.py. None of these values are meaningful; do not +# use them. RobotConfig.default_pid_gains and friends should be overridden +# in robots/fr10/config.py to avoid hitting this path at all. + +SERVO_PROTOCOL_SUPPORTED = False # tell legacy utils to skip serial setup + +DEFAULT_BAUD_RATE = 0 +SERIAL_READ_TIMEOUT = 0.0 + +SERVO_VALUE_MIN = 0 +SERVO_VALUE_MAX = 0 +SERVO_VALUE_CENTER = 0 + +DEFAULT_KP = 0 +DEFAULT_KI = 0 +DEFAULT_KD = 0 + +# Telemetry blocks: not applicable. State arrives via FairinoStateStream, not +# via packed register reads. +TELEMETRY_BLOCK1_ADDRESS = 0 +TELEMETRY_BLOCK1_LENGTH = 0 +TELEMETRY_BLOCK2_ADDRESS = 0 +TELEMETRY_BLOCK2_LENGTH = 0 +TELEMETRY_BLOCK3_ADDRESS = 0 +TELEMETRY_BLOCK3_LENGTH = 0 + + +def parse_telemetry_block1(_data: bytes) -> dict: + return {} + + +def parse_telemetry_block2(_data: bytes) -> dict: + return {} + + +def parse_telemetry_block3(_data: bytes) -> dict: + return {} + + +@dataclass(frozen=True) +class FairinoBackendConfig: + """Resolved network/runtime config for one FairinoBackend instance. + + Built once at backend construction. All fields are overridable via + environment variables so a developer can point at a staging FR10 or a + SDK-mock test rig without editing code. + """ + + controller_ip: str = DEFAULT_CONTROLLER_IP + rpc_port: int = DEFAULT_RPC_PORT + state_port: int = DEFAULT_STATE_PORT + state_poll_hz: float = DEFAULT_STATE_POLL_HZ + connect_timeout_s: float = DEFAULT_CONNECT_TIMEOUT_S + stale_snapshot_s: float = DEFAULT_STALE_SNAPSHOT_S + + @classmethod + def from_env(cls) -> "FairinoBackendConfig": + """Build a config, letting env vars override the defaults. + + Env vars (all optional): + - GRADIENT_FAIRINO_IP + - GRADIENT_FAIRINO_RPC_PORT + - GRADIENT_FAIRINO_STATE_PORT + - GRADIENT_FAIRINO_POLL_HZ + - GRADIENT_FAIRINO_CONNECT_TIMEOUT_S + - GRADIENT_FAIRINO_STALE_SNAPSHOT_S + """ + + def _str(name: str, default: str) -> str: + return os.environ.get(name, default) + + def _int(name: str, default: int) -> int: + raw = os.environ.get(name) + if raw is None or raw == "": + return default + try: + return int(raw) + except ValueError: + print(f"[Fairino config] WARNING: {name}={raw!r} is not an int; using {default}") + return default + + def _float(name: str, default: float) -> float: + raw = os.environ.get(name) + if raw is None or raw == "": + return default + try: + return float(raw) + except ValueError: + print(f"[Fairino config] WARNING: {name}={raw!r} is not a float; using {default}") + return default + + return cls( + controller_ip=_str("GRADIENT_FAIRINO_IP", DEFAULT_CONTROLLER_IP), + rpc_port=_int("GRADIENT_FAIRINO_RPC_PORT", DEFAULT_RPC_PORT), + state_port=_int("GRADIENT_FAIRINO_STATE_PORT", DEFAULT_STATE_PORT), + state_poll_hz=_float("GRADIENT_FAIRINO_POLL_HZ", DEFAULT_STATE_POLL_HZ), + connect_timeout_s=_float( + "GRADIENT_FAIRINO_CONNECT_TIMEOUT_S", DEFAULT_CONNECT_TIMEOUT_S + ), + stale_snapshot_s=_float( + "GRADIENT_FAIRINO_STALE_SNAPSHOT_S", DEFAULT_STALE_SNAPSHOT_S + ), + ) diff --git a/src/gradient_os/arm_controller/backends/fairino/frame_guard.py b/src/gradient_os/arm_controller/backends/fairino/frame_guard.py new file mode 100644 index 00000000..66d31c3a --- /dev/null +++ b/src/gradient_os/arm_controller/backends/fairino/frame_guard.py @@ -0,0 +1,79 @@ +# backends/fairino/frame_guard.py +# +# Enforces the Option B coordinate-frame strategy chosen for FR10 integration: +# the FR10 controller owns the calibrated tool/workpiece frames; GradientOS +# does NOT apply its own END_EFFECTOR_OFFSET on top. Mixing the two is the +# "offset-squared" bug — robot ends up double-offset, crashes the fixture. +# See Fairino-FR10/Docs/FR10_GRADIENTOS_COMPARISON.md for the full rationale. +# +# This module is called from FairinoBackend.initialize() *before* any network +# I/O. If the guard trips, we fail loud with FrameStrategyViolation rather +# than connecting and silently letting the user command motion. +# +# MILESTONE 3 PROMOTION: this guard goes away when END_EFFECTOR_OFFSET is +# moved off `gradient_os.ik_solver` (a global) and onto RobotConfig (a +# per-robot field). At that point the FR10 RobotConfig declares +# `end_effector_offset = np.zeros(3)` and ik_solver reads it from the active +# robot — there is nothing left to "double up" because the value is owned by +# exactly one place. Until that refactor lands, this guard is the one thing +# standing between us and a 380 mm crash. + +from __future__ import annotations + +import numpy as np + + +# Tolerance for "is this offset effectively zero?". 1 micron is well below +# any meaningful mechanical resolution and well above floating-point noise. +_IDENTITY_TOLERANCE_M = 1e-6 + + +class FrameStrategyViolation(RuntimeError): + """Raised when GradientOS's frame state conflicts with FR10's. + + Carries instructions for the operator in str(exc) — the message is the + primary fix-it artifact, so keep it specific. + """ + + +def assert_identity_end_effector_offset() -> None: + """Refuse to run Fairino if GradientOS holds a non-zero tool offset. + + Reads the current value of `gradient_os.ik_solver.END_EFFECTOR_OFFSET` + fresh on every call — no caching — so a developer who patches the value + at runtime gets the new behavior on the next initialize(). + + Raises: + FrameStrategyViolation: if the offset is non-identity. The message + includes the actual offset value and the exact one-line edit. + """ + # Imported inline to avoid pulling ik_solver (and numpy) into module-load + # cost for environments that don't actually use the Fairino backend. + from gradient_os.ik_solver import END_EFFECTOR_OFFSET + + offset = np.asarray(END_EFFECTOR_OFFSET, dtype=float).reshape(-1) + if offset.size != 3: + raise FrameStrategyViolation( + f"END_EFFECTOR_OFFSET has unexpected shape {offset.shape}; " + "expected a 3-vector." + ) + + if float(np.linalg.norm(offset)) <= _IDENTITY_TOLERANCE_M: + return # identity — Option B holds, FR10 owns the tool frame. + + raise FrameStrategyViolation( + "Refusing to start the Fairino backend: the FR10 controller already " + "applies its calibrated tool frame (Option B), and " + f"gradient_os.ik_solver.END_EFFECTOR_OFFSET is currently {offset.tolist()} " + "(non-zero). Running with both active produces an offset-squared error " + "and will crash the robot into the fixture.\n" + "\n" + "Fix: edit src/gradient_os/ik_solver.py line ~33 to " + "`END_EFFECTOR_OFFSET = np.array([0.0, 0.0, 0.0], dtype=float)` while " + "the Fairino backend is active. Restore the original value when you " + "switch back to feetech / ethercat_rtcore.\n" + "\n" + "Followup (Milestone 3): END_EFFECTOR_OFFSET will move from this " + "global onto RobotConfig so each robot owns its own value and this " + "guard becomes unnecessary." + ) diff --git a/src/gradient_os/arm_controller/backends/fairino/rpc_client.py b/src/gradient_os/arm_controller/backends/fairino/rpc_client.py new file mode 100644 index 00000000..3c2dfacc --- /dev/null +++ b/src/gradient_os/arm_controller/backends/fairino/rpc_client.py @@ -0,0 +1,220 @@ +# backends/fairino/rpc_client.py +# +# Quarantine for every Fairino Python SDK call. Nothing else in GradientOS +# should `import fairino` — only this file. Two reasons: +# +# 1. The Fairino SDK is a third-party dependency we don't yet have a copy of +# locally to verify against. Method names ("RPC", "GetActualJointPosDegree", +# "Connect", etc.) are taken from FR10_BRASS_TACKS_TOUR.md and Fairino's +# public docs but must be confirmed once the SDK is pip-installed. By +# keeping the SDK contact surface in one place, that confirmation is a +# small diff against TODO markers below — not a hunt across the codebase. +# +# 2. Tests must run without the SDK installed. FairinoRPCClient takes a +# `_sdk_factory` argument (defaults to the real `_RealFairinoSDK`) so a +# fake adapter can be injected. Every method on the adapter is a thin +# pass-through; the unit test verifies our usage, not the SDK. +# +# Joint-angle convention boundary lives here too: the Fairino SDK speaks +# degrees (FR10_INTEGRATION_GUIDE.md), GradientOS's ActuatorBackend speaks +# radians. Conversion happens at this layer so the rest of the backend never +# has to think about it. + +from __future__ import annotations + +import math +import threading +from dataclasses import dataclass, field +from typing import Callable, Optional, Protocol + + +@dataclass +class FairinoStatusFlags: + """Status bits we surface to the rest of GradientOS for UI / alerts. + + All booleans default to False, which is also the "we haven't heard from + the FR10 yet" state — a fresh FairinoRPCClient before connect() reports + everything as not-faulted, and consumers should check `is_connected` + rather than inferring from these flags alone. + """ + + in_collision: bool = False + estopped: bool = False + faulted: bool = False + program_state: str = "" # FR10's high-level mode string (free-form) + last_error_code: int = 0 # 0 == OK by Fairino convention + + +class FairinoSDKAdapter(Protocol): + """The narrow contract this backend needs from the Fairino SDK. + + Built as a Protocol so we can satisfy it with either the real SDK + wrapper (`_RealFairinoSDK`) or an in-test fake. Keep this surface tiny — + every method that lands here is one more SDK assumption to verify. + """ + + def connect(self, ip: str, timeout_s: float) -> None: ... + def disconnect(self) -> None: ... + def is_connected(self) -> bool: ... + def read_joint_positions_deg(self) -> list[float]: ... + def read_status_flags(self) -> FairinoStatusFlags: ... + + +class _RealFairinoSDK: + """Production adapter: delegates to the real `fairino` Python SDK. + + Each method wraps one (or two) actual SDK calls. The exact call names + are TODOs because we don't have the SDK in hand to verify; the FR10 + docs hint at the surface but don't show signatures. When the SDK is + pip-installed, walk through each TODO below and replace the stub with + the real call. **Keep all SDK-specific logic in this class.** + """ + + def __init__(self) -> None: + # Lazy-imported in connect() so that: + # 1) tests don't need the SDK installed; + # 2) module import of this file (e.g. for type hints) doesn't fail + # on a system where fairino isn't available. + self._sdk = None # type: ignore[assignment] + self._connected = False + + def connect(self, ip: str, timeout_s: float) -> None: + # TODO(SDK): verify the import path. The Fairino docs reference both + # `from fairino import Robot` and `Robot.RPC(ip)` and `from fairino + # import RPC`. Pick whichever the installed wheel actually ships. + try: + from fairino import Robot # type: ignore[import-not-found] + except ImportError as e: + raise RuntimeError( + "Fairino Python SDK not installed. Install with " + "`pip install -e '.[fairino]'` (or fix the dependency once " + "the exact PyPI name is confirmed)." + ) from e + + # TODO(SDK): confirm constructor — does Robot.RPC(ip) auto-connect + # and raise on failure, or return a status code? Some Fairino + # versions return (error_code, handle) tuples. The `timeout_s` + # value may need to be passed via a separate config call. + self._sdk = Robot.RPC(ip) + self._connected = True + + def disconnect(self) -> None: + if self._sdk is None: + return + # TODO(SDK): confirm disconnect call. May be `CloseRPC()`, + # `LogOut()`, `Disconnect()`, or simply releasing the object. + try: + close = getattr(self._sdk, "CloseRPC", None) + if callable(close): + close() + finally: + self._sdk = None + self._connected = False + + def is_connected(self) -> bool: + return self._connected and self._sdk is not None + + def read_joint_positions_deg(self) -> list[float]: + if self._sdk is None: + raise RuntimeError("Fairino SDK not connected") + # TODO(SDK): confirm this returns (error_code, [j1..j6]) tuple as + # degrees, with error_code==0 on success. The `0` arg (block flag + # vs. cached) varies by SDK version — verify against the wheel. + error_code, joints = self._sdk.GetActualJointPosDegree(0) + if error_code != 0: + raise RuntimeError( + f"FR10 GetActualJointPosDegree failed: code={error_code}" + ) + if len(joints) < 6: + raise RuntimeError( + f"FR10 returned {len(joints)} joints, expected ≥6" + ) + return [float(j) for j in joints[:6]] + + def read_status_flags(self) -> FairinoStatusFlags: + if self._sdk is None: + raise RuntimeError("Fairino SDK not connected") + # TODO(SDK): the FR10 surfaces robot state via several methods. Most + # likely candidates: GetRobotErrcode(), GetRobotState(), + # GetRobotMotionDone(). The exact set of fields depends on the + # SDK version. For Milestone 1 we only need fault/estop/collision + # bits for the UI; everything else is decoration. + flags = FairinoStatusFlags() + try: + err_code = self._sdk.GetRobotErrcode() + if isinstance(err_code, tuple): + # Some SDK methods return (return_code, value). + err_code = err_code[-1] + flags.last_error_code = int(err_code) + flags.faulted = flags.last_error_code != 0 + except AttributeError: + pass # Method name unverified — leave defaults. + return flags + + +class FairinoRPCClient: + """Thread-safe lifecycle wrapper around a single Fairino SDK adapter. + + Owns the connect/disconnect transitions and serializes adapter calls + behind a lock. Multiple state-stream pollers and the backend's own + on-demand reads can share one instance. + + Inject `_sdk_factory=lambda: FakeAdapter()` in tests to bypass the real + SDK entirely. + """ + + def __init__( + self, + ip: str, + connect_timeout_s: float, + _sdk_factory: Optional[Callable[[], FairinoSDKAdapter]] = None, + ) -> None: + self._ip = ip + self._connect_timeout_s = connect_timeout_s + self._sdk_factory = _sdk_factory or _RealFairinoSDK + self._sdk: Optional[FairinoSDKAdapter] = None + self._lock = threading.Lock() + + def connect(self) -> None: + """Connect to the FR10 controller. Idempotent.""" + with self._lock: + if self._sdk is not None and self._sdk.is_connected(): + return + sdk = self._sdk_factory() + sdk.connect(self._ip, self._connect_timeout_s) + self._sdk = sdk + + def disconnect(self) -> None: + """Disconnect from the FR10 controller. Idempotent and never raises.""" + with self._lock: + if self._sdk is None: + return + try: + self._sdk.disconnect() + except Exception as e: + print(f"[Fairino RPC] disconnect raised: {e}") + finally: + self._sdk = None + + @property + def is_connected(self) -> bool: + with self._lock: + return self._sdk is not None and self._sdk.is_connected() + + def read_joint_positions_rad(self) -> list[float]: + """Read the current 6 joint angles, converted from degrees to radians. + + Conversion is the SDK boundary — every layer above this gets radians, + matching the rest of GradientOS. + """ + with self._lock: + if self._sdk is None: + raise RuntimeError("FairinoRPCClient not connected") + joints_deg = self._sdk.read_joint_positions_deg() + return [math.radians(j) for j in joints_deg] + + def read_status_flags(self) -> FairinoStatusFlags: + with self._lock: + if self._sdk is None: + raise RuntimeError("FairinoRPCClient not connected") + return self._sdk.read_status_flags() diff --git a/src/gradient_os/arm_controller/backends/fairino/state_stream.py b/src/gradient_os/arm_controller/backends/fairino/state_stream.py new file mode 100644 index 00000000..3f14de14 --- /dev/null +++ b/src/gradient_os/arm_controller/backends/fairino/state_stream.py @@ -0,0 +1,151 @@ +# backends/fairino/state_stream.py +# +# Background poller that asks the Fairino RPC client for joint positions and +# status at a fixed rate, caches the latest snapshot in a thread-safe slot, +# and lets callers read it without blocking. +# +# Why polling instead of a true push stream from port 20004: +# - The Fairino SDK we're targeting wraps both ports 20003 (commands) and +# 20004 (state) behind one client object. The natural API is `client. +# GetActualJointPosDegree()` — a polled read. Until we verify whether +# the SDK exposes the raw 20004 stream, polling at a fixed rate is the +# simplest correct approach and is plenty for Milestone 1 (UI display). +# - At 50 Hz the latency is 20 ms — fine for a human watching joint +# readouts in a browser. Milestone 4+ may require lifting this to 125 Hz +# for closed-loop work, at which point we revisit. + +from __future__ import annotations + +import math +import threading +import time +from dataclasses import dataclass, field +from typing import Optional + +from .config import FR10_NUM_JOINTS +from .rpc_client import FairinoRPCClient, FairinoStatusFlags + + +@dataclass +class StateSnapshot: + """One frozen reading of the FR10's reported state. + + `timestamp_monotonic_s == 0.0` means "no reading received yet" — the + snapshot is at its initial sentinel value. Consumers should check + `has_reading` rather than inspecting the timestamp directly. + """ + + joint_positions_rad: list[float] = field( + default_factory=lambda: [0.0] * FR10_NUM_JOINTS + ) + status: FairinoStatusFlags = field(default_factory=FairinoStatusFlags) + timestamp_monotonic_s: float = 0.0 + sequence: int = 0 + + @property + def has_reading(self) -> bool: + return self.sequence > 0 + + def age_s(self, now_s: Optional[float] = None) -> float: + if not self.has_reading: + return math.inf + return (now_s if now_s is not None else time.monotonic()) - self.timestamp_monotonic_s + + +class FairinoStateStream: + """Daemon-thread state poller backed by a `FairinoRPCClient`. + + Lifecycle: + `start()` — spawn the poller thread (idempotent). + `stop()` — signal the poller to exit and join (idempotent, never + raises). Always called from `FairinoBackend.shutdown()`. + `latest()` — return a copy of the most recent snapshot, never blocks. + + The poller swallows transient read errors and just stops updating — + callers detect staleness via `latest().age_s()`. Bubbling up exceptions + from the daemon thread would be invisible to the main thread anyway. + """ + + def __init__(self, rpc: FairinoRPCClient, poll_hz: float) -> None: + if poll_hz <= 0.0: + raise ValueError(f"poll_hz must be > 0, got {poll_hz}") + self._rpc = rpc + self._period_s = 1.0 / float(poll_hz) + self._stop_evt = threading.Event() + self._thread: Optional[threading.Thread] = None + self._snapshot_lock = threading.Lock() + self._snapshot = StateSnapshot() + self._consecutive_errors = 0 + self._last_error_log_s = 0.0 + + def start(self) -> None: + if self._thread is not None and self._thread.is_alive(): + return + self._stop_evt.clear() + self._thread = threading.Thread( + target=self._run, + name="fairino-state-stream", + daemon=True, + ) + self._thread.start() + + def stop(self) -> None: + self._stop_evt.set() + thread = self._thread + if thread is not None and thread.is_alive(): + thread.join(timeout=2.0) + self._thread = None + + def latest(self) -> StateSnapshot: + """Return a snapshot copy. Never blocks on the poller.""" + with self._snapshot_lock: + snap = self._snapshot + return StateSnapshot( + joint_positions_rad=list(snap.joint_positions_rad), + status=FairinoStatusFlags( + in_collision=snap.status.in_collision, + estopped=snap.status.estopped, + faulted=snap.status.faulted, + program_state=snap.status.program_state, + last_error_code=snap.status.last_error_code, + ), + timestamp_monotonic_s=snap.timestamp_monotonic_s, + sequence=snap.sequence, + ) + + def _run(self) -> None: + next_tick = time.monotonic() + while not self._stop_evt.is_set(): + try: + joints = self._rpc.read_joint_positions_rad() + status = self._rpc.read_status_flags() + self._publish(joints, status) + self._consecutive_errors = 0 + except Exception as e: + self._consecutive_errors += 1 + # Log first error and every ~5s thereafter, not every cycle. + now = time.monotonic() + if self._consecutive_errors == 1 or (now - self._last_error_log_s) > 5.0: + print( + f"[Fairino state] poller error " + f"(consec={self._consecutive_errors}): {e}" + ) + self._last_error_log_s = now + + next_tick += self._period_s + sleep_s = next_tick - time.monotonic() + if sleep_s > 0: + # Use the stop event for sleep so stop() returns promptly. + self._stop_evt.wait(timeout=sleep_s) + else: + # We fell behind — reset the cadence rather than burst-catching. + next_tick = time.monotonic() + + def _publish(self, joints_rad: list[float], status: FairinoStatusFlags) -> None: + with self._snapshot_lock: + self._snapshot = StateSnapshot( + joint_positions_rad=list(joints_rad), + status=status, + timestamp_monotonic_s=time.monotonic(), + sequence=self._snapshot.sequence + 1, + ) diff --git a/src/gradient_os/arm_controller/robots/__init__.py b/src/gradient_os/arm_controller/robots/__init__.py index 90255925..0aa1296b 100644 --- a/src/gradient_os/arm_controller/robots/__init__.py +++ b/src/gradient_os/arm_controller/robots/__init__.py @@ -24,11 +24,13 @@ # ``` from .base import RobotConfig +from .fr10 import FR10Config from .gradient0 import Gradient0Config # Registry of available robot configurations _ROBOT_REGISTRY: dict[str, type[RobotConfig]] = { 'gradient0': Gradient0Config, + 'fr10': FR10Config, } @@ -78,6 +80,7 @@ def register_robot(name: str, config_class: type[RobotConfig]) -> None: __all__ = [ 'RobotConfig', 'Gradient0Config', + 'FR10Config', 'get_robot_config', 'list_available_robots', 'register_robot', diff --git a/src/gradient_os/arm_controller/robots/fr10/__init__.py b/src/gradient_os/arm_controller/robots/fr10/__init__.py new file mode 100644 index 00000000..a4e03c1b --- /dev/null +++ b/src/gradient_os/arm_controller/robots/fr10/__init__.py @@ -0,0 +1,9 @@ +# robots/fr10/__init__.py +# +# RobotConfig for the Fairino FR10 6-DOF industrial arm. Used together with +# the Fairino ActuatorBackend (backends/fairino/) which talks to the FR10 +# controller over its Python SDK. + +from .config import FR10Config + +__all__ = ["FR10Config"] diff --git a/src/gradient_os/arm_controller/robots/fr10/config.py b/src/gradient_os/arm_controller/robots/fr10/config.py new file mode 100644 index 00000000..99b1851f --- /dev/null +++ b/src/gradient_os/arm_controller/robots/fr10/config.py @@ -0,0 +1,179 @@ +# robots/fr10/config.py +# +# Configuration for the Fairino FR10 6-DOF industrial arm. +# +# The FR10 differs from the Gradient0 in important ways that shape this +# config: +# - It is a sealed industrial appliance: the controller owns calibration, +# PID, encoder mapping, joint limits (in firmware), and the real-time +# control loop. GradientOS is a network client, not a driver. +# - There are no Feetech-style "actuator IDs" exposed to us. We invent +# synthetic IDs 1..6 so the rest of GradientOS (telemetry, UI tables) +# has stable handles, but they don't correspond to any hardware register. +# - PID gains, encoder mapping, and master offsets are owned by the FR10 +# controller — we override the relevant RobotConfig hooks to return safe +# no-op values rather than letting the base class call into the +# fairino backend's (zero) registry shims. +# +# MILESTONE 3 PROMOTION: when END_EFFECTOR_OFFSET moves off the global in +# `gradient_os.ik_solver` and onto RobotConfig, this class will declare +# `end_effector_offset = np.zeros(3)` (the FR10 owns the tool transform via +# `toolcoord1`). Until then, the frame_guard module enforces the Option B +# rule by reading the global and refusing to start if it's non-zero. + +import math +from typing import Optional + +from ..base import RobotConfig + + +# Joint limits in radians. +# TODO: confirm against FR10 datasheet / WebApp soft-limit settings before +# Milestone 2 (motion). Values below are conservative placeholders chosen +# to be smaller than typical FR10 hardware envelopes so a planner that +# clamps against these will never demand motion the FR10 will reject. +# For Milestone 1 (read-only) these are display-only — no clamping happens. +_FR10_JOINT_LIMITS_RAD: list[tuple[float, float]] = [ + (-math.radians(170.0), math.radians(170.0)), # J1 base — verify + (-math.radians( 90.0), math.radians( 90.0)), # J2 shoulder — verify + (-math.radians(150.0), math.radians(150.0)), # J3 elbow — verify + (-math.radians(170.0), math.radians(170.0)), # J4 wrist 1 — verify + (-math.radians(120.0), math.radians(120.0)), # J5 wrist 2 — verify + (-math.radians(180.0), math.radians(180.0)), # J6 wrist 3 — verify +] + + +class FR10Config(RobotConfig): + """RobotConfig for the Fairino FR10 6-DOF industrial arm. + + Pairs with backends/fairino/. The default_servo_backend is "fairino" so + `gradient-controller --robot fr10` selects the right backend without an + explicit --backend flag. + """ + + # ========================================================================= + # Identity + # ========================================================================= + + @property + def name(self) -> str: + return "FR10" + + @property + def version(self) -> str: + return "1.0.0" + + @property + def default_servo_backend(self) -> str: + return "fairino" + + # ========================================================================= + # Kinematic structure + # ========================================================================= + + @property + def num_logical_joints(self) -> int: + return 6 + + @property + def num_physical_actuators(self) -> int: + # No twin motors; one logical joint per controlled axis. + return 6 + + @property + def actuator_ids(self) -> list[int]: + # Synthetic IDs 1..6 — the FR10 doesn't expose hardware servo IDs + # to network clients. These exist only to give telemetry and UI + # tables stable keys. + return [1, 2, 3, 4, 5, 6] + + @property + def logical_to_physical_map(self) -> dict[int, list[int]]: + return {i: [i] for i in range(6)} + + # ========================================================================= + # Joint limits (display only at Milestone 1) + # ========================================================================= + + @property + def logical_joint_limits_rad(self) -> list[tuple[float, float]]: + return list(_FR10_JOINT_LIMITS_RAD) + + @property + def actuator_limits_rad(self) -> list[tuple[float, float]]: + # 1:1 with logical joints — there is no twin-motor or gear-ratio + # remapping at this layer; the FR10 controller owns that. + return list(_FR10_JOINT_LIMITS_RAD) + + # ========================================================================= + # Encoder mapping — not applicable + # ========================================================================= + + @property + def actuator_mapping_ranges_rad(self) -> list[tuple[float, float]]: + # Required by the base class but unused — the Fairino backend works + # in radians end-to-end and never converts to raw encoder counts. + # Return ±π so any code that does try to use it gets a benign value. + return [(-math.pi, math.pi)] * 6 + + @property + def inverted_actuator_ids(self) -> set[int]: + # The FR10 reports joint angles in its own canonical convention via + # the SDK. Sign handling is the controller's problem, not ours. + return set() + + # ========================================================================= + # Calibration — owned by the FR10 controller + # ========================================================================= + + @property + def logical_joint_master_offsets_rad(self) -> list[float]: + # Calibration lives on the FR10 (its zero-position teach is an + # operator-side workflow on the WebApp / pendant). GradientOS adds + # nothing on top. + return [0.0] * 6 + + # ========================================================================= + # Gripper — TBD + # ========================================================================= + + @property + def gripper_actuator_id(self) -> Optional[int]: + # The FR10's gripper / welder I/O surface is Milestone 7. Set to + # None for now so `has_gripper` reports False and the gripper UI + # stays hidden. + return None + + # ========================================================================= + # Motion parameters + # ========================================================================= + + @property + def default_speed(self) -> int: + # Fairino's MoveJ/MoveL accept a speed percentage (0–100). + # 5 (5%) is the bring-up speed recommended in + # FR10_GRADIENTOS_INTEGRATION_PLAN.md for Milestone 2. + return 5 + + @property + def default_acceleration_deg_s2(self) -> float: + # Acceleration is owned by the FR10 controller; this is a no-op. + # Return a non-zero value so any consumer that divides by it + # doesn't blow up. + return 100.0 + + # ========================================================================= + # PID — owned by the FR10 servo drivers, not user-tunable from here + # ========================================================================= + + @property + def default_pid_gains(self) -> tuple[int, int, int]: + # Override the base class default (which calls into the active + # backend's config). Fairino has no PID concept at the network + # client layer — return zeros so anyone who reads this is forced to + # think about whether they actually need it for an FR10. + return (0, 0, 0) + + @property + def actuator_pid_gains(self) -> dict[int, tuple[int, int, int]]: + return {aid: (0, 0, 0) for aid in self.actuator_ids} diff --git a/tests/test_fairino_backend.py b/tests/test_fairino_backend.py new file mode 100644 index 00000000..fff185a3 --- /dev/null +++ b/tests/test_fairino_backend.py @@ -0,0 +1,634 @@ +"""Unit tests for the Fairino FR10 ActuatorBackend (Milestone 1, read-only). + +These tests do not touch the real Fairino SDK or any network; the SDK is +mocked through `FairinoSDKAdapter`. The tests cover: + +- frame_guard: identity vs. non-identity END_EFFECTOR_OFFSET behavior. +- rpc_client: lifecycle, deg→rad conversion, error paths. +- state_stream: start/stop, snapshot freshness, error resilience. +- backend: initialize/shutdown, motion-stub raises, joint-count validation. +""" + +import os + +# Avoid importing the ikfast pybind .so during test collection — the .so is +# only built in dev/prod environments. The numeric backend prints a warning +# but allows ik_solver to import, which is all the frame_guard tests need. +os.environ.setdefault("MINI_ARM_SOLVER", "numeric") + +import math +import threading +import time +import unittest +from typing import Optional +from unittest import mock + +import numpy as np + +# Force ik_solver into sys.modules so mock.patch can walk the dotted path. +import gradient_os.ik_solver # noqa: F401 + +from gradient_os.arm_controller.backends.fairino import ( + FairinoBackend, + FairinoBackendConfig, + FairinoRPCClient, + FairinoStateStream, + FairinoStatusFlags, + FrameStrategyViolation, +) +from gradient_os.arm_controller.backends.fairino import frame_guard +from gradient_os.arm_controller.robots import FR10Config + + +# ============================================================================= +# Test fakes +# ============================================================================= + + +class FakeFairinoSDK: + """In-memory stand-in for the real Fairino SDK. + + Mirrors the FairinoSDKAdapter Protocol. Tests can mutate + `joints_deg` / `status` to drive the backend's perception, and inspect + `connect_calls` / `disconnect_calls` to verify lifecycle. + """ + + def __init__(self) -> None: + self.joints_deg: list[float] = [0.0] * 6 + self.status: FairinoStatusFlags = FairinoStatusFlags() + self.connected: bool = False + self.connect_calls: list[tuple[str, float]] = [] + self.disconnect_calls: int = 0 + self.read_calls: int = 0 + self.fail_next_read_with: Optional[Exception] = None + + # FairinoSDKAdapter protocol -------------------------------------------- + + def connect(self, ip: str, timeout_s: float) -> None: + self.connect_calls.append((ip, timeout_s)) + self.connected = True + + def disconnect(self) -> None: + self.disconnect_calls += 1 + self.connected = False + + def is_connected(self) -> bool: + return self.connected + + def read_joint_positions_deg(self) -> list[float]: + self.read_calls += 1 + if self.fail_next_read_with is not None: + err = self.fail_next_read_with + self.fail_next_read_with = None + raise err + return list(self.joints_deg) + + def read_status_flags(self) -> FairinoStatusFlags: + return FairinoStatusFlags( + in_collision=self.status.in_collision, + estopped=self.status.estopped, + faulted=self.status.faulted, + program_state=self.status.program_state, + last_error_code=self.status.last_error_code, + ) + + +def _identity_offset_patch() -> mock.patch: + """Patch END_EFFECTOR_OFFSET to identity for the duration of a test.""" + return mock.patch( + "gradient_os.ik_solver.END_EFFECTOR_OFFSET", + np.zeros(3, dtype=float), + ) + + +def _make_rpc_with_fake( + fake: Optional[FakeFairinoSDK] = None, + ip: str = "10.0.0.1", +) -> tuple[FairinoRPCClient, FakeFairinoSDK]: + fake = fake or FakeFairinoSDK() + rpc = FairinoRPCClient( + ip=ip, + connect_timeout_s=0.5, + _sdk_factory=lambda: fake, + ) + return rpc, fake + + +# ============================================================================= +# Frame guard +# ============================================================================= + + +class TestFrameGuard(unittest.TestCase): + + def test_identity_offset_passes(self) -> None: + with _identity_offset_patch(): + frame_guard.assert_identity_end_effector_offset() # no raise + + def test_zero_within_tolerance_passes(self) -> None: + # 1e-9 m is well below the 1e-6 tolerance. + with mock.patch( + "gradient_os.ik_solver.END_EFFECTOR_OFFSET", + np.array([1e-9, 0.0, -1e-9]), + ): + frame_guard.assert_identity_end_effector_offset() + + def test_non_identity_offset_raises(self) -> None: + with mock.patch( + "gradient_os.ik_solver.END_EFFECTOR_OFFSET", + np.array([0.180, 0.0, 0.0]), + ): + with self.assertRaises(FrameStrategyViolation) as ctx: + frame_guard.assert_identity_end_effector_offset() + msg = str(ctx.exception) + # Message must surface the offending value AND the fix path. + self.assertIn("0.18", msg) + self.assertIn("ik_solver.py", msg) + self.assertIn("Milestone 3", msg) + + def test_wrong_shape_raises(self) -> None: + with mock.patch( + "gradient_os.ik_solver.END_EFFECTOR_OFFSET", + np.array([0.0, 0.0]), + ): + with self.assertRaises(FrameStrategyViolation): + frame_guard.assert_identity_end_effector_offset() + + +# ============================================================================= +# RPC client +# ============================================================================= + + +class TestFairinoRPCClient(unittest.TestCase): + + def test_connect_passes_ip_and_timeout(self) -> None: + rpc, fake = _make_rpc_with_fake(ip="192.168.58.2") + rpc.connect() + self.assertEqual(fake.connect_calls, [("192.168.58.2", 0.5)]) + self.assertTrue(rpc.is_connected) + + def test_connect_is_idempotent(self) -> None: + rpc, fake = _make_rpc_with_fake() + rpc.connect() + rpc.connect() + # Second call must not re-invoke the SDK. + self.assertEqual(len(fake.connect_calls), 1) + + def test_disconnect_is_idempotent_and_safe(self) -> None: + rpc, fake = _make_rpc_with_fake() + rpc.disconnect() # before connect + rpc.connect() + rpc.disconnect() + rpc.disconnect() # second call + self.assertFalse(rpc.is_connected) + self.assertEqual(fake.disconnect_calls, 1) # only the post-connect one + + def test_read_joints_converts_deg_to_rad(self) -> None: + rpc, fake = _make_rpc_with_fake() + fake.joints_deg = [0.0, 90.0, -45.0, 180.0, -180.0, 0.0] + rpc.connect() + joints_rad = rpc.read_joint_positions_rad() + expected = [ + 0.0, math.pi / 2, -math.pi / 4, math.pi, -math.pi, 0.0, + ] + for got, want in zip(joints_rad, expected): + self.assertAlmostEqual(got, want, places=9) + + def test_read_before_connect_raises(self) -> None: + rpc, _ = _make_rpc_with_fake() + with self.assertRaises(RuntimeError): + rpc.read_joint_positions_rad() + + +# ============================================================================= +# State stream +# ============================================================================= + + +class TestFairinoStateStream(unittest.TestCase): + + def test_latest_before_start_is_sentinel(self) -> None: + rpc, _ = _make_rpc_with_fake() + stream = FairinoStateStream(rpc=rpc, poll_hz=10.0) + snap = stream.latest() + self.assertFalse(snap.has_reading) + self.assertEqual(snap.sequence, 0) + + def test_poller_publishes_readings(self) -> None: + rpc, fake = _make_rpc_with_fake() + rpc.connect() + fake.joints_deg = [10.0, 20.0, 30.0, 40.0, 50.0, 60.0] + stream = FairinoStateStream(rpc=rpc, poll_hz=200.0) + stream.start() + try: + self._wait_for(lambda: stream.latest().sequence > 0, timeout_s=1.0) + snap = stream.latest() + self.assertTrue(snap.has_reading) + for got, want in zip( + snap.joint_positions_rad, + [math.radians(d) for d in fake.joints_deg], + ): + self.assertAlmostEqual(got, want, places=6) + finally: + stream.stop() + + def test_poller_survives_transient_errors(self) -> None: + rpc, fake = _make_rpc_with_fake() + rpc.connect() + fake.joints_deg = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] + stream = FairinoStateStream(rpc=rpc, poll_hz=200.0) + stream.start() + try: + # Get one good reading first. + self._wait_for(lambda: stream.latest().sequence > 0, timeout_s=1.0) + seq_before = stream.latest().sequence + + # Inject one failure, then verify subsequent reads still happen. + fake.fail_next_read_with = RuntimeError("transient") + self._wait_for( + lambda: stream.latest().sequence > seq_before, timeout_s=1.0 + ) + finally: + stream.stop() + + def test_stop_returns_promptly(self) -> None: + rpc, _ = _make_rpc_with_fake() + rpc.connect() + stream = FairinoStateStream(rpc=rpc, poll_hz=2.0) # 500ms period + stream.start() + t0 = time.monotonic() + stream.stop() + elapsed = time.monotonic() - t0 + # Stop event short-circuits the long sleep — should join in << 500ms. + self.assertLess(elapsed, 0.3) + + def test_zero_poll_hz_rejected(self) -> None: + rpc, _ = _make_rpc_with_fake() + with self.assertRaises(ValueError): + FairinoStateStream(rpc=rpc, poll_hz=0.0) + + @staticmethod + def _wait_for(predicate, timeout_s: float) -> None: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + if predicate(): + return + time.sleep(0.01) + raise AssertionError("predicate never became true") + + +# ============================================================================= +# FairinoBackend +# ============================================================================= + + +class TestFairinoBackend(unittest.TestCase): + + def _make_backend( + self, + fake: Optional[FakeFairinoSDK] = None, + poll_hz: float = 200.0, + ) -> tuple[FairinoBackend, FakeFairinoSDK]: + fake = fake or FakeFairinoSDK() + rpc = FairinoRPCClient( + ip="10.0.0.1", + connect_timeout_s=0.1, + _sdk_factory=lambda: fake, + ) + cfg = FairinoBackendConfig( + controller_ip="10.0.0.1", + state_poll_hz=poll_hz, + connect_timeout_s=0.1, + stale_snapshot_s=1.0, + ) + backend = FairinoBackend( + robot_config=FR10Config().get_config_dict(), + backend_config=cfg, + rpc_client=rpc, + state_stream=FairinoStateStream(rpc=rpc, poll_hz=poll_hz), + ) + return backend, fake + + def test_num_joints_is_six(self) -> None: + backend, _ = self._make_backend() + self.assertEqual(backend.num_joints, 6) + + def test_initialize_with_identity_offset_succeeds(self) -> None: + backend, fake = self._make_backend() + with _identity_offset_patch(): + ok = backend.initialize() + try: + self.assertTrue(ok) + self.assertTrue(backend.is_initialized) + self.assertEqual(len(fake.connect_calls), 1) + finally: + backend.shutdown() + self.assertEqual(fake.disconnect_calls, 1) + self.assertFalse(backend.is_initialized) + + def test_initialize_with_non_identity_offset_raises(self) -> None: + backend, fake = self._make_backend() + with mock.patch( + "gradient_os.ik_solver.END_EFFECTOR_OFFSET", + np.array([0.180, 0.0, 0.0]), + ): + with self.assertRaises(FrameStrategyViolation): + backend.initialize() + # Frame guard runs BEFORE network I/O — no connect attempt. + self.assertEqual(len(fake.connect_calls), 0) + + def test_get_joint_positions_returns_latest_snapshot(self) -> None: + backend, fake = self._make_backend() + fake.joints_deg = [0.0, 30.0, 60.0, 90.0, 120.0, 150.0] + with _identity_offset_patch(): + backend.initialize() + try: + self._wait_for( + lambda: backend.get_joint_positions()[1] != 0.0, + timeout_s=1.0, + ) + joints = backend.get_joint_positions() + for got, want in zip(joints, [math.radians(d) for d in fake.joints_deg]): + self.assertAlmostEqual(got, want, places=6) + finally: + backend.shutdown() + + def test_get_joint_positions_returns_zeros_before_first_reading(self) -> None: + # If no poll has completed yet, get_joint_positions returns zeros + # (sentinel) instead of raising — the UI must keep rendering. + backend, _ = self._make_backend(poll_hz=0.5) # 2s period + with _identity_offset_patch(): + backend.initialize() + try: + joints = backend.get_joint_positions() + self.assertEqual(joints, [0.0] * 6) + finally: + backend.shutdown() + + def test_set_joint_positions_raises_not_implemented(self) -> None: + backend, _ = self._make_backend() + with self.assertRaises(NotImplementedError) as ctx: + backend.set_joint_positions([0.0] * 6, speed=1.0, acceleration=1.0) + self.assertIn("Milestone 2", str(ctx.exception)) + + def test_sync_write_raises_not_implemented(self) -> None: + backend, _ = self._make_backend() + with self.assertRaises(NotImplementedError): + backend.sync_write([]) + + def test_set_single_actuator_raises_not_implemented(self) -> None: + backend, _ = self._make_backend() + with self.assertRaises(NotImplementedError): + backend.set_single_actuator_position(1, 0.0, 50, 0) + + def test_calibration_methods_return_false(self) -> None: + backend, _ = self._make_backend() + self.assertFalse(backend.set_current_position_as_zero(1)) + self.assertFalse(backend.set_pid_gains(1, 50, 0, 10)) + self.assertFalse(backend.apply_joint_limits()) + + def test_wrong_num_joints_in_robot_config_raises(self) -> None: + bad_config = FR10Config().get_config_dict() + bad_config["num_logical_joints"] = 7 + with self.assertRaises(ValueError) as ctx: + FairinoBackend(robot_config=bad_config) + self.assertIn("FR10", str(ctx.exception)) + + def test_present_actuator_ids_when_uninitialized(self) -> None: + backend, _ = self._make_backend() + self.assertEqual(backend.get_present_actuator_ids(), set()) + + def test_present_actuator_ids_when_initialized(self) -> None: + backend, _ = self._make_backend() + with _identity_offset_patch(): + backend.initialize() + try: + self.assertEqual(backend.get_present_actuator_ids(), {1, 2, 3, 4, 5, 6}) + finally: + backend.shutdown() + + @staticmethod + def _wait_for(predicate, timeout_s: float) -> None: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + if predicate(): + return + time.sleep(0.01) + raise AssertionError("predicate never became true") + + +# ============================================================================= +# Registry wiring +# ============================================================================= + + +class TestFairinoRegistry(unittest.TestCase): + + def test_fairino_registered_in_backend_classes(self) -> None: + from gradient_os.arm_controller.backends import registry + self.assertIn("fairino", registry.BACKEND_CLASSES) + + def test_fairino_config_module_registered(self) -> None: + from gradient_os.arm_controller.backends import registry + self.assertIn("fairino", registry.BACKEND_CONFIG_MODULES) + + def test_fr10_robot_config_targets_fairino_backend(self) -> None: + cfg = FR10Config() + self.assertEqual(cfg.default_servo_backend, "fairino") + self.assertEqual(cfg.num_logical_joints, 6) + + def test_create_backend_via_registry_factory(self) -> None: + # Exercises the full create_backend path end-to-end (the same path + # run_controller.py uses at startup) so a future regression in the + # factory wiring fails here, not in production. + from gradient_os.arm_controller.backends import registry + cfg = FR10Config().get_config_dict() + backend = registry.create_backend("fairino", cfg) + self.assertIsInstance(backend, FairinoBackend) + self.assertEqual(backend.num_joints, 6) + # Not initialized yet — must not have touched the network. + self.assertFalse(backend.is_initialized) + + +# ============================================================================= +# Status flags + snapshot semantics +# ============================================================================= + + +class TestStatusAndSnapshotSemantics(unittest.TestCase): + + def test_status_flags_propagate_through_stream(self) -> None: + rpc, fake = _make_rpc_with_fake() + rpc.connect() + fake.status = FairinoStatusFlags( + in_collision=False, + estopped=True, + faulted=True, + program_state="emergency_stop", + last_error_code=42, + ) + stream = FairinoStateStream(rpc=rpc, poll_hz=200.0) + stream.start() + try: + _wait_for(lambda: stream.latest().sequence > 0, timeout_s=1.0) + snap = stream.latest() + self.assertTrue(snap.status.faulted) + self.assertTrue(snap.status.estopped) + self.assertEqual(snap.status.last_error_code, 42) + self.assertEqual(snap.status.program_state, "emergency_stop") + finally: + stream.stop() + + def test_snapshot_is_deep_copied(self) -> None: + # Mutating a returned snapshot must not affect the cache (or other + # callers). This is what makes latest() safe to call concurrently + # with the poller. + rpc, fake = _make_rpc_with_fake() + rpc.connect() + fake.joints_deg = [10.0, 20.0, 30.0, 40.0, 50.0, 60.0] + stream = FairinoStateStream(rpc=rpc, poll_hz=200.0) + stream.start() + try: + _wait_for(lambda: stream.latest().sequence > 0, timeout_s=1.0) + snap_a = stream.latest() + snap_a.joint_positions_rad[0] = 999.0 # caller mutation + snap_a.status.faulted = True + snap_b = stream.latest() + self.assertNotEqual(snap_b.joint_positions_rad[0], 999.0) + self.assertFalse(snap_b.status.faulted) + finally: + stream.stop() + + def test_snapshot_age_s_is_finite_after_reading(self) -> None: + rpc, _ = _make_rpc_with_fake() + rpc.connect() + stream = FairinoStateStream(rpc=rpc, poll_hz=200.0) + stream.start() + try: + _wait_for(lambda: stream.latest().sequence > 0, timeout_s=1.0) + age = stream.latest().age_s() + self.assertGreaterEqual(age, 0.0) + self.assertLess(age, 1.0) + finally: + stream.stop() + + def test_snapshot_age_s_is_inf_before_any_reading(self) -> None: + rpc, _ = _make_rpc_with_fake() + stream = FairinoStateStream(rpc=rpc, poll_hz=10.0) + self.assertEqual(stream.latest().age_s(), math.inf) + + +# ============================================================================= +# Config + env overrides +# ============================================================================= + + +class TestFairinoBackendConfig(unittest.TestCase): + + def test_defaults_match_fr10_factory_settings(self) -> None: + cfg = FairinoBackendConfig() + self.assertEqual(cfg.controller_ip, "192.168.58.2") + self.assertEqual(cfg.rpc_port, 20003) + self.assertEqual(cfg.state_port, 20004) + + def test_env_overrides_apply(self) -> None: + with mock.patch.dict( + "os.environ", + { + "GRADIENT_FAIRINO_IP": "10.9.9.9", + "GRADIENT_FAIRINO_POLL_HZ": "75", + "GRADIENT_FAIRINO_CONNECT_TIMEOUT_S": "1.5", + }, + clear=False, + ): + cfg = FairinoBackendConfig.from_env() + self.assertEqual(cfg.controller_ip, "10.9.9.9") + self.assertEqual(cfg.state_poll_hz, 75.0) + self.assertEqual(cfg.connect_timeout_s, 1.5) + + def test_invalid_env_value_falls_back_to_default(self) -> None: + with mock.patch.dict( + "os.environ", + {"GRADIENT_FAIRINO_POLL_HZ": "not-a-number"}, + clear=False, + ): + cfg = FairinoBackendConfig.from_env() + # Falls back to module default rather than crashing startup. + self.assertEqual(cfg.state_poll_hz, 50.0) + + +# ============================================================================= +# Lifecycle +# ============================================================================= + + +class TestFairinoBackendLifecycle(unittest.TestCase): + + def _make_backend(self, fake=None, poll_hz=200.0): + fake = fake or FakeFairinoSDK() + rpc = FairinoRPCClient( + ip="10.0.0.1", + connect_timeout_s=0.1, + _sdk_factory=lambda: fake, + ) + cfg = FairinoBackendConfig( + controller_ip="10.0.0.1", + state_poll_hz=poll_hz, + connect_timeout_s=0.1, + ) + backend = FairinoBackend( + robot_config=FR10Config().get_config_dict(), + backend_config=cfg, + rpc_client=rpc, + state_stream=FairinoStateStream(rpc=rpc, poll_hz=poll_hz), + ) + return backend, fake + + def test_shutdown_is_idempotent(self) -> None: + backend, fake = self._make_backend() + with _identity_offset_patch(): + backend.initialize() + backend.shutdown() + backend.shutdown() # second call must not raise + self.assertEqual(fake.disconnect_calls, 1) + self.assertFalse(backend.is_initialized) + + def test_initialize_returns_false_on_connect_failure(self) -> None: + # If the SDK raises during connect, initialize logs and returns + # False (so run_controller stays alive in degraded mode), rather + # than propagating. + class FlakySDK(FakeFairinoSDK): + def connect(self, ip, timeout_s): # type: ignore[override] + raise ConnectionRefusedError("FR10 unreachable") + + backend, _ = self._make_backend(fake=FlakySDK()) + with _identity_offset_patch(): + ok = backend.initialize() + self.assertFalse(ok) + self.assertFalse(backend.is_initialized) + + def test_get_joint_positions_verbose_does_not_raise(self) -> None: + # Smoke test: verbose logging path must not crash on a stale or + # not-yet-arrived snapshot. The UI passes verbose=True for diagnostics. + backend, _ = self._make_backend(poll_hz=0.5) + with _identity_offset_patch(): + backend.initialize() + try: + joints = backend.get_joint_positions(verbose=True) + self.assertEqual(len(joints), 6) + finally: + backend.shutdown() + + +def _wait_for(predicate, timeout_s: float) -> None: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + if predicate(): + return + time.sleep(0.01) + raise AssertionError("predicate never became true") + + +if __name__ == "__main__": + unittest.main()