diff --git a/dimos/control/README.md b/dimos/control/README.md index 58490321fa..755bfbd939 100644 --- a/dimos/control/README.md +++ b/dimos/control/README.md @@ -1,4 +1,4 @@ -# Control Orchestrator +# Control Coordinator Centralized control system for multi-arm robots with per-joint arbitration. @@ -6,7 +6,7 @@ Centralized control system for multi-arm robots with per-joint arbitration. ``` ┌─────────────────────────────────────────────────────────────┐ -│ ControlOrchestrator │ +│ ControlCoordinator │ │ │ │ ┌──────────────────────────────────────────────────────┐ │ │ │ TickLoop (100Hz) │ │ @@ -16,8 +16,8 @@ Centralized control system for multi-arm robots with per-joint arbitration. │ │ │ │ │ │ │ ▼ ▼ ▼ ▼ │ │ ┌─────────┐ ┌───────┐ ┌─────────┐ ┌──────────┐ │ -│ │Hardware │ │ Tasks │ │Priority │ │ Backends │ │ -│ │Interface│ │ │ │ Winners │ │ │ │ +│ │Connected│ │ Tasks │ │Priority │ │ Adapters │ │ +│ │Hardware │ │ │ │ Winners │ │ │ │ │ └─────────┘ └───────┘ └─────────┘ └──────────┘ │ └─────────────────────────────────────────────────────────────┘ ``` @@ -25,13 +25,13 @@ Centralized control system for multi-arm robots with per-joint arbitration. ## Quick Start ```bash -# Terminal 1: Run orchestrator -dimos run orchestrator-mock # Single 7-DOF mock arm -dimos run orchestrator-dual-mock # Dual arms (7+6 DOF) -dimos run orchestrator-piper-xarm # Real hardware +# Terminal 1: Run coordinator +dimos run coordinator-mock # Single 7-DOF mock arm +dimos run coordinator-dual-mock # Dual arms (7+6 DOF) +dimos run coordinator-piper-xarm # Real hardware # Terminal 2: Control via CLI -python -m dimos.manipulation.control.orchestrator_client +python -m dimos.manipulation.control.coordinator_client ``` ## Core Concepts @@ -42,17 +42,17 @@ Single deterministic loop at 100Hz: 2. **Compute** - Each task calculates desired output 3. **Arbitrate** - Per-joint, highest priority wins 4. **Route** - Group commands by hardware -5. **Write** - Send commands to backends +5. **Write** - Send commands to adapters ### Tasks (Controllers) -Tasks are passive controllers called by the orchestrator: +Tasks are passive controllers called by the coordinator: ```python class MyController: def claim(self) -> ResourceClaim: return ResourceClaim(joints={"joint1", "joint2"}, priority=10) - def compute(self, state: OrchestratorState) -> JointCommandOutput: + def compute(self, state: CoordinatorState) -> JointCommandOutput: # Your control law here (PID, impedance, etc.) return JointCommandOutput( joint_names=["joint1", "joint2"], @@ -83,10 +83,11 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: ``` dimos/control/ -├── orchestrator.py # Module + RPC interface +├── coordinator.py # Module + RPC interface ├── tick_loop.py # 100Hz control loop ├── task.py # ControlTask protocol + types -├── hardware_interface.py # Backend wrapper +├── hardware_interface.py # ConnectedHardware wrapper +├── components.py # HardwareComponent config + type aliases ├── blueprints.py # Pre-configured setups └── tasks/ └── trajectory_task.py # Joint trajectory controller @@ -95,13 +96,25 @@ dimos/control/ ## Configuration ```python -from dimos.control import control_orchestrator, HardwareConfig, TaskConfig +from dimos.control import control_coordinator, HardwareComponent, TaskConfig -my_robot = control_orchestrator( +my_robot = control_coordinator( tick_rate=100.0, hardware=[ - HardwareConfig(id="left", type="xarm", dof=7, joint_prefix="left", ip="192.168.1.100"), - HardwareConfig(id="right", type="piper", dof=6, joint_prefix="right", can_port="can0"), + HardwareComponent( + hardware_id="left_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("left_arm", 7), + adapter_type="xarm", + address="192.168.1.100", + ), + HardwareComponent( + hardware_id="right_arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("right_arm", 6), + adapter_type="piper", + address="can0", + ), ], tasks=[ TaskConfig(name="traj_left", type="trajectory", joint_names=[...], priority=10), @@ -181,15 +194,15 @@ class PIDController: ## Joint State Output -The orchestrator publishes one aggregated `JointState` message containing all joints: +The coordinator publishes one aggregated `JointState` message containing all joints: ```python JointState( - name=["left_joint1", ..., "right_joint1", ...], # All joints + name=["left_arm_joint1", ..., "right_arm_joint1", ...], # All joints position=[...], velocity=[...], effort=[...], ) ``` -Subscribe via: `/orchestrator/joint_state` +Subscribe via: `/coordinator/joint_state` diff --git a/dimos/control/__init__.py b/dimos/control/__init__.py index 755cdc6f70..50d0330107 100644 --- a/dimos/control/__init__.py +++ b/dimos/control/__init__.py @@ -18,7 +18,7 @@ per-driver/per-controller loops with a single deterministic tick-based system. Features: -- Single tick loop (read → compute → arbitrate → route → write) +- Single tick loop (read -> compute -> arbitrate -> route -> write) - Per-joint arbitration (highest priority wins) - Mode conflict detection - Partial command support (hold last value) @@ -27,15 +27,15 @@ Example: >>> from dimos.control import ControlCoordinator >>> from dimos.control.tasks import JointTrajectoryTask, JointTrajectoryTaskConfig - >>> from dimos.hardware.manipulators.xarm import XArmBackend + >>> from dimos.hardware.manipulators.xarm import XArmAdapter >>> >>> # Create coordinator >>> coord = ControlCoordinator(tick_rate=100.0) >>> >>> # Add hardware - >>> backend = XArmBackend(ip="192.168.1.185", dof=7) - >>> backend.connect() - >>> coord.add_hardware("left_arm", backend) + >>> adapter = XArmAdapter(ip="192.168.1.185", dof=7) + >>> adapter.connect() + >>> coord.add_hardware("left_arm", adapter) >>> >>> # Add task >>> joints = [f"left_arm_joint{i+1}" for i in range(7)] @@ -63,10 +63,7 @@ TaskConfig, control_coordinator, ) -from dimos.control.hardware_interface import ( - BackendHardwareInterface, - HardwareInterface, -) +from dimos.control.hardware_interface import ConnectedHardware from dimos.control.task import ( ControlMode, ControlTask, @@ -78,8 +75,8 @@ from dimos.control.tick_loop import TickLoop __all__ = [ - # Hardware interface - "BackendHardwareInterface", + # Connected hardware + "ConnectedHardware", # Coordinator "ControlCoordinator", "ControlCoordinatorConfig", @@ -89,7 +86,6 @@ "CoordinatorState", "HardwareComponent", "HardwareId", - "HardwareInterface", "HardwareType", "JointCommandOutput", "JointName", diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py index f2365fbef6..515ff244d8 100644 --- a/dimos/control/blueprints.py +++ b/dimos/control/blueprints.py @@ -66,7 +66,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]: hardware_id="arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 7), - backend_type="mock", + adapter_type="mock", ), ], tasks=[ @@ -93,7 +93,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]: hardware_id="arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 7), - backend_type="xarm", + adapter_type="xarm", address="192.168.2.235", # Default IP, override via env or config auto_enable=True, ), @@ -122,7 +122,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]: hardware_id="arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), - backend_type="xarm", + adapter_type="xarm", address="192.168.1.210", auto_enable=True, ), @@ -151,7 +151,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]: hardware_id="arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), - backend_type="piper", + adapter_type="piper", address="can0", auto_enable=True, ), @@ -184,13 +184,13 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]: hardware_id="left_arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("left_arm", 7), - backend_type="mock", + adapter_type="mock", ), HardwareComponent( hardware_id="right_arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("right_arm", 6), - backend_type="mock", + adapter_type="mock", ), ], tasks=[ @@ -223,7 +223,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]: hardware_id="left_arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("left_arm", 7), - backend_type="xarm", + adapter_type="xarm", address="192.168.2.235", auto_enable=True, ), @@ -231,7 +231,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]: hardware_id="right_arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("right_arm", 6), - backend_type="xarm", + adapter_type="xarm", address="192.168.1.210", auto_enable=True, ), @@ -266,7 +266,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]: hardware_id="xarm_arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("xarm_arm", 6), - backend_type="xarm", + adapter_type="xarm", address="192.168.1.210", auto_enable=True, ), @@ -274,7 +274,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]: hardware_id="piper_arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("piper_arm", 6), - backend_type="piper", + adapter_type="piper", address="can0", auto_enable=True, ), @@ -313,7 +313,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]: hardware_id="arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 7), - backend_type="mock", + adapter_type="mock", ), ], tasks=[ diff --git a/dimos/control/components.py b/dimos/control/components.py index 0d48561f8a..e3022468ed 100644 --- a/dimos/control/components.py +++ b/dimos/control/components.py @@ -12,13 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Hardware component schema for the ControlCoordnator.""" +"""Hardware component schema for the ControlCoordinator.""" from dataclasses import dataclass, field from enum import Enum HardwareId = str JointName = str +TaskName = str class HardwareType(Enum): @@ -44,7 +45,7 @@ class HardwareComponent: hardware_id: Unique identifier, also used as joint name prefix hardware_type: Type of hardware (MANIPULATOR, BASE, GRIPPER) joints: List of joint names (e.g., ["arm_joint1", "arm_joint2", ...]) - backend_type: Backend type ("mock", "xarm", "piper") + adapter_type: Adapter type ("mock", "xarm", "piper") address: Connection address - IP for TCP, port for CAN auto_enable: Whether to auto-enable servos """ @@ -52,7 +53,7 @@ class HardwareComponent: hardware_id: HardwareId hardware_type: HardwareType joints: list[JointName] = field(default_factory=list) - backend_type: str = "mock" + adapter_type: str = "mock" address: str | None = None auto_enable: bool = True @@ -76,5 +77,6 @@ def make_joints(hardware_id: HardwareId, dof: int) -> list[JointName]: "HardwareType", "JointName", "JointState", + "TaskName", "make_joints", ] diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 6e8788381d..8931880e42 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -18,7 +18,7 @@ loops with a single deterministic tick-based system. Features: -- Single tick loop (read → compute → arbitrate → route → write) +- Single tick loop (read -> compute -> arbitrate -> route -> write) - Per-joint arbitration (highest priority wins) - Mode conflict detection - Partial command support (hold last value) @@ -32,8 +32,8 @@ import time from typing import TYPE_CHECKING, Any -from dimos.control.components import HardwareComponent -from dimos.control.hardware_interface import BackendHardwareInterface, HardwareInterface +from dimos.control.components import HardwareComponent, HardwareId, JointName, TaskName +from dimos.control.hardware_interface import ConnectedHardware from dimos.control.task import ControlTask from dimos.control.tick_loop import TickLoop from dimos.core import Module, Out, rpc @@ -45,7 +45,7 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from dimos.hardware.manipulators.spec import ManipulatorBackend + from dimos.hardware.manipulators.spec import ManipulatorAdapter logger = setup_logger() @@ -132,12 +132,12 @@ class ControlCoordinator(Module[ControlCoordinatorConfig]): Example: >>> from dimos.control import ControlCoordinator - >>> from dimos.hardware.manipulators.xarm import XArmBackend + >>> from dimos.hardware.manipulators.xarm import XArmAdapter >>> >>> orch = ControlCoordinator(tick_rate=100.0) - >>> backend = XArmBackend(ip="192.168.1.185", dof=7) - >>> backend.connect() - >>> orch.add_hardware("left_arm", backend, joint_prefix="left") + >>> adapter = XArmAdapter(ip="192.168.1.185", dof=7) + >>> adapter.connect() + >>> orch.add_hardware("left_arm", adapter, joint_prefix="left") >>> orch.start() """ @@ -150,15 +150,15 @@ class ControlCoordinator(Module[ControlCoordinatorConfig]): def __init__(self, *args: Any, **kwargs: Any) -> None: super().__init__(*args, **kwargs) - # Hardware interfaces (keyed by hardware_id) - self._hardware: dict[str, HardwareInterface] = {} + # Connected hardware (keyed by hardware_id) + self._hardware: dict[HardwareId, ConnectedHardware] = {} self._hardware_lock = threading.Lock() # Joint -> hardware mapping (built when hardware added) - self._joint_to_hardware: dict[str, str] = {} + self._joint_to_hardware: dict[JointName, HardwareId] = {} # Registered tasks - self._tasks: dict[str, ControlTask] = {} + self._tasks: dict[TaskName, ControlTask] = {} self._task_lock = threading.Lock() # Tick loop (created on start) @@ -193,41 +193,30 @@ def _setup_from_config(self) -> None: raise def _setup_hardware(self, component: HardwareComponent) -> None: - """Connect and add a single hardware backend.""" - backend = self._create_backend(component) + """Connect and add a single hardware adapter.""" + adapter = self._create_adapter(component) - if not backend.connect(): - raise RuntimeError(f"Failed to connect to {component.backend_type} backend") + if not adapter.connect(): + raise RuntimeError(f"Failed to connect to {component.adapter_type} adapter") try: - if component.auto_enable and hasattr(backend, "write_enable"): - backend.write_enable(True) + if component.auto_enable and hasattr(adapter, "write_enable"): + adapter.write_enable(True) - self.add_hardware(backend, component) + self.add_hardware(adapter, component) except Exception: - backend.disconnect() + adapter.disconnect() raise - def _create_backend(self, component: HardwareComponent) -> ManipulatorBackend: - """Create a manipulator backend from component config.""" - dof = len(component.joints) - match component.backend_type.lower(): - case "mock": - from dimos.hardware.manipulators.mock import MockBackend + def _create_adapter(self, component: HardwareComponent) -> ManipulatorAdapter: + """Create a manipulator adapter from component config.""" + from dimos.hardware.manipulators.registry import adapter_registry - return MockBackend(dof=dof) - case "xarm": - if component.address is None: - raise ValueError("address (IP) is required for xarm backend") - from dimos.hardware.manipulators.xarm import XArmBackend - - return XArmBackend(ip=component.address, dof=dof) - case "piper": - from dimos.hardware.manipulators.piper import PiperBackend - - return PiperBackend(can_port=component.address or "can0", dof=dof) - case _: - raise ValueError(f"Unknown backend type: {component.backend_type}") + return adapter_registry.create( + component.adapter_type, + dof=len(component.joints), + address=component.address, + ) def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: """Create a control task from config.""" @@ -254,26 +243,26 @@ def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask: @rpc def add_hardware( self, - backend: ManipulatorBackend, + adapter: ManipulatorAdapter, component: HardwareComponent, ) -> bool: - """Register a hardware backend with the coordinator.""" + """Register a hardware adapter with the coordinator.""" with self._hardware_lock: if component.hardware_id in self._hardware: logger.warning(f"Hardware {component.hardware_id} already registered") return False - interface = BackendHardwareInterface( - backend=backend, + connected = ConnectedHardware( + adapter=adapter, component=component, ) - self._hardware[component.hardware_id] = interface + self._hardware[component.hardware_id] = connected - for joint_name in interface.joint_names: + for joint_name in connected.joint_names: self._joint_to_hardware[joint_name] = component.hardware_id logger.info( - f"Added hardware {component.hardware_id} with joints: {interface.joint_names}" + f"Added hardware {component.hardware_id} with joints: {connected.joint_names}" ) return True @@ -483,7 +472,7 @@ def stop(self) -> None: if self._tick_loop: self._tick_loop.stop() - # Disconnect all hardware backends + # Disconnect all hardware adapters with self._hardware_lock: for hw_id, interface in self._hardware.items(): try: diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py index ed320abec5..2df1083511 100644 --- a/dimos/control/hardware_interface.py +++ b/dimos/control/hardware_interface.py @@ -12,9 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Hardware interface for the ControlCoordinator. +"""Connected hardware for the ControlCoordinator. -Wraps ManipulatorBackend with coordinator-specific features: +Wraps ManipulatorAdapter with coordinator-specific features: - Namespaced joint names (e.g., "left_joint1") - Unified read/write interface - Hold-last-value for partial commands @@ -24,9 +24,9 @@ import logging import time -from typing import TYPE_CHECKING, Protocol, runtime_checkable +from typing import TYPE_CHECKING -from dimos.hardware.manipulators.spec import ControlMode, ManipulatorBackend +from dimos.hardware.manipulators.spec import ControlMode, ManipulatorAdapter if TYPE_CHECKING: from dimos.control.components import HardwareComponent, HardwareId, JointName, JointState @@ -34,77 +34,33 @@ logger = logging.getLogger(__name__) -@runtime_checkable -class HardwareInterface(Protocol): - """Protocol for hardware that the coordinator can control. +class ConnectedHardware: + """Runtime wrapper for hardware connected to the coordinator. - This wraps ManipulatorBackend with coordinator-specific features: - - Namespaced joint names (e.g., "left_arm_joint1") - - Unified read/write interface - - State caching - """ - - @property - def hardware_id(self) -> HardwareId: - """Unique ID for this hardware (e.g., 'left_arm').""" - ... - - @property - def joint_names(self) -> list[JointName]: - """Ordered list of fully-qualified joint names this hardware controls.""" - ... - - def read_state(self) -> dict[JointName, JointState]: - """Read current state. - - Returns: - Dict of joint_name -> JointState(position, velocity, effort) - """ - ... - - def write_command(self, commands: dict[JointName, float], mode: ControlMode) -> bool: - """Write commands to hardware. - - IMPORTANT: Accepts partial joint sets. Missing joints hold last value. - - Args: - commands: {joint_name: value} - can be partial - mode: Control mode (POSITION, VELOCITY, TORQUE) - - Returns: - True if command was sent successfully - """ - ... - - def disconnect(self) -> None: - """Disconnect the underlying hardware.""" - ... - - -class BackendHardwareInterface: - """Concrete implementation wrapping a ManipulatorBackend. + Wraps a ManipulatorAdapter with coordinator-specific features: + - Joint names from HardwareComponent config + - Hold-last-value for partial commands + - Converts between joint names and array indices - Features: - - Uses joint names from HardwareComponent - - Holds last commanded value for partial commands - - On first tick, reads current position from hardware for missing joints + Created when hardware is added to the coordinator. One instance + per physical hardware device. """ def __init__( self, - backend: ManipulatorBackend, + adapter: ManipulatorAdapter, component: HardwareComponent, ) -> None: """Initialize hardware interface. Args: - backend: ManipulatorBackend instance (XArmBackend, PiperBackend, etc.) + adapter: ManipulatorAdapter instance (XArmAdapter, PiperAdapter, etc.) component: Hardware component with joints config """ - if not isinstance(backend, ManipulatorBackend): - raise TypeError("backend must implement ManipulatorBackend") + if not isinstance(adapter, ManipulatorAdapter): + raise TypeError("adapter must implement ManipulatorAdapter") - self._backend = backend + self._adapter = adapter self._component = component self._joint_names = component.joints @@ -135,8 +91,8 @@ def dof(self) -> int: return len(self._joint_names) def disconnect(self) -> None: - """Disconnect the underlying backend.""" - self._backend.disconnect() + """Disconnect the underlying adapter.""" + self._adapter.disconnect() def read_state(self) -> dict[JointName, JointState]: """Read state as {joint_name: JointState}. @@ -146,9 +102,9 @@ def read_state(self) -> dict[JointName, JointState]: """ from dimos.control.components import JointState - positions = self._backend.read_joint_positions() - velocities = self._backend.read_joint_velocities() - efforts = self._backend.read_joint_efforts() + positions = self._adapter.read_joint_positions() + velocities = self._adapter.read_joint_velocities() + efforts = self._adapter.read_joint_efforts() return { name: JointState( @@ -189,22 +145,22 @@ def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool: ) self._warned_unknown_joints.add(joint_name) - # Build ordered list for backend + # Build ordered list for adapter ordered = self._build_ordered_command() # Switch control mode if needed if mode != self._current_mode: - if not self._backend.set_control_mode(mode): + if not self._adapter.set_control_mode(mode): logger.warning(f"Hardware {self.hardware_id} failed to switch to {mode.name}") return False self._current_mode = mode - # Send to backend + # Send to adapter match mode: case ControlMode.POSITION | ControlMode.SERVO_POSITION: - return self._backend.write_joint_positions(ordered) + return self._adapter.write_joint_positions(ordered) case ControlMode.VELOCITY: - return self._backend.write_joint_velocities(ordered) + return self._adapter.write_joint_velocities(ordered) case ControlMode.TORQUE: logger.warning(f"Hardware {self.hardware_id} does not support torque mode") return False @@ -215,7 +171,7 @@ def _initialize_last_commanded(self) -> None: """Initialize last_commanded with current hardware positions.""" for _ in range(10): try: - current = self._backend.read_joint_positions() + current = self._adapter.read_joint_positions() for i, name in enumerate(self._joint_names): self._last_commanded[name] = current[i] self._initialized = True @@ -233,6 +189,5 @@ def _build_ordered_command(self) -> list[float]: __all__ = [ - "BackendHardwareInterface", - "HardwareInterface", + "ConnectedHardware", ] diff --git a/dimos/control/tasks/__init__.py b/dimos/control/tasks/__init__.py index 75460ffa26..63dcd93664 100644 --- a/dimos/control/tasks/__init__.py +++ b/dimos/control/tasks/__init__.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Task implementations for the ControlOrchestrator.""" +"""Task implementations for the ControlCoordinator.""" from dimos.control.tasks.trajectory_task import ( JointTrajectoryTask, diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index 8d51537bb3..656678d167 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -23,7 +23,7 @@ import pytest from dimos.control.components import HardwareComponent, HardwareType, make_joints -from dimos.control.hardware_interface import BackendHardwareInterface +from dimos.control.hardware_interface import ConnectedHardware from dimos.control.task import ( ControlMode, CoordinatorState, @@ -37,7 +37,7 @@ TrajectoryState, ) from dimos.control.tick_loop import TickLoop -from dimos.hardware.manipulators.spec import ManipulatorBackend +from dimos.hardware.manipulators.spec import ManipulatorAdapter from dimos.msgs.trajectory_msgs import JointTrajectory, TrajectoryPoint # ============================================================================= @@ -46,28 +46,28 @@ @pytest.fixture -def mock_backend(): - """Create a mock manipulator backend.""" - backend = MagicMock(spec=ManipulatorBackend) - backend.get_dof.return_value = 6 - backend.read_joint_positions.return_value = [0.0] * 6 - backend.read_joint_velocities.return_value = [0.0] * 6 - backend.read_joint_efforts.return_value = [0.0] * 6 - backend.write_joint_positions.return_value = True - backend.write_joint_velocities.return_value = True - backend.set_control_mode.return_value = True - return backend +def mock_adapter(): + """Create a mock manipulator adapter.""" + adapter = MagicMock(spec=ManipulatorAdapter) + adapter.get_dof.return_value = 6 + adapter.read_joint_positions.return_value = [0.0] * 6 + adapter.read_joint_velocities.return_value = [0.0] * 6 + adapter.read_joint_efforts.return_value = [0.0] * 6 + adapter.write_joint_positions.return_value = True + adapter.write_joint_velocities.return_value = True + adapter.set_control_mode.return_value = True + return adapter @pytest.fixture -def hardware_interface(mock_backend): - """Create a BackendHardwareInterface with mock backend.""" +def connected_hardware(mock_adapter): + """Create a ConnectedHardware instance with mock adapter.""" component = HardwareComponent( hardware_id="test_arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), ) - return BackendHardwareInterface(backend=mock_backend, component=component) + return ConnectedHardware(adapter=mock_adapter, component=component) @pytest.fixture @@ -172,13 +172,13 @@ def test_get_position(self): # ============================================================================= -# Test BackendHardwareInterface +# Test ConnectedHardware # ============================================================================= -class TestBackendHardwareInterface: - def test_joint_names_prefixed(self, hardware_interface): - names = hardware_interface.joint_names +class TestConnectedHardware: + def test_joint_names_prefixed(self, connected_hardware): + names = connected_hardware.joint_names assert names == [ "arm_joint1", "arm_joint2", @@ -188,8 +188,8 @@ def test_joint_names_prefixed(self, hardware_interface): "arm_joint6", ] - def test_read_state(self, hardware_interface): - state = hardware_interface.read_state() + def test_read_state(self, connected_hardware): + state = connected_hardware.read_state() assert "arm_joint1" in state assert len(state) == 6 joint_state = state["arm_joint1"] @@ -197,13 +197,13 @@ def test_read_state(self, hardware_interface): assert joint_state.velocity == 0.0 assert joint_state.effort == 0.0 - def test_write_command(self, hardware_interface, mock_backend): + def test_write_command(self, connected_hardware, mock_adapter): commands = { "arm_joint1": 0.5, "arm_joint2": 1.0, } - hardware_interface.write_command(commands, ControlMode.POSITION) - mock_backend.write_joint_positions.assert_called() + connected_hardware.write_command(commands, ControlMode.POSITION) + mock_adapter.write_joint_positions.assert_called() # ============================================================================= @@ -428,13 +428,13 @@ def test_non_overlapping_joints(self): class TestTickLoop: - def test_tick_loop_starts_and_stops(self, mock_backend): + def test_tick_loop_starts_and_stops(self, mock_adapter): component = HardwareComponent( hardware_id="arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), ) - hw = BackendHardwareInterface(mock_backend, component) + hw = ConnectedHardware(mock_adapter, component) hardware = {"arm": hw} tasks: dict = {} joint_to_hardware = {f"arm_joint{i + 1}": "arm" for i in range(6)} @@ -457,13 +457,13 @@ def test_tick_loop_starts_and_stops(self, mock_backend): time.sleep(0.02) assert tick_loop.tick_count == final_count - def test_tick_loop_calls_compute(self, mock_backend): + def test_tick_loop_calls_compute(self, mock_adapter): component = HardwareComponent( hardware_id="arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), ) - hw = BackendHardwareInterface(mock_backend, component) + hw = ConnectedHardware(mock_adapter, component) hardware = {"arm": hw} mock_task = MagicMock() @@ -504,13 +504,13 @@ def test_tick_loop_calls_compute(self, mock_backend): class TestIntegration: - def test_full_trajectory_execution(self, mock_backend): + def test_full_trajectory_execution(self, mock_adapter): component = HardwareComponent( hardware_id="arm", hardware_type=HardwareType.MANIPULATOR, joints=make_joints("arm", 6), ) - hw = BackendHardwareInterface(mock_backend, component) + hw = ConnectedHardware(mock_adapter, component) hardware = {"arm": hw} config = JointTrajectoryTaskConfig( @@ -554,4 +554,4 @@ def test_full_trajectory_execution(self, mock_backend): tick_loop.stop() assert traj_task.get_state() == TrajectoryState.COMPLETED - assert mock_backend.write_joint_positions.call_count > 0 + assert mock_adapter.write_joint_positions.call_count > 0 diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index 7812325ddf..e0020a34da 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -44,7 +44,8 @@ if TYPE_CHECKING: from collections.abc import Callable - from dimos.control.hardware_interface import HardwareInterface + from dimos.control.components import HardwareId, JointName, TaskName + from dimos.control.hardware_interface import ConnectedHardware from dimos.hardware.manipulators.spec import ControlMode logger = setup_logger() @@ -73,7 +74,7 @@ class TickLoop: Args: tick_rate: Control loop frequency in Hz - hardware: Dict of hardware_id -> HardwareInterface + hardware: Dict of hardware_id -> ConnectedHardware hardware_lock: Lock protecting hardware dict tasks: Dict of task_name -> ControlTask task_lock: Lock protecting tasks dict @@ -86,11 +87,11 @@ class TickLoop: def __init__( self, tick_rate: float, - hardware: dict[str, HardwareInterface], + hardware: dict[HardwareId, ConnectedHardware], hardware_lock: threading.Lock, - tasks: dict[str, ControlTask], + tasks: dict[TaskName, ControlTask], task_lock: threading.Lock, - joint_to_hardware: dict[str, str], + joint_to_hardware: dict[JointName, HardwareId], publish_callback: Callable[[JointState], None] | None = None, frame_id: str = "coordinator", log_ticks: bool = False, diff --git a/dimos/e2e_tests/test_control_coordinator.py b/dimos/e2e_tests/test_control_coordinator.py index 7eb563e243..880b78fafa 100644 --- a/dimos/e2e_tests/test_control_coordinator.py +++ b/dimos/e2e_tests/test_control_coordinator.py @@ -255,7 +255,7 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: left_status = client.get_trajectory_status("traj_left") right_status = client.get_trajectory_status("traj_right") - assert left_status is not None and left_status.state == TrajectoryState.COMPLETED.name - assert right_status is not None and right_status.state == TrajectoryState.COMPLETED.name + assert left_status.state == TrajectoryState.COMPLETED.name + assert right_status.state == TrajectoryState.COMPLETED.name finally: client.stop_rpc_client() diff --git a/dimos/hardware/manipulators/README.md b/dimos/hardware/manipulators/README.md index d3e54d4cb0..60d3c94567 100644 --- a/dimos/hardware/manipulators/README.md +++ b/dimos/hardware/manipulators/README.md @@ -1,6 +1,6 @@ # Manipulator Drivers -This module provides manipulator arm drivers using the **B-lite architecture**: Protocol-only with injectable backends. +This module provides manipulator arm drivers: Protocol-only with injectable adapters. ## Architecture Overview @@ -14,35 +14,32 @@ This module provides manipulator arm drivers using the **B-lite architecture**: └─────────────────────┬───────────────────────────────────────┘ │ uses ┌─────────────────────▼───────────────────────────────────────┐ -│ Backend (implements Protocol) │ +│ Adapter (implements Protocol) │ │ - Handles SDK communication │ │ - Unit conversions (radians ↔ vendor units) │ -│ - Swappable: XArmBackend, PiperBackend, MockBackend │ +│ - Swappable: XArmAdapter, PiperAdapter, MockAdapter │ └─────────────────────────────────────────────────────────────┘ ``` ## Key Benefits -- **Testable**: Inject `MockBackend` for unit tests without hardware +- **Testable**: Inject `MockAdapter` for unit tests without hardware - **Flexible**: Each arm controls its own threading/timing - **Simple**: No ABC inheritance required - just implement the Protocol -- **Type-safe**: Full type checking via `ManipulatorBackend` Protocol +- **Type-safe**: Full type checking via `ManipulatorAdapter` Protocol ## Directory Structure ``` manipulators/ -├── spec.py # ManipulatorBackend Protocol + shared types +├── spec.py # ManipulatorAdapter Protocol + shared types +├── registry.py # Adapter registry with auto-discovery ├── mock/ -│ └── backend.py # MockBackend for testing +│ └── adapter.py # MockAdapter for testing ├── xarm/ -│ ├── backend.py # XArmBackend (SDK wrapper) -│ ├── arm.py # XArm driver module -│ └── blueprints.py # Pre-configured blueprints +│ ├── adapter.py # XArmAdapter (SDK wrapper) └── piper/ - ├── backend.py # PiperBackend (SDK wrapper) - ├── arm.py # Piper driver module - └── blueprints.py # Pre-configured blueprints + ├── adapter.py # PiperAdapter (SDK wrapper) ``` ## Quick Start @@ -71,20 +68,20 @@ coordinator.loop() ### Testing Without Hardware ```python -from dimos.hardware.manipulators.mock import MockBackend +from dimos.hardware.manipulators.mock import MockAdapter from dimos.hardware.manipulators.xarm import XArm -arm = XArm(backend=MockBackend(dof=6)) +arm = XArm(adapter=MockAdapter(dof=6)) arm.start() # No hardware needed! arm.move_joint([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) ``` ## Adding a New Arm -1. **Create the backend** (`backend.py`): +1. **Create the adapter** (`adapter.py`): ```python -class MyArmBackend: # No inheritance needed - just match the Protocol +class MyArmAdapter: # No inheritance needed - just match the Protocol def __init__(self, ip: str = "192.168.1.100", dof: int = 6) -> None: self._ip = ip self._dof = dof @@ -100,16 +97,16 @@ class MyArmBackend: # No inheritance needed - just match the Protocol ```python from dimos.core import Module, ModuleConfig, In, Out, rpc -from .backend import MyArmBackend +from .adapter import MyArmAdapter class MyArm(Module[MyArmConfig]): joint_state: Out[JointState] robot_state: Out[RobotState] joint_position_command: In[JointCommand] - def __init__(self, backend=None, **kwargs): + def __init__(self, adapter=None, **kwargs): super().__init__(**kwargs) - self.backend = backend or MyArmBackend( + self.adapter = adapter or MyArmAdapter( ip=self.config.ip, dof=self.config.dof, ) @@ -118,9 +115,9 @@ class MyArm(Module[MyArmConfig]): 3. **Create blueprints** (`blueprints.py`) for common configurations. -## ManipulatorBackend Protocol +## ManipulatorAdapter Protocol -All backends must implement these core methods: +All adapters must implement these core methods: | Category | Methods | |----------|---------| @@ -138,7 +135,7 @@ Optional methods (return `None`/`False` if unsupported): ## Unit Conventions -All backends convert to/from SI units: +All adapters convert to/from SI units: | Quantity | Unit | |----------|------| @@ -147,17 +144,3 @@ All backends convert to/from SI units: | Torque | Nm | | Position | meters | | Force | Newtons | - -## Available Blueprints - -### XArm -- `xarm_servo` - Basic servo control (6-DOF) -- `xarm5_servo`, `xarm7_servo` - 5/7-DOF variants -- `xarm_trajectory` - Driver + trajectory controller -- `xarm_cartesian` - Driver + cartesian controller - -### Piper -- `piper_servo` - Basic servo control -- `piper_servo_gripper` - With gripper support -- `piper_trajectory` - Driver + trajectory controller -- `piper_left`, `piper_right` - Dual arm configurations diff --git a/dimos/hardware/manipulators/__init__.py b/dimos/hardware/manipulators/__init__.py index e4133dbb51..58986c9211 100644 --- a/dimos/hardware/manipulators/__init__.py +++ b/dimos/hardware/manipulators/__init__.py @@ -14,12 +14,11 @@ """Manipulator drivers for robotic arms. -Architecture: B-lite (Protocol-based backends with per-arm drivers) - -- spec.py: ManipulatorBackend Protocol and shared types -- xarm/: XArm driver and backend -- piper/: Piper driver and backend -- mock/: Mock backend for testing +Architecture: Protocol-based adapters for different manipulator hardware. +- spec.py: ManipulatorAdapter Protocol and shared types +- xarm/: XArm adapter +- piper/: Piper adapter +- mock/: Mock adapter for testing Usage: >>> from dimos.hardware.manipulators.xarm import XArm @@ -30,8 +29,8 @@ Testing: >>> from dimos.hardware.manipulators.xarm import XArm - >>> from dimos.hardware.manipulators.mock import MockBackend - >>> arm = XArm(backend=MockBackend()) + >>> from dimos.hardware.manipulators.mock import MockAdapter + >>> arm = XArm(adapter=MockAdapter()) >>> arm.start() # No hardware needed! """ @@ -39,7 +38,7 @@ ControlMode, DriverStatus, JointLimits, - ManipulatorBackend, + ManipulatorAdapter, ManipulatorInfo, ) @@ -47,6 +46,6 @@ "ControlMode", "DriverStatus", "JointLimits", - "ManipulatorBackend", + "ManipulatorAdapter", "ManipulatorInfo", ] diff --git a/dimos/hardware/manipulators/mock/__init__.py b/dimos/hardware/manipulators/mock/__init__.py index 87428973a4..63be6f7e98 100644 --- a/dimos/hardware/manipulators/mock/__init__.py +++ b/dimos/hardware/manipulators/mock/__init__.py @@ -12,17 +12,17 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Mock backend for testing manipulator drivers without hardware. +"""Mock adapter for testing manipulator drivers without hardware. Usage: >>> from dimos.hardware.manipulators.xarm import XArm - >>> from dimos.hardware.manipulators.mock import MockBackend - >>> arm = XArm(backend=MockBackend()) + >>> from dimos.hardware.manipulators.mock import MockAdapter + >>> arm = XArm(adapter=MockAdapter()) >>> arm.start() # No hardware needed! >>> arm.move_joint([0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) - >>> assert arm.backend.read_joint_positions() == [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] + >>> assert arm.adapter.read_joint_positions() == [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] """ -from dimos.hardware.manipulators.mock.backend import MockBackend +from dimos.hardware.manipulators.mock.adapter import MockAdapter -__all__ = ["MockBackend"] +__all__ = ["MockAdapter"] diff --git a/dimos/hardware/manipulators/mock/backend.py b/dimos/hardware/manipulators/mock/adapter.py similarity index 92% rename from dimos/hardware/manipulators/mock/backend.py rename to dimos/hardware/manipulators/mock/adapter.py index 80b3543739..ff299669f7 100644 --- a/dimos/hardware/manipulators/mock/backend.py +++ b/dimos/hardware/manipulators/mock/adapter.py @@ -12,16 +12,22 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Mock backend for testing - no hardware required. +"""Mock adapter for testing - no hardware required. Usage: >>> from dimos.hardware.manipulators.xarm import XArm - >>> from dimos.hardware.manipulators.mock import MockBackend - >>> arm = XArm(backend=MockBackend()) + >>> from dimos.hardware.manipulators.mock import MockAdapter + >>> arm = XArm(adapter=MockAdapter()) >>> arm.start() # No hardware! """ +from __future__ import annotations + import math +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from dimos.hardware.manipulators.registry import AdapterRegistry from dimos.hardware.manipulators.spec import ( ControlMode, @@ -30,17 +36,17 @@ ) -class MockBackend: - """Fake backend for unit tests. +class MockAdapter: + """Fake adapter for unit tests. - Implements ManipulatorBackend protocol with in-memory state. + Implements ManipulatorAdapter protocol with in-memory state. Useful for: - Unit testing driver logic without hardware - Integration testing with predictable behavior - Development without physical robot """ - def __init__(self, dof: int = 6) -> None: + def __init__(self, dof: int = 6, **_: object) -> None: self._dof = dof self._positions = [0.0] * dof self._velocities = [0.0] * dof @@ -247,4 +253,9 @@ def set_efforts(self, efforts: list[float]) -> None: self._efforts = list(efforts) -__all__ = ["MockBackend"] +def register(registry: AdapterRegistry) -> None: + """Register this adapter with the registry.""" + registry.register("mock", MockAdapter) + + +__all__ = ["MockAdapter"] diff --git a/dimos/hardware/manipulators/piper/__init__.py b/dimos/hardware/manipulators/piper/__init__.py index 16c6e451cd..bfeb89b1c0 100644 --- a/dimos/hardware/manipulators/piper/__init__.py +++ b/dimos/hardware/manipulators/piper/__init__.py @@ -12,15 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Piper manipulator hardware backend. +"""Piper manipulator hardware adapter. Usage: - >>> from dimos.hardware.manipulators.piper import PiperBackend - >>> backend = PiperBackend(can_port="can0") - >>> backend.connect() - >>> positions = backend.read_joint_positions() + >>> from dimos.hardware.manipulators.piper import PiperAdapter + >>> adapter = PiperAdapter(can_port="can0") + >>> adapter.connect() + >>> positions = adapter.read_joint_positions() """ -from dimos.hardware.manipulators.piper.backend import PiperBackend +from dimos.hardware.manipulators.piper.adapter import PiperAdapter -__all__ = ["PiperBackend"] +__all__ = ["PiperAdapter"] diff --git a/dimos/hardware/manipulators/piper/backend.py b/dimos/hardware/manipulators/piper/adapter.py similarity index 86% rename from dimos/hardware/manipulators/piper/backend.py rename to dimos/hardware/manipulators/piper/adapter.py index 1ce91dccd1..68b5769a95 100644 --- a/dimos/hardware/manipulators/piper/backend.py +++ b/dimos/hardware/manipulators/piper/adapter.py @@ -12,32 +12,45 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Piper backend - implements ManipulatorBackend protocol. +"""Piper adapter - implements ManipulatorAdapter protocol. -Handles all Piper SDK communication and unit conversion. +SDK Units: angles=0.001 degrees (millidegrees), distance=mm +DimOS Units: angles=radians, distance=meters """ +from __future__ import annotations + import math import time -from typing import Any +from typing import TYPE_CHECKING, Any + +if TYPE_CHECKING: + from dimos.hardware.manipulators.registry import AdapterRegistry from dimos.hardware.manipulators.spec import ( ControlMode, JointLimits, - ManipulatorBackend, + ManipulatorAdapter, ManipulatorInfo, ) # Unit conversion constants -# Piper uses 0.001 degrees internally -RAD_TO_PIPER = 57295.7795 # radians to Piper units (0.001 degrees) -PIPER_TO_RAD = 1.0 / RAD_TO_PIPER # Piper units to radians +# Piper uses 0.001 degrees (millidegrees) for angles +RAD_TO_MILLIDEG = 57295.7795 # radians -> millidegrees +MILLIDEG_TO_RAD = 1.0 / RAD_TO_MILLIDEG # millidegrees -> radians +MM_TO_M = 0.001 # mm -> meters + +# Hardware specs +GRIPPER_MAX_OPENING_M = 0.08 # Max gripper opening in meters + +# Default configurable parameters +DEFAULT_GRIPPER_SPEED = 1000 -class PiperBackend(ManipulatorBackend): - """Piper-specific backend. +class PiperAdapter(ManipulatorAdapter): + """Piper-specific adapter. - Implements ManipulatorBackend protocol via duck typing. + Implements ManipulatorAdapter protocol via duck typing. No inheritance required - just matching method signatures. Unit conversions: @@ -45,11 +58,18 @@ class PiperBackend(ManipulatorBackend): - Velocities: Piper uses internal units, we use rad/s """ - def __init__(self, can_port: str = "can0", dof: int = 6) -> None: + def __init__( + self, + address: str = "can0", + dof: int = 6, + gripper_speed: int = DEFAULT_GRIPPER_SPEED, + **_: object, + ) -> None: if dof != 6: - raise ValueError(f"PiperBackend only supports 6 DOF (got {dof})") - self._can_port = can_port + raise ValueError(f"PiperAdapter only supports 6 DOF (got {dof})") + self._can_port = address self._dof = dof + self._gripper_speed = gripper_speed self._sdk: Any = None self._connected: bool = False self._enabled: bool = False @@ -202,12 +222,12 @@ def read_joint_positions(self) -> list[float]: js = joint_msgs.joint_state return [ - js.joint_1 * PIPER_TO_RAD, - js.joint_2 * PIPER_TO_RAD, - js.joint_3 * PIPER_TO_RAD, - js.joint_4 * PIPER_TO_RAD, - js.joint_5 * PIPER_TO_RAD, - js.joint_6 * PIPER_TO_RAD, + js.joint_1 * MILLIDEG_TO_RAD, + js.joint_2 * MILLIDEG_TO_RAD, + js.joint_3 * MILLIDEG_TO_RAD, + js.joint_4 * MILLIDEG_TO_RAD, + js.joint_5 * MILLIDEG_TO_RAD, + js.joint_6 * MILLIDEG_TO_RAD, ] def read_joint_velocities(self) -> list[float]: @@ -294,7 +314,7 @@ def write_joint_positions( return False # Convert radians to Piper units (0.001 degrees) - piper_joints = [round(rad * RAD_TO_PIPER) for rad in positions] + piper_joints = [round(rad * RAD_TO_MILLIDEG) for rad in positions] # Set speed rate if not full speed if velocity < 1.0: @@ -426,12 +446,12 @@ def read_cartesian_position(self) -> dict[str, float] | None: if pose_msgs and pose_msgs.end_pose: ep = pose_msgs.end_pose return { - "x": ep.X_axis / 1000.0, # mm -> m - "y": ep.Y_axis / 1000.0, - "z": ep.Z_axis / 1000.0, - "roll": ep.RX_axis * PIPER_TO_RAD, - "pitch": ep.RY_axis * PIPER_TO_RAD, - "yaw": ep.RZ_axis * PIPER_TO_RAD, + "x": ep.X_axis * MM_TO_M, + "y": ep.Y_axis * MM_TO_M, + "z": ep.Z_axis * MM_TO_M, + "roll": ep.RX_axis * MILLIDEG_TO_RAD, + "pitch": ep.RY_axis * MILLIDEG_TO_RAD, + "yaw": ep.RZ_axis * MILLIDEG_TO_RAD, } except Exception: pass @@ -464,9 +484,8 @@ def read_gripper_position(self) -> float | None: gripper_msgs = self._sdk.GetArmGripperMsgs() if gripper_msgs and gripper_msgs.gripper_state: # Piper gripper position is 0-100 percentage - # Convert to meters (assume max opening 0.08m) - pos = gripper_msgs.gripper_state.grippers_angle - return float(pos / 100.0) * 0.08 + pos: float = gripper_msgs.gripper_state.grippers_angle + return (pos / 100.0) * GRIPPER_MAX_OPENING_M except Exception: pass @@ -480,10 +499,9 @@ def write_gripper_position(self, position: float) -> bool: try: if hasattr(self._sdk, "GripperCtrl"): # Convert meters to percentage (0-100) - # Assume max opening 0.08m - percentage = int((position / 0.08) * 100) + percentage = int((position / GRIPPER_MAX_OPENING_M) * 100) percentage = max(0, min(100, percentage)) - self._sdk.GripperCtrl(percentage, 1000, 0x01, 0) + self._sdk.GripperCtrl(percentage, self._gripper_speed, 0x01, 0) return True except Exception: pass @@ -502,4 +520,9 @@ def read_force_torque(self) -> list[float] | None: return None -__all__ = ["PiperBackend"] +def register(registry: AdapterRegistry) -> None: + """Register this adapter with the registry.""" + registry.register("piper", PiperAdapter) + + +__all__ = ["PiperAdapter"] diff --git a/dimos/hardware/manipulators/registry.py b/dimos/hardware/manipulators/registry.py new file mode 100644 index 0000000000..65dbe74b50 --- /dev/null +++ b/dimos/hardware/manipulators/registry.py @@ -0,0 +1,99 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Adapter registry with auto-discovery. + +Automatically discovers and registers manipulator adapters from subpackages. +Each adapter provides a `register()` function in its adapter.py module. + +Usage: + from dimos.hardware.manipulators.registry import adapter_registry + + # Create an adapter by name + adapter = adapter_registry.create("xarm", ip="192.168.1.185", dof=6) + adapter = adapter_registry.create("piper", can_port="can0", dof=6) + adapter = adapter_registry.create("mock", dof=7) + + # List available adapters + print(adapter_registry.available()) # ["mock", "piper", "xarm"] +""" + +from __future__ import annotations + +import importlib +import logging +import pkgutil +from typing import TYPE_CHECKING, Any + +if TYPE_CHECKING: + from dimos.hardware.manipulators.spec import ManipulatorAdapter + +logger = logging.getLogger(__name__) + + +class AdapterRegistry: + """Registry for manipulator adapters with auto-discovery.""" + + def __init__(self) -> None: + self._adapters: dict[str, type[ManipulatorAdapter]] = {} + + def register(self, name: str, cls: type[ManipulatorAdapter]) -> None: + """Register an adapter class.""" + self._adapters[name.lower()] = cls + + def create(self, name: str, **kwargs: Any) -> ManipulatorAdapter: + """Create an adapter instance by name. + + Args: + name: Adapter name (e.g., "xarm", "piper", "mock") + **kwargs: Arguments passed to adapter constructor + + Returns: + Configured adapter instance + + Raises: + KeyError: If adapter name is not found + """ + key = name.lower() + if key not in self._adapters: + raise KeyError(f"Unknown adapter: {name}. Available: {self.available()}") + + return self._adapters[key](**kwargs) + + def available(self) -> list[str]: + """List available adapter names.""" + return sorted(self._adapters.keys()) + + def discover(self) -> None: + """Discover and register adapters from subpackages. + + Can be called multiple times to pick up newly added adapters. + """ + import dimos.hardware.manipulators as pkg + + for _, name, ispkg in pkgutil.iter_modules(pkg.__path__): + if not ispkg: + continue + try: + module = importlib.import_module(f"dimos.hardware.manipulators.{name}.adapter") + if hasattr(module, "register"): + module.register(self) + except ImportError as e: + logger.debug(f"Skipping adapter {name}: {e}") + + +adapter_registry = AdapterRegistry() +adapter_registry.discover() + +__all__ = ["AdapterRegistry", "adapter_registry"] diff --git a/dimos/hardware/manipulators/spec.py b/dimos/hardware/manipulators/spec.py index 585043421e..ff4d38c54f 100644 --- a/dimos/hardware/manipulators/spec.py +++ b/dimos/hardware/manipulators/spec.py @@ -16,7 +16,7 @@ This file defines: 1. Shared enums and dataclasses used by all arms -2. ManipulatorBackend Protocol that backends must implement +2. ManipulatorAdapter Protocol that adapters must implement Note: No ABC for drivers. Each arm implements its own driver with full control over threading and logic. @@ -84,12 +84,12 @@ def default_base_transform() -> Transform: # ============================================================================ -# BACKEND PROTOCOL +# ADAPTER PROTOCOL # ============================================================================ @runtime_checkable -class ManipulatorBackend(Protocol): +class ManipulatorAdapter(Protocol): """Protocol for hardware-specific IO. Implement this per vendor SDK. All methods use SI units: @@ -255,7 +255,7 @@ def read_force_torque(self) -> list[float] | None: "ControlMode", "DriverStatus", "JointLimits", - "ManipulatorBackend", + "ManipulatorAdapter", "ManipulatorInfo", "default_base_transform", ] diff --git a/dimos/hardware/manipulators/xarm/__init__.py b/dimos/hardware/manipulators/xarm/__init__.py index 343ebc4e0e..8bcab667c1 100644 --- a/dimos/hardware/manipulators/xarm/__init__.py +++ b/dimos/hardware/manipulators/xarm/__init__.py @@ -12,15 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""XArm manipulator hardware backend. +"""XArm manipulator hardware adapter. Usage: - >>> from dimos.hardware.manipulators.xarm import XArmBackend - >>> backend = XArmBackend(ip="192.168.1.185", dof=6) - >>> backend.connect() - >>> positions = backend.read_joint_positions() + >>> from dimos.hardware.manipulators.xarm import XArmAdapter + >>> adapter = XArmAdapter(ip="192.168.1.185", dof=6) + >>> adapter.connect() + >>> positions = adapter.read_joint_positions() """ -from dimos.hardware.manipulators.xarm.backend import XArmBackend +from dimos.hardware.manipulators.xarm.adapter import XArmAdapter -__all__ = ["XArmBackend"] +__all__ = ["XArmAdapter"] diff --git a/dimos/hardware/manipulators/xarm/backend.py b/dimos/hardware/manipulators/xarm/adapter.py similarity index 83% rename from dimos/hardware/manipulators/xarm/backend.py rename to dimos/hardware/manipulators/xarm/adapter.py index 9adcdca24f..56b9d0357c 100644 --- a/dimos/hardware/manipulators/xarm/backend.py +++ b/dimos/hardware/manipulators/xarm/adapter.py @@ -12,22 +12,34 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""XArm backend - implements ManipulatorBackend protocol. +"""XArm adapter - implements ManipulatorAdapter protocol. -Handles all XArm SDK communication and unit conversion. +SDK Units: angles=degrees, distance=mm, velocity=deg/s +DimOS Units: angles=radians, distance=meters, velocity=rad/s """ +from __future__ import annotations + import math +from typing import TYPE_CHECKING from xarm.wrapper import XArmAPI +if TYPE_CHECKING: + from dimos.hardware.manipulators.registry import AdapterRegistry + from dimos.hardware.manipulators.spec import ( ControlMode, JointLimits, - ManipulatorBackend, + ManipulatorAdapter, ManipulatorInfo, ) +# Unit conversion constants +MM_TO_M = 0.001 +M_TO_MM = 1000.0 +MAX_CARTESIAN_SPEED_MM = 500.0 # Max cartesian speed in mm/s + # XArm mode codes _XARM_MODE_POSITION = 0 _XARM_MODE_SERVO_CARTESIAN = 1 @@ -36,48 +48,17 @@ _XARM_MODE_JOINT_TORQUE = 6 -class XArmBackend(ManipulatorBackend): - """XArm-specific backend. +class XArmAdapter(ManipulatorAdapter): + """XArm-specific adapter. - Implements ManipulatorBackend protocol via duck typing. + Implements ManipulatorAdapter protocol via duck typing. No inheritance required - just matching method signatures. - - Unit conversions: - - Angles: XArm uses degrees, we use radians - - Positions: XArm uses mm, we use meters - - Velocities: XArm uses deg/s, we use rad/s - - TODO: Consider creating XArmPose/XArmVelocity types to encapsulate - unit conversions instead of helper methods. See ManipulatorPose discussion. """ - # ========================================================================= - # Unit Conversions (SI <-> XArm units) - # ========================================================================= - - @staticmethod - def _m_to_mm(m: float) -> float: - return m * 1000.0 - - @staticmethod - def _mm_to_m(mm: float) -> float: - return mm / 1000.0 - - @staticmethod - def _rad_to_deg(rad: float) -> float: - return math.degrees(rad) - - @staticmethod - def _deg_to_rad(deg: float) -> float: - return math.radians(deg) - - @staticmethod - def _velocity_to_speed_mm(velocity: float) -> float: - """Convert 0-1 velocity fraction to mm/s (max ~500 mm/s).""" - return velocity * 500 - - def __init__(self, ip: str, dof: int = 6) -> None: - self._ip = ip + def __init__(self, address: str, dof: int = 6, **_: object) -> None: + if not address: + raise ValueError("address (IP) is required for XArmAdapter") + self._ip = address self._dof = dof self._arm: XArmAPI | None = None self._control_mode: ControlMode = ControlMode.POSITION @@ -319,12 +300,12 @@ def read_cartesian_position(self) -> dict[str, float] | None: _, pose = self._arm.get_position() if pose and len(pose) >= 6: return { - "x": self._mm_to_m(pose[0]), - "y": self._mm_to_m(pose[1]), - "z": self._mm_to_m(pose[2]), - "roll": self._deg_to_rad(pose[3]), - "pitch": self._deg_to_rad(pose[4]), - "yaw": self._deg_to_rad(pose[5]), + "x": pose[0] * MM_TO_M, + "y": pose[1] * MM_TO_M, + "z": pose[2] * MM_TO_M, + "roll": math.radians(pose[3]), + "pitch": math.radians(pose[4]), + "yaw": math.radians(pose[5]), } return None @@ -338,13 +319,13 @@ def write_cartesian_position( return False code: int = self._arm.set_position( - x=self._m_to_mm(pose.get("x", 0)), - y=self._m_to_mm(pose.get("y", 0)), - z=self._m_to_mm(pose.get("z", 0)), - roll=self._rad_to_deg(pose.get("roll", 0)), - pitch=self._rad_to_deg(pose.get("pitch", 0)), - yaw=self._rad_to_deg(pose.get("yaw", 0)), - speed=self._velocity_to_speed_mm(velocity), + x=pose.get("x", 0) * M_TO_MM, + y=pose.get("y", 0) * M_TO_MM, + z=pose.get("z", 0) * M_TO_MM, + roll=math.degrees(pose.get("roll", 0)), + pitch=math.degrees(pose.get("pitch", 0)), + yaw=math.degrees(pose.get("yaw", 0)), + speed=velocity * MAX_CARTESIAN_SPEED_MM, wait=False, ) return code == 0 @@ -362,7 +343,7 @@ def read_gripper_position(self) -> float | None: code: int = result[0] pos: float | None = result[1] if code == 0 and pos is not None: - return pos / 1000.0 # mm -> m + return pos * MM_TO_M return None def write_gripper_position(self, position: float) -> bool: @@ -370,7 +351,7 @@ def write_gripper_position(self, position: float) -> bool: if not self._arm: return False - pos_mm = position * 1000.0 # m -> mm + pos_mm = position * M_TO_MM code: int = self._arm.set_gripper_position(pos_mm) return code == 0 @@ -389,4 +370,9 @@ def read_force_torque(self) -> list[float] | None: return None -__all__ = ["XArmBackend"] +def register(registry: AdapterRegistry) -> None: + """Register this adapter with the registry.""" + registry.register("xarm", XArmAdapter) + + +__all__ = ["XArmAdapter"]