Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 36 additions & 0 deletions docs/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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 <name>`. 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
Expand Down
7 changes: 7 additions & 0 deletions pyproject.toml
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down
22 changes: 21 additions & 1 deletion src/gradient_os/arm_controller/backends/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
# Import backend classes
from .feetech import FeetechBackend
from .ethercat_rtcore import EthercatRTCoreBackend
from .fairino import FairinoBackend
from .simulation import SimulationBackend

# =============================================================================
Expand Down Expand Up @@ -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).
Expand Down Expand Up @@ -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']
30 changes: 30 additions & 0 deletions src/gradient_os/arm_controller/backends/fairino/__init__.py
Original file line number Diff line number Diff line change
@@ -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",
]
253 changes: 253 additions & 0 deletions src/gradient_os/arm_controller/backends/fairino/backend.py
Original file line number Diff line number Diff line change
@@ -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
Loading