diff --git a/dimos/control/README.md b/dimos/control/README.md
new file mode 100644
index 0000000000..58490321fa
--- /dev/null
+++ b/dimos/control/README.md
@@ -0,0 +1,195 @@
+# Control Orchestrator
+
+Centralized control system for multi-arm robots with per-joint arbitration.
+
+## Architecture
+
+```
+┌─────────────────────────────────────────────────────────────┐
+│                   ControlOrchestrator                       │
+│                                                             │
+│  ┌──────────────────────────────────────────────────────┐   │
+│  │                    TickLoop (100Hz)                  │   │
+│  │                                                      │   │
+│  │   READ ──► COMPUTE ──► ARBITRATE ──► ROUTE ──► WRITE │   │
+│  └──────────────────────────────────────────────────────┘   │
+│         │           │           │              │            │
+│         ▼           ▼           ▼              ▼            │
+│    ┌─────────┐  ┌───────┐  ┌─────────┐   ┌──────────┐       │
+│    │Hardware │  │ Tasks │  │Priority │   │ Backends │       │
+│    │Interface│  │       │  │ Winners │   │          │       │
+│    └─────────┘  └───────┘  └─────────┘   └──────────┘       │
+└─────────────────────────────────────────────────────────────┘
+```
+
+## 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 2: Control via CLI
+python -m dimos.manipulation.control.orchestrator_client
+```
+
+## Core Concepts
+
+### Tick Loop
+Single deterministic loop at 100Hz:
+1. **Read** - Get joint positions from all hardware
+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
+
+### Tasks (Controllers)
+Tasks are passive controllers called by the orchestrator:
+
+```python
+class MyController:
+    def claim(self) -> ResourceClaim:
+        return ResourceClaim(joints={"joint1", "joint2"}, priority=10)
+
+    def compute(self, state: OrchestratorState) -> JointCommandOutput:
+        # Your control law here (PID, impedance, etc.)
+        return JointCommandOutput(
+            joint_names=["joint1", "joint2"],
+            positions=[0.5, 0.3],
+            mode=ControlMode.POSITION,
+        )
+```
+
+### Priority & Arbitration
+Higher priority always wins. Arbitration happens every tick:
+
+```
+traj_arm (priority=10) wants joint1 = 0.5
+safety   (priority=100) wants joint1 = 0.0
+                              ↓
+                    safety wins, traj_arm preempted
+```
+
+### Preemption
+When a task loses a joint to higher priority, it gets notified:
+
+```python
+def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
+    self._state = TrajectoryState.PREEMPTED
+```
+
+## Files
+
+```
+dimos/control/
+├── orchestrator.py      # Module + RPC interface
+├── tick_loop.py         # 100Hz control loop
+├── task.py              # ControlTask protocol + types
+├── hardware_interface.py # Backend wrapper
+├── blueprints.py        # Pre-configured setups
+└── tasks/
+    └── trajectory_task.py  # Joint trajectory controller
+```
+
+## Configuration
+
+```python
+from dimos.control import control_orchestrator, HardwareConfig, TaskConfig
+
+my_robot = control_orchestrator(
+    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"),
+    ],
+    tasks=[
+        TaskConfig(name="traj_left", type="trajectory", joint_names=[...], priority=10),
+        TaskConfig(name="traj_right", type="trajectory", joint_names=[...], priority=10),
+        TaskConfig(name="safety", type="trajectory", joint_names=[...], priority=100),
+    ],
+)
+```
+
+## RPC Methods
+
+| Method | Description |
+|--------|-------------|
+| `list_hardware()` | List hardware IDs |
+| `list_joints()` | List all joint names |
+| `list_tasks()` | List task names |
+| `get_joint_positions()` | Get current positions |
+| `execute_trajectory(task, traj)` | Execute trajectory |
+| `get_trajectory_status(task)` | Get task status |
+| `cancel_trajectory(task)` | Cancel active trajectory |
+
+## Control Modes
+
+Tasks output commands in one of three modes:
+
+| Mode | Output | Use Case |
+|------|--------|----------|
+| POSITION | `q` | Trajectory following |
+| VELOCITY | `q_dot` | Joystick teleoperation |
+| TORQUE | `tau` | Force control, impedance |
+
+## Writing a Custom Task
+
+```python
+from dimos.control.task import ControlTask, ResourceClaim, JointCommandOutput, ControlMode
+
+class PIDController:
+    def __init__(self, joints: list[str], priority: int = 10):
+        self._name = "pid_controller"
+        self._claim = ResourceClaim(joints=frozenset(joints), priority=priority)
+        self._joints = joints
+        self.Kp, self.Ki, self.Kd = 10.0, 0.1, 1.0
+        self._integral = [0.0] * len(joints)
+        self._last_error = [0.0] * len(joints)
+        self.target = [0.0] * len(joints)
+
+    @property
+    def name(self) -> str:
+        return self._name
+
+    def claim(self) -> ResourceClaim:
+        return self._claim
+
+    def is_active(self) -> bool:
+        return True
+
+    def compute(self, state) -> JointCommandOutput:
+        positions = [state.joints.joint_positions[j] for j in self._joints]
+        error = [t - p for t, p in zip(self.target, positions)]
+
+        # PID
+        self._integral = [i + e * state.dt for i, e in zip(self._integral, error)]
+        derivative = [(e - le) / state.dt for e, le in zip(error, self._last_error)]
+        output = [self.Kp*e + self.Ki*i + self.Kd*d
+                  for e, i, d in zip(error, self._integral, derivative)]
+        self._last_error = error
+
+        return JointCommandOutput(
+            joint_names=self._joints,
+            positions=output,
+            mode=ControlMode.POSITION,
+        )
+
+    def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
+        pass  # Handle preemption
+```
+
+## Joint State Output
+
+The orchestrator publishes one aggregated `JointState` message containing all joints:
+
+```python
+JointState(
+    name=["left_joint1", ..., "right_joint1", ...],  # All joints
+    position=[...],
+    velocity=[...],
+    effort=[...],
+)
+```
+
+Subscribe via: `/orchestrator/joint_state`
diff --git a/dimos/control/__init__.py b/dimos/control/__init__.py
new file mode 100644
index 0000000000..3d7d647cd4
--- /dev/null
+++ b/dimos/control/__init__.py
@@ -0,0 +1,92 @@
+# 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.
+
+"""ControlOrchestrator - Centralized control for multi-arm coordination.
+
+This module provides a centralized control orchestrator that replaces
+per-driver/per-controller loops with a single deterministic tick-based system.
+
+Features:
+- Single tick loop (read → compute → arbitrate → route → write)
+- Per-joint arbitration (highest priority wins)
+- Mode conflict detection
+- Partial command support (hold last value)
+- Aggregated preemption notifications
+
+Example:
+    >>> from dimos.control import ControlOrchestrator
+    >>> from dimos.control.tasks import JointTrajectoryTask, JointTrajectoryTaskConfig
+    >>> from dimos.hardware.manipulators.xarm import XArmBackend
+    >>>
+    >>> # Create orchestrator
+    >>> orch = ControlOrchestrator(tick_rate=100.0)
+    >>>
+    >>> # Add hardware
+    >>> backend = XArmBackend(ip="192.168.1.185", dof=7)
+    >>> backend.connect()
+    >>> orch.add_hardware("left_arm", backend, joint_prefix="left")
+    >>>
+    >>> # Add task
+    >>> joints = [f"left_joint{i+1}" for i in range(7)]
+    >>> task = JointTrajectoryTask(
+    ...     "traj_left",
+    ...     JointTrajectoryTaskConfig(joint_names=joints, priority=10),
+    ... )
+    >>> orch.add_task(task)
+    >>>
+    >>> # Start
+    >>> orch.start()
+"""
+
+from dimos.control.hardware_interface import (
+    BackendHardwareInterface,
+    HardwareInterface,
+)
+from dimos.control.orchestrator import (
+    ControlOrchestrator,
+    ControlOrchestratorConfig,
+    HardwareConfig,
+    TaskConfig,
+    control_orchestrator,
+)
+from dimos.control.task import (
+    ControlMode,
+    ControlTask,
+    JointCommandOutput,
+    JointStateSnapshot,
+    OrchestratorState,
+    ResourceClaim,
+)
+from dimos.control.tick_loop import TickLoop
+
+__all__ = [
+    # Hardware interface
+    "BackendHardwareInterface",
+    "ControlMode",
+    # Orchestrator
+    "ControlOrchestrator",
+    "ControlOrchestratorConfig",
+    # Task protocol and types
+    "ControlTask",
+    "HardwareConfig",
+    "HardwareInterface",
+    "JointCommandOutput",
+    "JointStateSnapshot",
+    "OrchestratorState",
+    "ResourceClaim",
+    "TaskConfig",
+    # Tick loop
+    "TickLoop",
+    "control_orchestrator",
+]
diff --git a/dimos/control/blueprints.py b/dimos/control/blueprints.py
new file mode 100644
index 0000000000..5cbc109ece
--- /dev/null
+++ b/dimos/control/blueprints.py
@@ -0,0 +1,367 @@
+# 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.
+
+"""Pre-configured blueprints for the ControlOrchestrator.
+
+This module provides ready-to-use orchestrator blueprints for common setups.
+
+Usage:
+    # Run via CLI:
+    dimos run orchestrator-mock           # Mock 7-DOF arm
+    dimos run orchestrator-xarm7          # XArm7 real hardware
+    dimos run orchestrator-dual-mock      # Dual mock arms
+
+    # Or programmatically:
+    from dimos.control.blueprints import orchestrator_mock
+    coordinator = orchestrator_mock.build()
+    coordinator.loop()
+
+Example with trajectory setter:
+    # Terminal 1: Run the orchestrator
+    dimos run orchestrator-mock
+
+    # Terminal 2: Send trajectories via RPC
+    python -m dimos.control.examples.orchestrator_trajectory_setter --task traj_arm
+"""
+
+from __future__ import annotations
+
+from dimos.control.orchestrator import (
+    ControlOrchestrator,
+    HardwareConfig,
+    TaskConfig,
+    control_orchestrator,
+)
+from dimos.core.transport import LCMTransport
+from dimos.msgs.sensor_msgs import JointState
+
+# =============================================================================
+# Helper function to generate joint names
+# =============================================================================
+
+
+def _joint_names(prefix: str, dof: int) -> list[str]:
+    """Generate joint names with prefix."""
+    return [f"{prefix}_joint{i + 1}" for i in range(dof)]
+
+
+# =============================================================================
+# Single Arm Blueprints
+# =============================================================================
+
+# Mock 7-DOF arm (for testing)
+orchestrator_mock = control_orchestrator(
+    tick_rate=100.0,
+    publish_joint_state=True,
+    joint_state_frame_id="orchestrator",
+    hardware=[
+        HardwareConfig(
+            id="arm",
+            type="mock",
+            dof=7,
+            joint_prefix="arm",
+        ),
+    ],
+    tasks=[
+        TaskConfig(
+            name="traj_arm",
+            type="trajectory",
+            joint_names=_joint_names("arm", 7),
+            priority=10,
+        ),
+    ],
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState),
+    }
+)
+
+# XArm7 real hardware (requires IP configuration)
+orchestrator_xarm7 = control_orchestrator(
+    tick_rate=100.0,
+    publish_joint_state=True,
+    joint_state_frame_id="orchestrator",
+    hardware=[
+        HardwareConfig(
+            id="arm",
+            type="xarm",
+            dof=7,
+            joint_prefix="arm",
+            ip="192.168.2.235",  # Default IP, override via env or config
+            auto_enable=True,
+        ),
+    ],
+    tasks=[
+        TaskConfig(
+            name="traj_arm",
+            type="trajectory",
+            joint_names=_joint_names("arm", 7),
+            priority=10,
+        ),
+    ],
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState),
+    }
+)
+
+# XArm6 real hardware
+orchestrator_xarm6 = control_orchestrator(
+    tick_rate=100.0,
+    publish_joint_state=True,
+    joint_state_frame_id="orchestrator",
+    hardware=[
+        HardwareConfig(
+            id="arm",
+            type="xarm",
+            dof=6,
+            joint_prefix="arm",
+            ip="192.168.1.210",
+            auto_enable=True,
+        ),
+    ],
+    tasks=[
+        TaskConfig(
+            name="traj_xarm",
+            type="trajectory",
+            joint_names=_joint_names("arm", 6),
+            priority=10,
+        ),
+    ],
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState),
+    }
+)
+
+# Piper arm (6-DOF, CAN bus)
+orchestrator_piper = control_orchestrator(
+    tick_rate=100.0,
+    publish_joint_state=True,
+    joint_state_frame_id="orchestrator",
+    hardware=[
+        HardwareConfig(
+            id="arm",
+            type="piper",
+            dof=6,
+            joint_prefix="arm",
+            can_port="can0",
+            auto_enable=True,
+        ),
+    ],
+    tasks=[
+        TaskConfig(
+            name="traj_piper",
+            type="trajectory",
+            joint_names=_joint_names("arm", 6),
+            priority=10,
+        ),
+    ],
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState),
+    }
+)
+
+# =============================================================================
+# Dual Arm Blueprints
+# =============================================================================
+
+# Dual mock arms (7-DOF left, 6-DOF right) for testing
+orchestrator_dual_mock = control_orchestrator(
+    tick_rate=100.0,
+    publish_joint_state=True,
+    joint_state_frame_id="orchestrator",
+    hardware=[
+        HardwareConfig(
+            id="left_arm",
+            type="mock",
+            dof=7,
+            joint_prefix="left",
+        ),
+        HardwareConfig(
+            id="right_arm",
+            type="mock",
+            dof=6,
+            joint_prefix="right",
+        ),
+    ],
+    tasks=[
+        TaskConfig(
+            name="traj_left",
+            type="trajectory",
+            joint_names=_joint_names("left", 7),
+            priority=10,
+        ),
+        TaskConfig(
+            name="traj_right",
+            type="trajectory",
+            joint_names=_joint_names("right", 6),
+            priority=10,
+        ),
+    ],
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState),
+    }
+)
+
+# Dual XArm setup (XArm7 left, XArm6 right)
+orchestrator_dual_xarm = control_orchestrator(
+    tick_rate=100.0,
+    publish_joint_state=True,
+    joint_state_frame_id="orchestrator",
+    hardware=[
+        HardwareConfig(
+            id="left_arm",
+            type="xarm",
+            dof=7,
+            joint_prefix="left",
+            ip="192.168.2.235",
+            auto_enable=True,
+        ),
+        HardwareConfig(
+            id="right_arm",
+            type="xarm",
+            dof=6,
+            joint_prefix="right",
+            ip="192.168.1.210",
+            auto_enable=True,
+        ),
+    ],
+    tasks=[
+        TaskConfig(
+            name="traj_left",
+            type="trajectory",
+            joint_names=_joint_names("left", 7),
+            priority=10,
+        ),
+        TaskConfig(
+            name="traj_right",
+            type="trajectory",
+            joint_names=_joint_names("right", 6),
+            priority=10,
+        ),
+    ],
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState),
+    }
+)
+
+# Dual Arm setup (XArm6 , Piper )
+orchestrator_piper_xarm = control_orchestrator(
+    tick_rate=100.0,
+    publish_joint_state=True,
+    joint_state_frame_id="orchestrator",
+    hardware=[
+        HardwareConfig(
+            id="xarm_arm",
+            type="xarm",
+            dof=6,
+            joint_prefix="xarm",
+            ip="192.168.1.210",
+            auto_enable=True,
+        ),
+        HardwareConfig(
+            id="piper_arm",
+            type="piper",
+            dof=6,
+            joint_prefix="piper",
+            can_port="can0",
+            auto_enable=True,
+        ),
+    ],
+    tasks=[
+        TaskConfig(
+            name="traj_xarm",
+            type="trajectory",
+            joint_names=_joint_names("xarm", 6),
+            priority=10,
+        ),
+        TaskConfig(
+            name="traj_piper",
+            type="trajectory",
+            joint_names=_joint_names("piper", 6),
+            priority=10,
+        ),
+    ],
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState),
+    }
+)
+
+# =============================================================================
+# High-frequency Blueprints (200Hz)
+# =============================================================================
+
+# High-frequency mock for demanding applications
+orchestrator_highfreq_mock = control_orchestrator(
+    tick_rate=200.0,
+    publish_joint_state=True,
+    joint_state_frame_id="orchestrator",
+    hardware=[
+        HardwareConfig(
+            id="arm",
+            type="mock",
+            dof=7,
+            joint_prefix="arm",
+        ),
+    ],
+    tasks=[
+        TaskConfig(
+            name="traj_arm",
+            type="trajectory",
+            joint_names=_joint_names("arm", 7),
+            priority=10,
+        ),
+    ],
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState),
+    }
+)
+
+# =============================================================================
+# Raw Blueprints (no hardware/tasks configured - for programmatic setup)
+# =============================================================================
+
+# Basic orchestrator with transport only (add hardware/tasks programmatically)
+orchestrator_basic = control_orchestrator(
+    tick_rate=100.0,
+    publish_joint_state=True,
+    joint_state_frame_id="orchestrator",
+).transports(
+    {
+        ("joint_state", JointState): LCMTransport("/orchestrator/joint_state", JointState),
+    }
+)
+
+
+__all__ = [
+    # Raw blueprints (for programmatic setup)
+    "orchestrator_basic",
+    # Dual arm blueprints
+    "orchestrator_dual_mock",
+    "orchestrator_dual_xarm",
+    # High-frequency blueprints
+    "orchestrator_highfreq_mock",
+    # Single arm blueprints
+    "orchestrator_mock",
+    "orchestrator_piper",
+    "orchestrator_piper_xarm",
+    "orchestrator_xarm6",
+    "orchestrator_xarm7",
+]
diff --git a/dimos/control/hardware_interface.py b/dimos/control/hardware_interface.py
new file mode 100644
index 0000000000..e4fba70e1c
--- /dev/null
+++ b/dimos/control/hardware_interface.py
@@ -0,0 +1,230 @@
+# 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.
+
+"""Hardware interface for the ControlOrchestrator.
+
+Wraps ManipulatorBackend with orchestrator-specific features:
+- Namespaced joint names (e.g., "left_joint1")
+- Unified read/write interface
+- Hold-last-value for partial commands
+"""
+
+from __future__ import annotations
+
+import logging
+import time
+from typing import TYPE_CHECKING, Protocol, runtime_checkable
+
+from dimos.hardware.manipulators.spec import ControlMode, ManipulatorBackend
+
+logger = logging.getLogger(__name__)
+
+
+@runtime_checkable
+class HardwareInterface(Protocol):
+    """Protocol for hardware that the orchestrator can control.
+
+    This wraps ManipulatorBackend with orchestrator-specific features:
+    - Namespaced joint names (e.g., "left_arm_joint1")
+    - Unified read/write interface
+    - State caching
+    """
+
+    @property
+    def hardware_id(self) -> str:
+        """Unique ID for this hardware (e.g., 'left_arm')."""
+        ...
+
+    @property
+    def joint_names(self) -> list[str]:
+        """Ordered list of fully-qualified joint names this hardware controls."""
+        ...
+
+    def read_state(self) -> dict[str, tuple[float, float, float]]:
+        """Read current state.
+
+        Returns:
+            Dict of joint_name -> (position, velocity, effort)
+        """
+        ...
+
+    def write_command(self, commands: dict[str, 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.
+
+    Features:
+    - Generates namespaced joint names (prefix_joint1, prefix_joint2, ...)
+    - Holds last commanded value for partial commands
+    - On first tick, reads current position from hardware for missing joints
+    """
+
+    def __init__(
+        self,
+        backend: ManipulatorBackend,
+        hardware_id: str,
+        joint_prefix: str | None = None,
+    ) -> None:
+        """Initialize hardware interface.
+
+        Args:
+            backend: ManipulatorBackend instance (XArmBackend, PiperBackend, etc.)
+            hardware_id: Unique identifier for this hardware
+            joint_prefix: Prefix for joint names (defaults to hardware_id)
+        """
+        if not isinstance(backend, ManipulatorBackend):
+            raise TypeError("backend must implement ManipulatorBackend")
+
+        self._backend = backend
+        self._hardware_id = hardware_id
+        self._prefix = joint_prefix or hardware_id
+        self._dof = backend.get_dof()
+
+        # Generate joint names: prefix_joint1, prefix_joint2, ...
+        self._joint_names = [f"{self._prefix}_joint{i + 1}" for i in range(self._dof)]
+
+        # Track last commanded values for hold-last behavior
+        self._last_commanded: dict[str, float] = {}
+        self._initialized = False
+        self._warned_unknown_joints: set[str] = set()
+        self._current_mode: ControlMode | None = None
+
+    @property
+    def hardware_id(self) -> str:
+        """Unique ID for this hardware."""
+        return self._hardware_id
+
+    @property
+    def joint_names(self) -> list[str]:
+        """Ordered list of joint names."""
+        return self._joint_names
+
+    @property
+    def dof(self) -> int:
+        """Degrees of freedom."""
+        return self._dof
+
+    def disconnect(self) -> None:
+        """Disconnect the underlying backend."""
+        self._backend.disconnect()
+
+    def read_state(self) -> dict[str, tuple[float, float, float]]:
+        """Read state as {joint_name: (position, velocity, effort)}.
+
+        Returns:
+            Dict mapping joint name to (position, velocity, effort) tuple
+        """
+        positions = self._backend.read_joint_positions()
+        velocities = self._backend.read_joint_velocities()
+        efforts = self._backend.read_joint_efforts()
+
+        return {
+            name: (positions[i], velocities[i], efforts[i])
+            for i, name in enumerate(self._joint_names)
+        }
+
+    def write_command(self, commands: dict[str, float], mode: ControlMode) -> bool:
+        """Write commands - allows partial joint sets, holds last for missing.
+
+        This is critical for:
+        - Partial WBC overrides
+        - Safety controllers
+        - Mixed task ownership
+
+        Args:
+            commands: {joint_name: value} - can be partial
+            mode: Control mode
+
+        Returns:
+            True if command was sent successfully
+        """
+        # Initialize on first write if needed
+        if not self._initialized:
+            self._initialize_last_commanded()
+
+        # Update last commanded for joints we received
+        for joint_name, value in commands.items():
+            if joint_name in self._joint_names:
+                self._last_commanded[joint_name] = value
+            elif joint_name not in self._warned_unknown_joints:
+                logger.warning(
+                    f"Hardware {self._hardware_id} received command for unknown joint "
+                    f"{joint_name}. Valid joints: {self._joint_names}"
+                )
+                self._warned_unknown_joints.add(joint_name)
+
+        # Build ordered list for backend
+        ordered = self._build_ordered_command()
+
+        # Switch control mode if needed
+        if mode != self._current_mode:
+            if not self._backend.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
+        match mode:
+            case ControlMode.POSITION | ControlMode.SERVO_POSITION:
+                return self._backend.write_joint_positions(ordered)
+            case ControlMode.VELOCITY:
+                return self._backend.write_joint_velocities(ordered)
+            case ControlMode.TORQUE:
+                logger.warning(f"Hardware {self._hardware_id} does not support torque mode")
+                return False
+            case _:
+                return False
+
+    def _initialize_last_commanded(self) -> None:
+        """Initialize last_commanded with current hardware positions."""
+        for _ in range(10):
+            try:
+                current = self._backend.read_joint_positions()
+                for i, name in enumerate(self._joint_names):
+                    self._last_commanded[name] = current[i]
+                self._initialized = True
+                return
+            except Exception:
+                time.sleep(0.01)
+
+        raise RuntimeError(
+            f"Hardware {self._hardware_id} failed to read initial positions after retries"
+        )
+
+    def _build_ordered_command(self) -> list[float]:
+        """Build ordered command list from last_commanded dict."""
+        return [self._last_commanded[name] for name in self._joint_names]
+
+
+__all__ = [
+    "BackendHardwareInterface",
+    "HardwareInterface",
+]
diff --git a/dimos/control/orchestrator.py b/dimos/control/orchestrator.py
new file mode 100644
index 0000000000..e7ea147dcf
--- /dev/null
+++ b/dimos/control/orchestrator.py
@@ -0,0 +1,539 @@
+# 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.
+
+"""ControlOrchestrator module.
+
+Centralized control orchestrator that replaces per-driver/per-controller
+loops with a single deterministic tick-based system.
+
+Features:
+- Single tick loop (read → compute → arbitrate → route → write)
+- Per-joint arbitration (highest priority wins)
+- Mode conflict detection
+- Partial command support (hold last value)
+- Aggregated preemption notifications
+"""
+
+from __future__ import annotations
+
+from collections.abc import Callable
+from dataclasses import dataclass, field
+import threading
+import time
+from typing import TYPE_CHECKING, Any
+
+from dimos.control.hardware_interface import BackendHardwareInterface, HardwareInterface
+from dimos.control.task import ControlTask
+from dimos.control.tick_loop import TickLoop
+from dimos.core import Module, Out, rpc
+from dimos.core.module import ModuleConfig
+from dimos.msgs.sensor_msgs import (
+    JointState,  # noqa: TC001 - needed at runtime for Out[JointState]
+)
+from dimos.msgs.trajectory_msgs import JointTrajectory, TrajectoryState
+from dimos.utils.logging_config import setup_logger
+
+if TYPE_CHECKING:
+    from dimos.hardware.manipulators.spec import ManipulatorBackend
+
+logger = setup_logger()
+
+
+# =============================================================================
+# Configuration
+# =============================================================================
+
+
+@dataclass
+class HardwareConfig:
+    """Configuration for a hardware backend.
+
+    Attributes:
+        id: Unique hardware identifier (e.g., "arm", "left_arm")
+        type: Backend type ("mock", "xarm", "piper")
+        dof: Degrees of freedom
+        joint_prefix: Prefix for joint names (defaults to id)
+        ip: IP address (required for xarm)
+        can_port: CAN port (for piper, default "can0")
+        auto_enable: Whether to auto-enable servos (default True)
+    """
+
+    id: str
+    type: str = "mock"
+    dof: int = 7
+    joint_prefix: str | None = None
+    ip: str | None = None
+    can_port: str | None = None
+    auto_enable: bool = True
+
+
+@dataclass
+class TaskConfig:
+    """Configuration for a control task.
+
+    Attributes:
+        name: Task name (e.g., "traj_arm")
+        type: Task type ("trajectory")
+        joint_names: List of joint names this task controls
+        priority: Task priority (higher wins arbitration)
+    """
+
+    name: str
+    type: str = "trajectory"
+    joint_names: list[str] = field(default_factory=lambda: [])
+    priority: int = 10
+
+
+@dataclass
+class TaskStatus:
+    """Status of a control task.
+
+    Attributes:
+        active: Whether the task is currently active
+        state: Task state name (e.g., "IDLE", "RUNNING", "DONE")
+        progress: Task progress from 0.0 to 1.0
+    """
+
+    active: bool
+    state: str | None = None
+    progress: float | None = None
+
+
+@dataclass
+class ControlOrchestratorConfig(ModuleConfig):
+    """Configuration for the ControlOrchestrator.
+
+    Attributes:
+        tick_rate: Control loop frequency in Hz (default: 100)
+        publish_joint_state: Whether to publish aggregated JointState
+        joint_state_frame_id: Frame ID for published JointState
+        log_ticks: Whether to log tick information (verbose)
+        hardware: List of hardware configurations to create on start
+        tasks: List of task configurations to create on start
+    """
+
+    tick_rate: float = 100.0
+    publish_joint_state: bool = True
+    joint_state_frame_id: str = "orchestrator"
+    log_ticks: bool = False
+    hardware: list[HardwareConfig] = field(default_factory=lambda: [])
+    tasks: list[TaskConfig] = field(default_factory=lambda: [])
+
+
+# =============================================================================
+# ControlOrchestrator Module
+# =============================================================================
+
+
+class ControlOrchestrator(Module[ControlOrchestratorConfig]):
+    """Centralized control orchestrator with per-joint arbitration.
+
+    Single tick loop that:
+    1. Reads state from all hardware
+    2. Runs all active tasks
+    3. Arbitrates conflicts per-joint (highest priority wins)
+    4. Routes commands to hardware
+    5. Publishes aggregated joint state
+
+    Key design decisions:
+    - Joint-centric commands (not hardware-centric)
+    - Per-joint arbitration (not per-hardware)
+    - Centralized time (tasks use state.t_now, never time.time())
+    - Partial commands OK (hardware holds last value)
+    - Aggregated preemption (one notification per task per tick)
+
+    Example:
+        >>> from dimos.control import ControlOrchestrator
+        >>> from dimos.hardware.manipulators.xarm import XArmBackend
+        >>>
+        >>> orch = ControlOrchestrator(tick_rate=100.0)
+        >>> backend = XArmBackend(ip="192.168.1.185", dof=7)
+        >>> backend.connect()
+        >>> orch.add_hardware("left_arm", backend, joint_prefix="left")
+        >>> orch.start()
+    """
+
+    # Output: Aggregated joint state for external consumers
+    joint_state: Out[JointState]
+
+    config: ControlOrchestratorConfig
+    default_config = ControlOrchestratorConfig
+
+    def __init__(self, *args: Any, **kwargs: Any) -> None:
+        super().__init__(*args, **kwargs)
+
+        # Hardware interfaces (keyed by hardware_id)
+        self._hardware: dict[str, HardwareInterface] = {}
+        self._hardware_lock = threading.Lock()
+
+        # Joint -> hardware mapping (built when hardware added)
+        self._joint_to_hardware: dict[str, str] = {}
+
+        # Registered tasks
+        self._tasks: dict[str, ControlTask] = {}
+        self._task_lock = threading.Lock()
+
+        # Tick loop (created on start)
+        self._tick_loop: TickLoop | None = None
+
+        logger.info(f"ControlOrchestrator initialized at {self.config.tick_rate}Hz")
+
+    # =========================================================================
+    # Config-based Setup
+    # =========================================================================
+
+    def _setup_from_config(self) -> None:
+        """Create hardware and tasks from config (called on start)."""
+        hardware_added: list[str] = []
+
+        try:
+            for hw_cfg in self.config.hardware:
+                self._setup_hardware(hw_cfg)
+                hardware_added.append(hw_cfg.id)
+
+            for task_cfg in self.config.tasks:
+                task = self._create_task_from_config(task_cfg)
+                self.add_task(task)
+
+        except Exception:
+            # Rollback: clean up all successfully added hardware
+            for hw_id in hardware_added:
+                try:
+                    self.remove_hardware(hw_id)
+                except Exception:
+                    pass
+            raise
+
+    def _setup_hardware(self, hw_cfg: HardwareConfig) -> None:
+        """Connect and add a single hardware backend."""
+        backend = self._create_backend_from_config(hw_cfg)
+
+        if not backend.connect():
+            raise RuntimeError(f"Failed to connect to {hw_cfg.type} backend")
+
+        try:
+            if hw_cfg.auto_enable and hasattr(backend, "write_enable"):
+                backend.write_enable(True)
+            self.add_hardware(
+                hw_cfg.id,
+                backend,
+                joint_prefix=hw_cfg.joint_prefix or hw_cfg.id,
+            )
+        except Exception:
+            backend.disconnect()
+            raise
+
+    def _create_backend_from_config(self, cfg: HardwareConfig) -> ManipulatorBackend:
+        """Create a manipulator backend from config."""
+        match cfg.type.lower():
+            case "mock":
+                from dimos.hardware.manipulators.mock import MockBackend
+
+                return MockBackend(dof=cfg.dof)
+            case "xarm":
+                if cfg.ip is None:
+                    raise ValueError("ip is required for xarm backend")
+                from dimos.hardware.manipulators.xarm import XArmBackend
+
+                return XArmBackend(ip=cfg.ip, dof=cfg.dof)
+            case "piper":
+                from dimos.hardware.manipulators.piper import PiperBackend
+
+                return PiperBackend(can_port=cfg.can_port or "can0", dof=cfg.dof)
+            case _:
+                raise ValueError(f"Unknown backend type: {cfg.type}")
+
+    def _create_task_from_config(self, cfg: TaskConfig) -> ControlTask:
+        """Create a control task from config."""
+        task_type = cfg.type.lower()
+
+        if task_type == "trajectory":
+            from dimos.control.tasks import JointTrajectoryTask, JointTrajectoryTaskConfig
+
+            return JointTrajectoryTask(
+                cfg.name,
+                JointTrajectoryTaskConfig(
+                    joint_names=cfg.joint_names,
+                    priority=cfg.priority,
+                ),
+            )
+
+        else:
+            raise ValueError(f"Unknown task type: {task_type}")
+
+    # =========================================================================
+    # Hardware Management (RPC)
+    # =========================================================================
+
+    @rpc
+    def add_hardware(
+        self,
+        hardware_id: str,
+        backend: ManipulatorBackend,
+        joint_prefix: str | None = None,
+    ) -> bool:
+        """Register a hardware backend with the orchestrator."""
+        with self._hardware_lock:
+            if hardware_id in self._hardware:
+                logger.warning(f"Hardware {hardware_id} already registered")
+                return False
+
+            interface = BackendHardwareInterface(
+                backend=backend,
+                hardware_id=hardware_id,
+                joint_prefix=joint_prefix,
+            )
+            self._hardware[hardware_id] = interface
+
+            for joint_name in interface.joint_names:
+                self._joint_to_hardware[joint_name] = hardware_id
+
+            logger.info(f"Added hardware {hardware_id} with joints: {interface.joint_names}")
+            return True
+
+    @rpc
+    def remove_hardware(self, hardware_id: str) -> bool:
+        """Remove a hardware interface.
+
+        Note: For safety, call this only when no tasks are actively using this
+        hardware. Consider stopping the orchestrator before removing hardware.
+        """
+        with self._hardware_lock:
+            if hardware_id not in self._hardware:
+                return False
+
+            interface = self._hardware[hardware_id]
+            hw_joints = set(interface.joint_names)
+
+            with self._task_lock:
+                for task in self._tasks.values():
+                    if task.is_active():
+                        claimed_joints = task.claim().joints
+                        overlap = hw_joints & claimed_joints
+                        if overlap:
+                            logger.error(
+                                f"Cannot remove hardware {hardware_id}: "
+                                f"task '{task.name}' is actively using joints {overlap}"
+                            )
+                            return False
+
+            for joint_name in interface.joint_names:
+                del self._joint_to_hardware[joint_name]
+
+            interface.disconnect()
+            del self._hardware[hardware_id]
+            logger.info(f"Removed hardware {hardware_id}")
+            return True
+
+    @rpc
+    def list_hardware(self) -> list[str]:
+        """List registered hardware IDs."""
+        with self._hardware_lock:
+            return list(self._hardware.keys())
+
+    @rpc
+    def list_joints(self) -> list[str]:
+        """List all joint names across all hardware."""
+        with self._hardware_lock:
+            return list(self._joint_to_hardware.keys())
+
+    @rpc
+    def get_joint_positions(self) -> dict[str, float]:
+        """Get current joint positions for all joints."""
+        with self._hardware_lock:
+            positions: dict[str, float] = {}
+            for hw in self._hardware.values():
+                state = hw.read_state()  # {joint_name: (pos, vel, effort)}
+                for joint_name, (pos, _vel, _effort) in state.items():
+                    positions[joint_name] = pos
+            return positions
+
+    # =========================================================================
+    # Task Management (RPC)
+    # =========================================================================
+
+    @rpc
+    def add_task(self, task: ControlTask) -> bool:
+        """Register a task with the orchestrator."""
+        if not isinstance(task, ControlTask):
+            raise TypeError("task must implement ControlTask")
+
+        with self._task_lock:
+            if task.name in self._tasks:
+                logger.warning(f"Task {task.name} already registered")
+                return False
+            self._tasks[task.name] = task
+            logger.info(f"Added task {task.name}")
+            return True
+
+    @rpc
+    def remove_task(self, task_name: str) -> bool:
+        """Remove a task by name."""
+        with self._task_lock:
+            if task_name in self._tasks:
+                del self._tasks[task_name]
+                logger.info(f"Removed task {task_name}")
+                return True
+            return False
+
+    @rpc
+    def get_task(self, task_name: str) -> ControlTask | None:
+        """Get a task by name."""
+        with self._task_lock:
+            return self._tasks.get(task_name)
+
+    @rpc
+    def list_tasks(self) -> list[str]:
+        """List registered task names."""
+        with self._task_lock:
+            return list(self._tasks.keys())
+
+    @rpc
+    def get_active_tasks(self) -> list[str]:
+        """List currently active task names."""
+        with self._task_lock:
+            return [name for name, task in self._tasks.items() if task.is_active()]
+
+    # =========================================================================
+    # Trajectory Execution (RPC)
+    # =========================================================================
+
+    @rpc
+    def execute_trajectory(self, task_name: str, trajectory: JointTrajectory) -> bool:
+        """Execute a trajectory on a named task."""
+        with self._task_lock:
+            task = self._tasks.get(task_name)
+            if task is None:
+                logger.warning(f"Task {task_name} not found")
+                return False
+
+            if not hasattr(task, "execute"):
+                logger.warning(f"Task {task_name} doesn't support execute()")
+                return False
+
+            logger.info(
+                f"Executing trajectory on {task_name}: "
+                f"{len(trajectory.points)} points, duration={trajectory.duration:.3f}s"
+            )
+            return task.execute(trajectory)  # type: ignore[attr-defined,no-any-return]
+
+    @rpc
+    def get_trajectory_status(self, task_name: str) -> TaskStatus | None:
+        """Get the status of a trajectory task."""
+        with self._task_lock:
+            task = self._tasks.get(task_name)
+            if task is None:
+                return None
+
+            state: str | None = None
+            if hasattr(task, "get_state"):
+                task_state: TrajectoryState = task.get_state()  # type: ignore[attr-defined]
+                state = (
+                    task_state.name if isinstance(task_state, TrajectoryState) else str(task_state)
+                )
+
+            progress: float | None = None
+            if hasattr(task, "get_progress"):
+                t_now = time.perf_counter()
+                progress = task.get_progress(t_now)  # type: ignore[attr-defined]
+
+            return TaskStatus(active=task.is_active(), state=state, progress=progress)
+
+    @rpc
+    def cancel_trajectory(self, task_name: str) -> bool:
+        """Cancel an active trajectory on a task."""
+        with self._task_lock:
+            task = self._tasks.get(task_name)
+            if task is None:
+                logger.warning(f"Task {task_name} not found")
+                return False
+
+            if not hasattr(task, "cancel"):
+                logger.warning(f"Task {task_name} doesn't support cancel()")
+                return False
+
+            logger.info(f"Cancelling trajectory on {task_name}")
+            return task.cancel()  # type: ignore[attr-defined,no-any-return]
+
+    # =========================================================================
+    # Lifecycle
+    # =========================================================================
+
+    @rpc
+    def start(self) -> None:
+        """Start the orchestrator control loop."""
+        if self._tick_loop and self._tick_loop.is_running:
+            logger.warning("Orchestrator already running")
+            return
+
+        super().start()
+
+        # Setup hardware and tasks from config (if any)
+        if self.config.hardware or self.config.tasks:
+            self._setup_from_config()
+
+        # Create and start tick loop
+        publish_cb = self.joint_state.publish if self.config.publish_joint_state else None
+        self._tick_loop = TickLoop(
+            tick_rate=self.config.tick_rate,
+            hardware=self._hardware,
+            hardware_lock=self._hardware_lock,
+            tasks=self._tasks,
+            task_lock=self._task_lock,
+            joint_to_hardware=self._joint_to_hardware,
+            publish_callback=publish_cb,
+            frame_id=self.config.joint_state_frame_id,
+            log_ticks=self.config.log_ticks,
+        )
+        self._tick_loop.start()
+
+        logger.info(f"ControlOrchestrator started at {self.config.tick_rate}Hz")
+
+    @rpc
+    def stop(self) -> None:
+        """Stop the orchestrator."""
+        logger.info("Stopping ControlOrchestrator...")
+
+        if self._tick_loop:
+            self._tick_loop.stop()
+
+        # Disconnect all hardware backends
+        with self._hardware_lock:
+            for hw_id, interface in self._hardware.items():
+                try:
+                    interface.disconnect()
+                    logger.info(f"Disconnected hardware {hw_id}")
+                except Exception as e:
+                    logger.error(f"Error disconnecting hardware {hw_id}: {e}")
+
+        super().stop()
+        logger.info("ControlOrchestrator stopped")
+
+    @rpc
+    def get_tick_count(self) -> int:
+        """Get the number of ticks since start."""
+        return self._tick_loop.tick_count if self._tick_loop else 0
+
+
+# Blueprint export
+control_orchestrator = ControlOrchestrator.blueprint
+
+
+__all__ = [
+    "ControlOrchestrator",
+    "ControlOrchestratorConfig",
+    "HardwareConfig",
+    "TaskConfig",
+    "control_orchestrator",
+]
diff --git a/dimos/control/task.py b/dimos/control/task.py
new file mode 100644
index 0000000000..49589188d9
--- /dev/null
+++ b/dimos/control/task.py
@@ -0,0 +1,299 @@
+# 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.
+
+"""ControlTask protocol and types for the ControlOrchestrator.
+
+This module defines:
+- Data types used by tasks and the orchestrator (ResourceClaim, JointStateSnapshot, etc.)
+- ControlTask protocol that all tasks must implement
+
+Tasks are "passive" - they don't own threads. The orchestrator calls
+compute() at each tick, passing current state and time.
+
+CRITICAL: Tasks must NEVER call time.time() directly.
+Use the t_now passed in OrchestratorState.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import Protocol, runtime_checkable
+
+from dimos.hardware.manipulators.spec import ControlMode
+
+# =============================================================================
+# Data Types
+# =============================================================================
+
+
+@dataclass(frozen=True)
+class ResourceClaim:
+    """Declares which joints a task wants to control.
+
+    Used by the orchestrator to determine resource ownership and
+    resolve conflicts between competing tasks.
+
+    Attributes:
+        joints: Set of joint names this task wants to control.
+                Example: frozenset({"left_joint1", "left_joint2"})
+        priority: Priority level for conflict resolution. Higher wins.
+                  Typical values: 10 (trajectory), 50 (WBC), 100 (safety)
+        mode: Control mode (POSITION, VELOCITY, TORQUE)
+    """
+
+    joints: frozenset[str]
+    priority: int = 0
+    mode: ControlMode = ControlMode.POSITION
+
+    def conflicts_with(self, other: ResourceClaim) -> bool:
+        """Check if two claims compete for the same joints."""
+        return bool(self.joints & other.joints)
+
+
+@dataclass
+class JointStateSnapshot:
+    """Aggregated joint states from all hardware.
+
+    Provides a unified view of all joint states across all hardware
+    interfaces, indexed by fully-qualified joint name.
+
+    Attributes:
+        joint_positions: Joint name -> position (radians)
+        joint_velocities: Joint name -> velocity (rad/s)
+        joint_efforts: Joint name -> effort (Nm)
+        timestamp: Unix timestamp when state was read
+    """
+
+    joint_positions: dict[str, float] = field(default_factory=dict)
+    joint_velocities: dict[str, float] = field(default_factory=dict)
+    joint_efforts: dict[str, float] = field(default_factory=dict)
+    timestamp: float = 0.0
+
+    def get_position(self, joint_name: str) -> float | None:
+        """Get position for a specific joint."""
+        return self.joint_positions.get(joint_name)
+
+    def get_velocity(self, joint_name: str) -> float | None:
+        """Get velocity for a specific joint."""
+        return self.joint_velocities.get(joint_name)
+
+    def get_effort(self, joint_name: str) -> float | None:
+        """Get effort for a specific joint."""
+        return self.joint_efforts.get(joint_name)
+
+
+@dataclass
+class OrchestratorState:
+    """Complete state snapshot for tasks to read.
+
+    Passed to each task's compute() method every tick. Contains
+    all information a task needs to compute its output.
+
+    CRITICAL: Tasks should use t_now for timing, never time.time()!
+
+    Attributes:
+        joints: Aggregated joint states from all hardware
+        t_now: Current tick time (time.perf_counter())
+        dt: Time since last tick (seconds)
+    """
+
+    joints: JointStateSnapshot
+    t_now: float  # Orchestrator time (perf_counter) - USE THIS, NOT time.time()!
+    dt: float  # Time since last tick
+
+
+@dataclass
+class JointCommandOutput:
+    """Joint-centric command output from a task.
+
+    Commands are addressed by joint name, NOT by hardware ID.
+    The orchestrator routes commands to the appropriate hardware.
+
+    This design enables:
+    - WBC spanning multiple hardware interfaces
+    - Partial joint ownership
+    - Per-joint arbitration
+
+    Attributes:
+        joint_names: Which joints this command is for
+        positions: Position commands (radians), or None
+        velocities: Velocity commands (rad/s), or None
+        efforts: Effort commands (Nm), or None
+        mode: Control mode - must match which field is populated
+    """
+
+    joint_names: list[str]
+    positions: list[float] | None = None
+    velocities: list[float] | None = None
+    efforts: list[float] | None = None
+    mode: ControlMode = ControlMode.POSITION
+
+    def __post_init__(self) -> None:
+        """Validate that lengths match and at least one value field is set."""
+        n = len(self.joint_names)
+
+        if self.positions is not None and len(self.positions) != n:
+            raise ValueError(f"positions length {len(self.positions)} != joint_names length {n}")
+        if self.velocities is not None and len(self.velocities) != n:
+            raise ValueError(f"velocities length {len(self.velocities)} != joint_names length {n}")
+        if self.efforts is not None and len(self.efforts) != n:
+            raise ValueError(f"efforts length {len(self.efforts)} != joint_names length {n}")
+
+    def get_values(self) -> list[float] | None:
+        """Get the active values based on mode."""
+        match self.mode:
+            case ControlMode.POSITION | ControlMode.SERVO_POSITION:
+                return self.positions
+            case ControlMode.VELOCITY:
+                return self.velocities
+            case ControlMode.TORQUE:
+                return self.efforts
+            case _:
+                return None
+
+
+# =============================================================================
+# ControlTask Protocol
+# =============================================================================
+
+
+@runtime_checkable
+class ControlTask(Protocol):
+    """Protocol for passive tasks that run within the orchestrator.
+
+    Tasks are "passive" - they don't own threads. The orchestrator
+    calls compute() at each tick, passing current state and time.
+
+    Lifecycle:
+    1. Task is added to orchestrator via add_task()
+    2. Orchestrator calls claim() to understand resource needs
+    3. Each tick: is_active() → compute() → output merged via arbitration
+    4. Task removed via remove_task() or transitions to inactive
+
+    CRITICAL: Tasks must NEVER call time.time() directly.
+    Use state.t_now passed to compute() for all timing.
+
+    Example:
+        >>> class MyTask:
+        ...     @property
+        ...     def name(self) -> str:
+        ...         return "my_task"
+        ...
+        ...     def claim(self) -> ResourceClaim:
+        ...         return ResourceClaim(
+        ...             joints=frozenset(["left_joint1", "left_joint2"]),
+        ...             priority=10,
+        ...         )
+        ...
+        ...     def is_active(self) -> bool:
+        ...         return self._executing
+        ...
+        ...     def compute(self, state: OrchestratorState) -> JointCommandOutput | None:
+        ...         # Use state.t_now, NOT time.time()!
+        ...         t_elapsed = state.t_now - self._start_time
+        ...         positions = self._trajectory.sample(t_elapsed)
+        ...         return JointCommandOutput(
+        ...             joint_names=["left_joint1", "left_joint2"],
+        ...             positions=positions,
+        ...         )
+        ...
+        ...     def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
+        ...         print(f"Preempted by {by_task} on {joints}")
+    """
+
+    @property
+    def name(self) -> str:
+        """Unique identifier for this task instance.
+
+        Used for logging, debugging, and task management.
+        Must be unique across all tasks in the orchestrator.
+        """
+        ...
+
+    def claim(self) -> ResourceClaim:
+        """Declare resource requirements.
+
+        Called by orchestrator to determine:
+        - Which joints this task wants to control
+        - Priority for conflict resolution
+        - Control mode (position/velocity/effort)
+
+        Returns:
+            ResourceClaim with joints (frozenset) and priority (int)
+
+        Note:
+            The claim can change dynamically - orchestrator calls this
+            every tick for active tasks.
+        """
+        ...
+
+    def is_active(self) -> bool:
+        """Check if task should run this tick.
+
+        Inactive tasks:
+        - Skip compute() call
+        - Don't participate in arbitration
+        - Don't consume resources
+
+        Returns:
+            True if task should execute this tick
+        """
+        ...
+
+    def compute(self, state: OrchestratorState) -> JointCommandOutput | None:
+        """Compute output command given current state.
+
+        Called by orchestrator for active tasks each tick.
+
+        CRITICAL: Use state.t_now for timing, NEVER time.time()!
+        This ensures deterministic behavior and enables simulation.
+
+        Args:
+            state: OrchestratorState containing:
+                   - joints: JointStateSnapshot with all joint states
+                   - t_now: Current tick time (use this for all timing!)
+                   - dt: Time since last tick
+
+        Returns:
+            JointCommandOutput with joint_names and values, or None if
+            no command should be sent this tick.
+        """
+        ...
+
+    def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
+        """Called ONCE per tick with ALL preempted joints aggregated.
+
+        Called when a higher-priority task takes control of some of this
+        task's joints. Allows task to gracefully handle being overridden.
+
+        This is called ONCE per tick with ALL preempted joints, not once
+        per joint. This reduces noise and improves performance.
+
+        Args:
+            by_task: Name of the preempting task (or "arbitration" if multiple)
+            joints: All joints that were preempted this tick
+        """
+        ...
+
+
+__all__ = [
+    # Types
+    "ControlMode",
+    # Protocol
+    "ControlTask",
+    "JointCommandOutput",
+    "JointStateSnapshot",
+    "OrchestratorState",
+    "ResourceClaim",
+]
diff --git a/dimos/hardware/manipulators/base/tests/__init__.py b/dimos/control/tasks/__init__.py
similarity index 70%
rename from dimos/hardware/manipulators/base/tests/__init__.py
rename to dimos/control/tasks/__init__.py
index f863fa5120..75460ffa26 100644
--- a/dimos/hardware/manipulators/base/tests/__init__.py
+++ b/dimos/control/tasks/__init__.py
@@ -12,4 +12,14 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-"""Tests for manipulator base module."""
+"""Task implementations for the ControlOrchestrator."""
+
+from dimos.control.tasks.trajectory_task import (
+    JointTrajectoryTask,
+    JointTrajectoryTaskConfig,
+)
+
+__all__ = [
+    "JointTrajectoryTask",
+    "JointTrajectoryTaskConfig",
+]
diff --git a/dimos/control/tasks/trajectory_task.py b/dimos/control/tasks/trajectory_task.py
new file mode 100644
index 0000000000..08e3ae337e
--- /dev/null
+++ b/dimos/control/tasks/trajectory_task.py
@@ -0,0 +1,261 @@
+# 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.
+
+"""Joint trajectory task for the ControlOrchestrator.
+
+Passive trajectory execution - called by orchestrator each tick.
+Unlike JointTrajectoryController which owns a thread, this task
+is compute-only and relies on the orchestrator for timing.
+
+CRITICAL: Uses t_now from OrchestratorState, never calls time.time()
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+from dimos.control.task import (
+    ControlMode,
+    ControlTask,
+    JointCommandOutput,
+    OrchestratorState,
+    ResourceClaim,
+)
+from dimos.msgs.trajectory_msgs import JointTrajectory, TrajectoryState
+from dimos.utils.logging_config import setup_logger
+
+logger = setup_logger()
+
+
+@dataclass
+class JointTrajectoryTaskConfig:
+    """Configuration for trajectory task.
+
+    Attributes:
+        joint_names: List of joint names this task controls
+        priority: Priority for arbitration (higher wins)
+    """
+
+    joint_names: list[str]
+    priority: int = 10
+
+
+class JointTrajectoryTask(ControlTask):
+    """Passive trajectory execution task.
+
+    Unlike JointTrajectoryController which owns a thread, this task
+    is called by the orchestrator at each tick.
+
+    CRITICAL: Uses t_now from OrchestratorState, never calls time.time()
+
+    State Machine:
+        IDLE ──execute()──► EXECUTING ──done──► COMPLETED
+          ▲                     │                    │
+          │                  cancel()             reset()
+          │                     ▼                    │
+          └─────reset()───── ABORTED ◄──────────────┘
+
+    Example:
+        >>> task = JointTrajectoryTask(
+        ...     name="traj_left",
+        ...     config=JointTrajectoryTaskConfig(
+        ...         joint_names=["left_joint1", "left_joint2"],
+        ...         priority=10,
+        ...     ),
+        ... )
+        >>> orchestrator.add_task(task)
+        >>> task.execute(my_trajectory, t_now=orchestrator_t_now)
+    """
+
+    def __init__(self, name: str, config: JointTrajectoryTaskConfig) -> None:
+        """Initialize trajectory task.
+
+        Args:
+            name: Unique task name
+            config: Task configuration
+        """
+        if not config.joint_names:
+            raise ValueError(f"JointTrajectoryTask '{name}' requires at least one joint")
+        self._name = name
+        self._config = config
+        self._joint_names = frozenset(config.joint_names)
+        self._joint_names_list = list(config.joint_names)
+
+        # State machine
+        self._state = TrajectoryState.IDLE
+        self._trajectory: JointTrajectory | None = None
+        self._start_time: float = 0.0
+        self._pending_start: bool = False  # Defer start time to first compute()
+
+        logger.info(f"JointTrajectoryTask {name} initialized for joints: {config.joint_names}")
+
+    @property
+    def name(self) -> str:
+        """Unique task identifier."""
+        return self._name
+
+    def claim(self) -> ResourceClaim:
+        """Declare resource requirements."""
+        return ResourceClaim(
+            joints=self._joint_names,
+            priority=self._config.priority,
+            mode=ControlMode.SERVO_POSITION,
+        )
+
+    def is_active(self) -> bool:
+        """Check if task should run this tick."""
+        return self._state == TrajectoryState.EXECUTING
+
+    def compute(self, state: OrchestratorState) -> JointCommandOutput | None:
+        """Compute trajectory output for this tick.
+
+        CRITICAL: Uses state.t_now for timing, NOT time.time()!
+
+        Args:
+            state: Current orchestrator state
+
+        Returns:
+            JointCommandOutput with positions, or None if not executing
+        """
+        if self._trajectory is None:
+            return None
+
+        # Set start time on first compute() for consistent timing
+        if self._pending_start:
+            self._start_time = state.t_now
+            self._pending_start = False
+
+        t_elapsed = state.t_now - self._start_time
+
+        # Check completion - clamp to final position to ensure we reach goal
+        if t_elapsed >= self._trajectory.duration:
+            self._state = TrajectoryState.COMPLETED
+            logger.info(f"Trajectory {self._name} completed after {t_elapsed:.3f}s")
+            # Return final position to hold at goal
+            q_ref, _ = self._trajectory.sample(self._trajectory.duration)
+            return JointCommandOutput(
+                joint_names=self._joint_names_list,
+                positions=list(q_ref),
+                mode=ControlMode.SERVO_POSITION,
+            )
+
+        # Sample trajectory
+        q_ref, _ = self._trajectory.sample(t_elapsed)
+
+        return JointCommandOutput(
+            joint_names=self._joint_names_list,
+            positions=list(q_ref),
+            mode=ControlMode.SERVO_POSITION,
+        )
+
+    def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:
+        """Handle preemption by higher-priority task.
+
+        Args:
+            by_task: Name of preempting task
+            joints: Joints that were preempted
+        """
+        logger.warning(f"Trajectory {self._name} preempted by {by_task} on joints {joints}")
+        # Abort if any of our joints were preempted
+        if joints & self._joint_names:
+            self._state = TrajectoryState.ABORTED
+
+    # =========================================================================
+    # Task-specific methods
+    # =========================================================================
+
+    def execute(self, trajectory: JointTrajectory) -> bool:
+        """Start executing a trajectory.
+
+        Args:
+            trajectory: Trajectory to execute
+
+        Returns:
+            True if accepted, False if invalid or in FAULT state
+        """
+        if self._state == TrajectoryState.FAULT:
+            logger.warning(f"Cannot execute: {self._name} in FAULT state")
+            return False
+
+        if trajectory is None or trajectory.duration <= 0:
+            logger.warning(f"Invalid trajectory for {self._name}")
+            return False
+
+        if not trajectory.points:
+            logger.warning(f"Empty trajectory for {self._name}")
+            return False
+
+        # Preempt any active trajectory
+        if self._state == TrajectoryState.EXECUTING:
+            logger.info(f"Preempting active trajectory on {self._name}")
+
+        self._trajectory = trajectory
+        self._pending_start = True  # Start time set on first compute()
+        self._state = TrajectoryState.EXECUTING
+
+        logger.info(
+            f"Executing trajectory on {self._name}: "
+            f"{len(trajectory.points)} points, duration={trajectory.duration:.3f}s"
+        )
+        return True
+
+    def cancel(self) -> bool:
+        """Cancel current trajectory.
+
+        Returns:
+            True if cancelled, False if not executing
+        """
+        if self._state != TrajectoryState.EXECUTING:
+            return False
+        self._state = TrajectoryState.ABORTED
+        logger.info(f"Trajectory {self._name} cancelled")
+        return True
+
+    def reset(self) -> bool:
+        """Reset to idle state.
+
+        Returns:
+            True if reset, False if currently executing
+        """
+        if self._state == TrajectoryState.EXECUTING:
+            logger.warning(f"Cannot reset {self._name} while executing")
+            return False
+        self._state = TrajectoryState.IDLE
+        self._trajectory = None
+        logger.info(f"Trajectory {self._name} reset to IDLE")
+        return True
+
+    def get_state(self) -> TrajectoryState:
+        """Get current state."""
+        return self._state
+
+    def get_progress(self, t_now: float) -> float:
+        """Get execution progress (0.0 to 1.0).
+
+        Args:
+            t_now: Current orchestrator time
+
+        Returns:
+            Progress as fraction, or 0.0 if not executing
+        """
+        if self._state != TrajectoryState.EXECUTING or self._trajectory is None:
+            return 0.0
+        t_elapsed = t_now - self._start_time
+        return min(1.0, t_elapsed / self._trajectory.duration)
+
+
+__all__ = [
+    "JointTrajectoryTask",
+    "JointTrajectoryTaskConfig",
+]
diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py
new file mode 100644
index 0000000000..e35a9853d5
--- /dev/null
+++ b/dimos/control/test_control.py
@@ -0,0 +1,542 @@
+# Copyright 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.
+
+"""Tests for the Control Orchestrator module."""
+
+from __future__ import annotations
+
+import threading
+import time
+from unittest.mock import MagicMock
+
+import pytest
+
+from dimos.control.hardware_interface import BackendHardwareInterface
+from dimos.control.task import (
+    ControlMode,
+    JointCommandOutput,
+    JointStateSnapshot,
+    OrchestratorState,
+    ResourceClaim,
+)
+from dimos.control.tasks.trajectory_task import (
+    JointTrajectoryTask,
+    JointTrajectoryTaskConfig,
+    TrajectoryState,
+)
+from dimos.control.tick_loop import TickLoop
+from dimos.msgs.trajectory_msgs import JointTrajectory, TrajectoryPoint
+
+# =============================================================================
+# Fixtures
+# =============================================================================
+
+
+@pytest.fixture
+def mock_backend():
+    """Create a mock manipulator backend."""
+    backend = MagicMock()
+    backend.get_dof.return_value = 6
+    backend.get_joint_names.return_value = [f"joint{i + 1}" for i in range(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
+
+
+@pytest.fixture
+def hardware_interface(mock_backend):
+    """Create a BackendHardwareInterface with mock backend."""
+    return BackendHardwareInterface(
+        backend=mock_backend,
+        hardware_id="test_arm",
+        joint_prefix="arm",
+    )
+
+
+@pytest.fixture
+def trajectory_task():
+    """Create a JointTrajectoryTask for testing."""
+    config = JointTrajectoryTaskConfig(
+        joint_names=["arm_joint1", "arm_joint2", "arm_joint3"],
+        priority=10,
+    )
+    return JointTrajectoryTask(name="test_traj", config=config)
+
+
+@pytest.fixture
+def simple_trajectory():
+    """Create a simple 2-point trajectory."""
+    return JointTrajectory(
+        joint_names=["arm_joint1", "arm_joint2", "arm_joint3"],
+        points=[
+            TrajectoryPoint(
+                positions=[0.0, 0.0, 0.0],
+                velocities=[0.0, 0.0, 0.0],
+                time_from_start=0.0,
+            ),
+            TrajectoryPoint(
+                positions=[1.0, 0.5, 0.25],
+                velocities=[0.0, 0.0, 0.0],
+                time_from_start=1.0,
+            ),
+        ],
+    )
+
+
+@pytest.fixture
+def orchestrator_state():
+    """Create a sample OrchestratorState."""
+    joints = JointStateSnapshot(
+        joint_positions={"arm_joint1": 0.0, "arm_joint2": 0.0, "arm_joint3": 0.0},
+        joint_velocities={"arm_joint1": 0.0, "arm_joint2": 0.0, "arm_joint3": 0.0},
+        joint_efforts={"arm_joint1": 0.0, "arm_joint2": 0.0, "arm_joint3": 0.0},
+        timestamp=time.perf_counter(),
+    )
+    return OrchestratorState(joints=joints, t_now=time.perf_counter(), dt=0.01)
+
+
+# =============================================================================
+# Test JointCommandOutput
+# =============================================================================
+
+
+class TestJointCommandOutput:
+    def test_position_output(self):
+        output = JointCommandOutput(
+            joint_names=["j1", "j2"],
+            positions=[0.5, 1.0],
+            mode=ControlMode.POSITION,
+        )
+        assert output.get_values() == [0.5, 1.0]
+        assert output.mode == ControlMode.POSITION
+
+    def test_velocity_output(self):
+        output = JointCommandOutput(
+            joint_names=["j1", "j2"],
+            velocities=[0.1, 0.2],
+            mode=ControlMode.VELOCITY,
+        )
+        assert output.get_values() == [0.1, 0.2]
+        assert output.mode == ControlMode.VELOCITY
+
+    def test_torque_output(self):
+        output = JointCommandOutput(
+            joint_names=["j1", "j2"],
+            efforts=[5.0, 10.0],
+            mode=ControlMode.TORQUE,
+        )
+        assert output.get_values() == [5.0, 10.0]
+        assert output.mode == ControlMode.TORQUE
+
+    def test_no_values_returns_none(self):
+        output = JointCommandOutput(
+            joint_names=["j1"],
+            mode=ControlMode.POSITION,
+        )
+        assert output.get_values() is None
+
+
+# =============================================================================
+# Test JointStateSnapshot
+# =============================================================================
+
+
+class TestJointStateSnapshot:
+    def test_get_position(self):
+        snapshot = JointStateSnapshot(
+            joint_positions={"j1": 0.5, "j2": 1.0},
+            joint_velocities={"j1": 0.0, "j2": 0.1},
+            joint_efforts={"j1": 1.0, "j2": 2.0},
+            timestamp=100.0,
+        )
+        assert snapshot.get_position("j1") == 0.5
+        assert snapshot.get_position("j2") == 1.0
+        assert snapshot.get_position("nonexistent") is None
+
+
+# =============================================================================
+# Test BackendHardwareInterface
+# =============================================================================
+
+
+class TestBackendHardwareInterface:
+    def test_joint_names_prefixed(self, hardware_interface):
+        names = hardware_interface.joint_names
+        assert names == [
+            "arm_joint1",
+            "arm_joint2",
+            "arm_joint3",
+            "arm_joint4",
+            "arm_joint5",
+            "arm_joint6",
+        ]
+
+    def test_read_state(self, hardware_interface):
+        state = hardware_interface.read_state()
+        assert "arm_joint1" in state
+        assert len(state) == 6
+        pos, vel, eff = state["arm_joint1"]
+        assert pos == 0.0
+        assert vel == 0.0
+        assert eff == 0.0
+
+    def test_write_command(self, hardware_interface, mock_backend):
+        commands = {
+            "arm_joint1": 0.5,
+            "arm_joint2": 1.0,
+        }
+        hardware_interface.write_command(commands, ControlMode.POSITION)
+        mock_backend.write_joint_positions.assert_called()
+
+
+# =============================================================================
+# Test JointTrajectoryTask
+# =============================================================================
+
+
+class TestJointTrajectoryTask:
+    def test_initial_state(self, trajectory_task):
+        assert trajectory_task.name == "test_traj"
+        assert not trajectory_task.is_active()
+        assert trajectory_task.get_state() == TrajectoryState.IDLE
+
+    def test_claim(self, trajectory_task):
+        claim = trajectory_task.claim()
+        assert claim.priority == 10
+        assert "arm_joint1" in claim.joints
+        assert "arm_joint2" in claim.joints
+        assert "arm_joint3" in claim.joints
+
+    def test_execute_trajectory(self, trajectory_task, simple_trajectory):
+        time.perf_counter()
+        result = trajectory_task.execute(simple_trajectory)
+        assert result is True
+        assert trajectory_task.is_active()
+        assert trajectory_task.get_state() == TrajectoryState.EXECUTING
+
+    def test_compute_during_trajectory(
+        self, trajectory_task, simple_trajectory, orchestrator_state
+    ):
+        t_start = time.perf_counter()
+        trajectory_task.execute(simple_trajectory)
+
+        # First compute sets start time (deferred start)
+        state0 = OrchestratorState(
+            joints=orchestrator_state.joints,
+            t_now=t_start,
+            dt=0.01,
+        )
+        trajectory_task.compute(state0)
+
+        # Compute at 0.5s into trajectory
+        state = OrchestratorState(
+            joints=orchestrator_state.joints,
+            t_now=t_start + 0.5,
+            dt=0.01,
+        )
+        output = trajectory_task.compute(state)
+
+        assert output is not None
+        assert output.mode == ControlMode.SERVO_POSITION
+        assert len(output.positions) == 3
+        assert 0.4 < output.positions[0] < 0.6
+
+    def test_trajectory_completes(self, trajectory_task, simple_trajectory, orchestrator_state):
+        t_start = time.perf_counter()
+        trajectory_task.execute(simple_trajectory)
+
+        # First compute sets start time (deferred start)
+        state0 = OrchestratorState(
+            joints=orchestrator_state.joints,
+            t_now=t_start,
+            dt=0.01,
+        )
+        trajectory_task.compute(state0)
+
+        # Compute past trajectory duration
+        state = OrchestratorState(
+            joints=orchestrator_state.joints,
+            t_now=t_start + 1.5,
+            dt=0.01,
+        )
+        output = trajectory_task.compute(state)
+
+        # On completion, returns final position (not None) to hold at goal
+        assert output is not None
+        assert output.positions == [1.0, 0.5, 0.25]  # Final trajectory point
+        assert not trajectory_task.is_active()
+        assert trajectory_task.get_state() == TrajectoryState.COMPLETED
+
+    def test_cancel_trajectory(self, trajectory_task, simple_trajectory):
+        trajectory_task.execute(simple_trajectory)
+        assert trajectory_task.is_active()
+
+        trajectory_task.cancel()
+        assert not trajectory_task.is_active()
+        assert trajectory_task.get_state() == TrajectoryState.ABORTED
+
+    def test_preemption(self, trajectory_task, simple_trajectory):
+        trajectory_task.execute(simple_trajectory)
+
+        trajectory_task.on_preempted("safety_task", frozenset({"arm_joint1"}))
+        assert trajectory_task.get_state() == TrajectoryState.ABORTED
+        assert not trajectory_task.is_active()
+
+    def test_progress(self, trajectory_task, simple_trajectory, orchestrator_state):
+        t_start = time.perf_counter()
+        trajectory_task.execute(simple_trajectory)
+
+        # First compute sets start time (deferred start)
+        state0 = OrchestratorState(
+            joints=orchestrator_state.joints,
+            t_now=t_start,
+            dt=0.01,
+        )
+        trajectory_task.compute(state0)
+
+        assert trajectory_task.get_progress(t_start) == pytest.approx(0.0, abs=0.01)
+        assert trajectory_task.get_progress(t_start + 0.5) == pytest.approx(0.5, abs=0.01)
+        assert trajectory_task.get_progress(t_start + 1.0) == pytest.approx(1.0, abs=0.01)
+
+
+# =============================================================================
+# Test Arbitration Logic
+# =============================================================================
+
+
+class TestArbitration:
+    def test_single_task_wins(self):
+        outputs = [
+            (
+                MagicMock(name="task1"),
+                ResourceClaim(joints=frozenset({"j1"}), priority=10),
+                JointCommandOutput(joint_names=["j1"], positions=[0.5], mode=ControlMode.POSITION),
+            ),
+        ]
+
+        winners = {}
+        for task, claim, output in outputs:
+            if output is None:
+                continue
+            values = output.get_values()
+            if values is None:
+                continue
+            for i, joint in enumerate(output.joint_names):
+                if joint not in winners:
+                    winners[joint] = (claim.priority, values[i], output.mode, task.name)
+
+        assert "j1" in winners
+        assert winners["j1"][1] == 0.5
+
+    def test_higher_priority_wins(self):
+        task_low = MagicMock()
+        task_low.name = "low_priority"
+        task_high = MagicMock()
+        task_high.name = "high_priority"
+
+        outputs = [
+            (
+                task_low,
+                ResourceClaim(joints=frozenset({"j1"}), priority=10),
+                JointCommandOutput(joint_names=["j1"], positions=[0.5], mode=ControlMode.POSITION),
+            ),
+            (
+                task_high,
+                ResourceClaim(joints=frozenset({"j1"}), priority=100),
+                JointCommandOutput(joint_names=["j1"], positions=[0.0], mode=ControlMode.POSITION),
+            ),
+        ]
+
+        winners = {}
+        for task, claim, output in outputs:
+            if output is None:
+                continue
+            values = output.get_values()
+            if values is None:
+                continue
+            for i, joint in enumerate(output.joint_names):
+                if joint not in winners:
+                    winners[joint] = (claim.priority, values[i], output.mode, task.name)
+                elif claim.priority > winners[joint][0]:
+                    winners[joint] = (claim.priority, values[i], output.mode, task.name)
+
+        assert winners["j1"][3] == "high_priority"
+        assert winners["j1"][1] == 0.0
+
+    def test_non_overlapping_joints(self):
+        task1 = MagicMock()
+        task1.name = "task1"
+        task2 = MagicMock()
+        task2.name = "task2"
+
+        outputs = [
+            (
+                task1,
+                ResourceClaim(joints=frozenset({"j1", "j2"}), priority=10),
+                JointCommandOutput(
+                    joint_names=["j1", "j2"],
+                    positions=[0.5, 0.6],
+                    mode=ControlMode.POSITION,
+                ),
+            ),
+            (
+                task2,
+                ResourceClaim(joints=frozenset({"j3", "j4"}), priority=10),
+                JointCommandOutput(
+                    joint_names=["j3", "j4"],
+                    positions=[0.7, 0.8],
+                    mode=ControlMode.POSITION,
+                ),
+            ),
+        ]
+
+        winners = {}
+        for task, claim, output in outputs:
+            if output is None:
+                continue
+            values = output.get_values()
+            if values is None:
+                continue
+            for i, joint in enumerate(output.joint_names):
+                if joint not in winners:
+                    winners[joint] = (claim.priority, values[i], output.mode, task.name)
+
+        assert winners["j1"][3] == "task1"
+        assert winners["j2"][3] == "task1"
+        assert winners["j3"][3] == "task2"
+        assert winners["j4"][3] == "task2"
+
+
+# =============================================================================
+# Test TickLoop
+# =============================================================================
+
+
+class TestTickLoop:
+    def test_tick_loop_starts_and_stops(self, mock_backend):
+        hw = BackendHardwareInterface(mock_backend, "arm", "arm")
+        hardware = {"arm": hw}
+        tasks: dict = {}
+        joint_to_hardware = {f"arm_joint{i + 1}": "arm" for i in range(6)}
+
+        tick_loop = TickLoop(
+            tick_rate=100.0,
+            hardware=hardware,
+            hardware_lock=threading.Lock(),
+            tasks=tasks,
+            task_lock=threading.Lock(),
+            joint_to_hardware=joint_to_hardware,
+        )
+
+        tick_loop.start()
+        time.sleep(0.05)
+        assert tick_loop.tick_count > 0
+
+        tick_loop.stop()
+        final_count = tick_loop.tick_count
+        time.sleep(0.02)
+        assert tick_loop.tick_count == final_count
+
+    def test_tick_loop_calls_compute(self, mock_backend):
+        hw = BackendHardwareInterface(mock_backend, "arm", "arm")
+        hardware = {"arm": hw}
+
+        mock_task = MagicMock()
+        mock_task.name = "test_task"
+        mock_task.is_active.return_value = True
+        mock_task.claim.return_value = ResourceClaim(
+            joints=frozenset({"arm_joint1"}),
+            priority=10,
+        )
+        mock_task.compute.return_value = JointCommandOutput(
+            joint_names=["arm_joint1"],
+            positions=[0.5],
+            mode=ControlMode.POSITION,
+        )
+
+        tasks = {"test_task": mock_task}
+        joint_to_hardware = {f"arm_joint{i + 1}": "arm" for i in range(6)}
+
+        tick_loop = TickLoop(
+            tick_rate=100.0,
+            hardware=hardware,
+            hardware_lock=threading.Lock(),
+            tasks=tasks,
+            task_lock=threading.Lock(),
+            joint_to_hardware=joint_to_hardware,
+        )
+
+        tick_loop.start()
+        time.sleep(0.05)
+        tick_loop.stop()
+
+        assert mock_task.compute.call_count > 0
+
+
+# =============================================================================
+# Integration Test
+# =============================================================================
+
+
+class TestIntegration:
+    def test_full_trajectory_execution(self, mock_backend):
+        hw = BackendHardwareInterface(mock_backend, "arm", "arm")
+        hardware = {"arm": hw}
+
+        config = JointTrajectoryTaskConfig(
+            joint_names=[f"arm_joint{i + 1}" for i in range(6)],
+            priority=10,
+        )
+        traj_task = JointTrajectoryTask(name="traj_arm", config=config)
+        tasks = {"traj_arm": traj_task}
+
+        joint_to_hardware = {f"arm_joint{i + 1}": "arm" for i in range(6)}
+
+        tick_loop = TickLoop(
+            tick_rate=100.0,
+            hardware=hardware,
+            hardware_lock=threading.Lock(),
+            tasks=tasks,
+            task_lock=threading.Lock(),
+            joint_to_hardware=joint_to_hardware,
+        )
+
+        trajectory = JointTrajectory(
+            joint_names=[f"arm_joint{i + 1}" for i in range(6)],
+            points=[
+                TrajectoryPoint(
+                    positions=[0.0] * 6,
+                    velocities=[0.0] * 6,
+                    time_from_start=0.0,
+                ),
+                TrajectoryPoint(
+                    positions=[0.5] * 6,
+                    velocities=[0.0] * 6,
+                    time_from_start=0.5,
+                ),
+            ],
+        )
+
+        tick_loop.start()
+        traj_task.execute(trajectory)
+
+        time.sleep(0.6)
+        tick_loop.stop()
+
+        assert traj_task.get_state() == TrajectoryState.COMPLETED
+        assert mock_backend.write_joint_positions.call_count > 0
diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py
new file mode 100644
index 0000000000..03e4e0ebd0
--- /dev/null
+++ b/dimos/control/tick_loop.py
@@ -0,0 +1,399 @@
+# 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.
+
+"""Tick loop for the ControlOrchestrator.
+
+This module contains the core control loop logic:
+- Read state from all hardware
+- Compute outputs from all active tasks
+- Arbitrate conflicts per-joint (highest priority wins)
+- Route commands to hardware
+- Publish aggregated joint state
+
+Separated from orchestrator.py following the DimOS pattern of
+splitting coordination logic from module wrapper.
+"""
+
+from __future__ import annotations
+
+import threading
+import time
+from typing import TYPE_CHECKING, NamedTuple
+
+from dimos.control.task import (
+    ControlTask,
+    JointCommandOutput,
+    JointStateSnapshot,
+    OrchestratorState,
+    ResourceClaim,
+)
+from dimos.msgs.sensor_msgs import JointState
+from dimos.utils.logging_config import setup_logger
+
+if TYPE_CHECKING:
+    from collections.abc import Callable
+
+    from dimos.control.hardware_interface import HardwareInterface
+    from dimos.hardware.manipulators.spec import ControlMode
+
+logger = setup_logger()
+
+
+class JointWinner(NamedTuple):
+    """Tracks the winning task for a joint during arbitration."""
+
+    priority: int
+    value: float
+    mode: ControlMode
+    task_name: str
+
+
+class TickLoop:
+    """Core tick loop for the control orchestrator.
+
+    Runs the deterministic control cycle:
+    1. READ: Collect joint state from all hardware
+    2. COMPUTE: Run all active tasks
+    3. ARBITRATE: Per-joint conflict resolution (highest priority wins)
+    4. NOTIFY: Send preemption notifications to affected tasks
+    5. ROUTE: Convert joint-centric commands to hardware-centric
+    6. WRITE: Send commands to hardware
+    7. PUBLISH: Output aggregated JointState
+
+    Args:
+        tick_rate: Control loop frequency in Hz
+        hardware: Dict of hardware_id -> HardwareInterface
+        hardware_lock: Lock protecting hardware dict
+        tasks: Dict of task_name -> ControlTask
+        task_lock: Lock protecting tasks dict
+        joint_to_hardware: Dict mapping joint_name -> hardware_id
+        publish_callback: Optional callback to publish JointState
+        frame_id: Frame ID for published JointState
+        log_ticks: Whether to log tick information
+    """
+
+    def __init__(
+        self,
+        tick_rate: float,
+        hardware: dict[str, HardwareInterface],
+        hardware_lock: threading.Lock,
+        tasks: dict[str, ControlTask],
+        task_lock: threading.Lock,
+        joint_to_hardware: dict[str, str],
+        publish_callback: Callable[[JointState], None] | None = None,
+        frame_id: str = "orchestrator",
+        log_ticks: bool = False,
+    ) -> None:
+        self._tick_rate = tick_rate
+        self._hardware = hardware
+        self._hardware_lock = hardware_lock
+        self._tasks = tasks
+        self._task_lock = task_lock
+        self._joint_to_hardware = joint_to_hardware
+        self._publish_callback = publish_callback
+        self._frame_id = frame_id
+        self._log_ticks = log_ticks
+
+        self._stop_event = threading.Event()
+        self._stop_event.set()  # Initially stopped
+        self._tick_thread: threading.Thread | None = None
+        self._last_tick_time: float = 0.0
+        self._tick_count: int = 0
+
+    @property
+    def tick_count(self) -> int:
+        """Number of ticks since start."""
+        return self._tick_count
+
+    @property
+    def is_running(self) -> bool:
+        """Whether the tick loop is currently running."""
+        return not self._stop_event.is_set()
+
+    def start(self) -> None:
+        """Start the tick loop in a daemon thread."""
+        if not self._stop_event.is_set():
+            logger.warning("TickLoop already running")
+            return
+
+        self._stop_event.clear()
+        self._last_tick_time = time.perf_counter()
+        self._tick_count = 0
+
+        self._tick_thread = threading.Thread(
+            target=self._loop,
+            name="ControlOrchestrator-Tick",
+            daemon=True,
+        )
+        self._tick_thread.start()
+        logger.info(f"TickLoop started at {self._tick_rate}Hz")
+
+    def stop(self) -> None:
+        """Stop the tick loop."""
+        self._stop_event.set()
+        if self._tick_thread and self._tick_thread.is_alive():
+            self._tick_thread.join(timeout=2.0)
+        logger.info("TickLoop stopped")
+
+    def _loop(self) -> None:
+        """Main control loop - deterministic read → compute → arbitrate → write."""
+        period = 1.0 / self._tick_rate
+
+        while not self._stop_event.is_set():
+            tick_start = time.perf_counter()
+
+            try:
+                self._tick()
+            except Exception as e:
+                logger.error(f"TickLoop tick error: {e}")
+
+            # Rate control - recalculate sleep time to account for overhead
+            next_tick_time = tick_start + period
+            sleep_time = next_tick_time - time.perf_counter()
+            if sleep_time > 0:
+                time.sleep(sleep_time)
+
+    def _tick(self) -> None:
+        """Single tick: read → compute → arbitrate → route → write."""
+        t_now = time.perf_counter()
+        dt = t_now - self._last_tick_time
+        self._last_tick_time = t_now
+        self._tick_count += 1
+
+        # === PHASE 1: READ ALL HARDWARE ===
+        joint_states = self._read_all_hardware()
+        state = OrchestratorState(joints=joint_states, t_now=t_now, dt=dt)
+
+        # === PHASE 2: COMPUTE ALL ACTIVE TASKS ===
+        commands = self._compute_all_tasks(state)
+
+        # === PHASE 3: ARBITRATE (with mode validation) ===
+        joint_commands, preemptions = self._arbitrate(commands)
+
+        # === PHASE 4: NOTIFY PREEMPTIONS (once per task) ===
+        self._notify_preemptions(preemptions)
+
+        # === PHASE 5: ROUTE TO HARDWARE ===
+        hw_commands = self._route_to_hardware(joint_commands)
+
+        # === PHASE 6: WRITE TO HARDWARE ===
+        self._write_all_hardware(hw_commands)
+
+        # === PHASE 7: PUBLISH AGGREGATED STATE ===
+        if self._publish_callback:
+            self._publish_joint_state(joint_states)
+
+        # Optional logging
+        if self._log_ticks:
+            active = len([c for c in commands if c[2] is not None])
+            logger.debug(
+                f"Tick {self._tick_count}: dt={dt:.4f}s, "
+                f"{len(joint_states.joint_positions)} joints, "
+                f"{active} active tasks"
+            )
+
+    def _read_all_hardware(self) -> JointStateSnapshot:
+        """Read state from all hardware interfaces."""
+        joint_positions: dict[str, float] = {}
+        joint_velocities: dict[str, float] = {}
+        joint_efforts: dict[str, float] = {}
+
+        with self._hardware_lock:
+            for hw in self._hardware.values():
+                try:
+                    state = hw.read_state()
+                    for joint_name, (pos, vel, eff) in state.items():
+                        joint_positions[joint_name] = pos
+                        joint_velocities[joint_name] = vel
+                        joint_efforts[joint_name] = eff
+                except Exception as e:
+                    logger.error(f"Failed to read {hw.hardware_id}: {e}")
+
+        return JointStateSnapshot(
+            joint_positions=joint_positions,
+            joint_velocities=joint_velocities,
+            joint_efforts=joint_efforts,
+            timestamp=time.time(),
+        )
+
+    def _compute_all_tasks(
+        self, state: OrchestratorState
+    ) -> list[tuple[ControlTask, ResourceClaim, JointCommandOutput | None]]:
+        """Compute outputs from all active tasks."""
+        results: list[tuple[ControlTask, ResourceClaim, JointCommandOutput | None]] = []
+
+        with self._task_lock:
+            for task in self._tasks.values():
+                if not task.is_active():
+                    continue
+
+                try:
+                    claim = task.claim()
+                    output = task.compute(state)
+                    results.append((task, claim, output))
+                except Exception as e:
+                    logger.error(f"Task {task.name} compute error: {e}")
+
+        return results
+
+    def _arbitrate(
+        self,
+        commands: list[tuple[ControlTask, ResourceClaim, JointCommandOutput | None]],
+    ) -> tuple[
+        dict[str, tuple[float, ControlMode, str]],
+        dict[str, dict[str, str]],
+    ]:
+        """Per-joint arbitration with mode conflict detection.
+
+        Returns:
+            Tuple of:
+            - joint_commands: {joint_name: (value, mode, task_name)}
+            - preemptions: {preempted_task: {joint: winning_task}}
+        """
+        winners: dict[str, JointWinner] = {}  # joint_name -> current winner
+        preemptions: dict[str, dict[str, str]] = {}  # loser_task -> {joint: winner_task}
+
+        for task, claim, output in commands:
+            if output is None:
+                continue
+
+            values = output.get_values()
+            if values is None:
+                continue
+
+            for i, joint_name in enumerate(output.joint_names):
+                candidate = JointWinner(claim.priority, values[i], output.mode, task.name)
+
+                # First claim on this joint
+                if joint_name not in winners:
+                    winners[joint_name] = candidate
+                    continue
+
+                current = winners[joint_name]
+
+                # Lower priority loses - notify preemption
+                if candidate.priority < current.priority:
+                    preemptions.setdefault(task.name, {})[joint_name] = current.task_name
+                    continue
+
+                # Higher priority - take over
+                if candidate.priority > current.priority:
+                    preemptions.setdefault(current.task_name, {})[joint_name] = task.name
+                    winners[joint_name] = candidate
+                    continue
+
+                # Same priority - check for mode conflict
+                if candidate.mode != current.mode:
+                    logger.warning(
+                        f"Mode conflict on {joint_name}: {task.name} wants "
+                        f"{candidate.mode.name}, but {current.task_name} wants "
+                        f"{current.mode.name}. Dropping {task.name}."
+                    )
+                    preemptions.setdefault(task.name, {})[joint_name] = current.task_name
+                # Same priority + same mode: first wins (keep current)
+
+        # Convert to output format: joint -> (value, mode, task_name)
+        joint_commands = {joint: (w.value, w.mode, w.task_name) for joint, w in winners.items()}
+
+        return joint_commands, preemptions
+
+    def _notify_preemptions(self, preemptions: dict[str, dict[str, str]]) -> None:
+        """Notify each preempted task with affected joints, grouped by winning task."""
+        with self._task_lock:
+            for task_name, joint_winners in preemptions.items():
+                task = self._tasks.get(task_name)
+                if not task:
+                    continue
+
+                # Group joints by winning task
+                by_winner: dict[str, set[str]] = {}
+                for joint, winner in joint_winners.items():
+                    if winner not in by_winner:
+                        by_winner[winner] = set()
+                    by_winner[winner].add(joint)
+
+                # Notify once per distinct winning task
+                for winner, joints in by_winner.items():
+                    try:
+                        task.on_preempted(
+                            by_task=winner,
+                            joints=frozenset(joints),
+                        )
+                    except Exception as e:
+                        logger.error(f"Error notifying {task_name} of preemption: {e}")
+
+    def _route_to_hardware(
+        self,
+        joint_commands: dict[str, tuple[float, ControlMode, str]],
+    ) -> dict[str, tuple[dict[str, float], ControlMode]]:
+        """Route joint-centric commands to hardware.
+
+        Returns:
+            {hardware_id: ({joint: value}, mode)}
+        """
+        hw_commands: dict[str, tuple[dict[str, float], ControlMode]] = {}
+
+        with self._hardware_lock:
+            for joint_name, (value, mode, _) in joint_commands.items():
+                hw_id = self._joint_to_hardware.get(joint_name)
+                if hw_id is None:
+                    logger.warning(f"Unknown joint {joint_name}, cannot route")
+                    continue
+
+                if hw_id not in hw_commands:
+                    hw_commands[hw_id] = ({}, mode)
+                else:
+                    # Check for mode conflict across joints on same hardware
+                    existing_mode = hw_commands[hw_id][1]
+                    if mode != existing_mode:
+                        logger.error(
+                            f"Mode conflict for hardware {hw_id}: joint {joint_name} wants "
+                            f"{mode.name} but hardware already has {existing_mode.name}. "
+                            f"Dropping command for {joint_name}."
+                        )
+                        continue
+
+                hw_commands[hw_id][0][joint_name] = value
+
+        return hw_commands
+
+    def _write_all_hardware(
+        self,
+        hw_commands: dict[str, tuple[dict[str, float], ControlMode]],
+    ) -> None:
+        """Write commands to all hardware interfaces."""
+        with self._hardware_lock:
+            for hw_id, (positions, mode) in hw_commands.items():
+                if hw_id in self._hardware:
+                    try:
+                        self._hardware[hw_id].write_command(positions, mode)
+                    except Exception as e:
+                        logger.error(f"Failed to write to {hw_id}: {e}")
+
+    def _publish_joint_state(self, snapshot: JointStateSnapshot) -> None:
+        """Publish aggregated JointState for external consumers."""
+        names = list(snapshot.joint_positions.keys())
+        msg = JointState(
+            ts=snapshot.timestamp,
+            frame_id=self._frame_id,
+            name=names,
+            position=[snapshot.joint_positions[n] for n in names],
+            velocity=[snapshot.joint_velocities.get(n, 0.0) for n in names],
+            effort=[snapshot.joint_efforts.get(n, 0.0) for n in names],
+        )
+        if self._publish_callback:
+            self._publish_callback(msg)
+
+
+__all__ = ["TickLoop"]
diff --git a/dimos/e2e_tests/test_control_orchestrator.py b/dimos/e2e_tests/test_control_orchestrator.py
new file mode 100644
index 0000000000..a97c045ce6
--- /dev/null
+++ b/dimos/e2e_tests/test_control_orchestrator.py
@@ -0,0 +1,263 @@
+# 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.
+
+"""End-to-end tests for the ControlOrchestrator.
+
+These tests start a real orchestrator process and communicate via LCM/RPC.
+Unlike unit tests, these verify the full system integration.
+
+Run with:
+    pytest dimos/e2e_tests/test_control_orchestrator.py -v -s
+"""
+
+import os
+import time
+
+import pytest
+
+from dimos.control.orchestrator import ControlOrchestrator
+from dimos.core.rpc_client import RPCClient
+from dimos.msgs.sensor_msgs import JointState
+from dimos.msgs.trajectory_msgs import JointTrajectory, TrajectoryPoint, TrajectoryState
+
+
+@pytest.mark.skipif(bool(os.getenv("CI")), reason="LCM doesn't work in CI.")
+class TestControlOrchestratorE2E:
+    """End-to-end tests for ControlOrchestrator."""
+
+    def test_orchestrator_starts_and_responds_to_rpc(self, lcm_spy, start_blueprint) -> None:
+        """Test that orchestrator starts and responds to RPC queries."""
+        # Save topics we care about (LCM topics include type suffix)
+        joint_state_topic = "/orchestrator/joint_state#sensor_msgs.JointState"
+        lcm_spy.save_topic(joint_state_topic)
+        lcm_spy.save_topic("/rpc/ControlOrchestrator/list_joints/res")
+        lcm_spy.save_topic("/rpc/ControlOrchestrator/list_tasks/res")
+
+        # Start the mock orchestrator blueprint
+        start_blueprint("orchestrator-mock")
+
+        # Wait for joint state to be published (proves tick loop is running)
+        lcm_spy.wait_for_saved_topic(
+            joint_state_topic,
+            timeout=10.0,
+        )
+
+        # Create RPC client and query
+        client = RPCClient(None, ControlOrchestrator)
+        try:
+            # Test list_joints RPC
+            joints = client.list_joints()
+            assert joints is not None
+            assert len(joints) == 7  # Mock arm has 7 DOF
+            assert "arm_joint1" in joints
+
+            # Test list_tasks RPC
+            tasks = client.list_tasks()
+            assert tasks is not None
+            assert "traj_arm" in tasks
+
+            # Test list_hardware RPC
+            hardware = client.list_hardware()
+            assert hardware is not None
+            assert "arm" in hardware
+        finally:
+            client.stop_rpc_client()
+
+    def test_orchestrator_executes_trajectory(self, lcm_spy, start_blueprint) -> None:
+        """Test that orchestrator executes a trajectory via RPC."""
+        # Save topics
+        lcm_spy.save_topic("/orchestrator/joint_state#sensor_msgs.JointState")
+        lcm_spy.save_topic("/rpc/ControlOrchestrator/execute_trajectory/res")
+        lcm_spy.save_topic("/rpc/ControlOrchestrator/get_trajectory_status/res")
+
+        # Start orchestrator
+        start_blueprint("orchestrator-mock")
+
+        # Wait for it to be ready
+        lcm_spy.wait_for_saved_topic(
+            "/orchestrator/joint_state#sensor_msgs.JointState", timeout=10.0
+        )
+
+        # Create RPC client
+        client = RPCClient(None, ControlOrchestrator)
+        try:
+            # Get initial joint positions
+            initial_positions = client.get_joint_positions()
+            assert initial_positions is not None
+
+            # Create a simple trajectory
+            trajectory = JointTrajectory(
+                joint_names=[f"arm_joint{i + 1}" for i in range(7)],
+                points=[
+                    TrajectoryPoint(
+                        time_from_start=0.0,
+                        positions=[0.0] * 7,
+                        velocities=[0.0] * 7,
+                    ),
+                    TrajectoryPoint(
+                        time_from_start=0.5,
+                        positions=[0.1] * 7,
+                        velocities=[0.0] * 7,
+                    ),
+                ],
+            )
+
+            # Execute trajectory
+            result = client.execute_trajectory("traj_arm", trajectory)
+            assert result is True
+
+            # Poll for completion
+            timeout = 5.0
+            start_time = time.time()
+            completed = False
+
+            while time.time() - start_time < timeout:
+                status = client.get_trajectory_status("traj_arm")
+                if status is not None and status.get("state") == TrajectoryState.COMPLETED.name:
+                    completed = True
+                    break
+                time.sleep(0.1)
+
+            assert completed, "Trajectory did not complete within timeout"
+        finally:
+            client.stop_rpc_client()
+
+    def test_orchestrator_joint_state_published(self, lcm_spy, start_blueprint) -> None:
+        """Test that joint state messages are published at expected rate."""
+        joint_state_topic = "/orchestrator/joint_state#sensor_msgs.JointState"
+        lcm_spy.save_topic(joint_state_topic)
+
+        # Start orchestrator
+        start_blueprint("orchestrator-mock")
+
+        # Wait for initial message
+        lcm_spy.wait_for_saved_topic(joint_state_topic, timeout=10.0)
+
+        # Collect messages for 1 second
+        time.sleep(1.0)
+
+        # Check we received messages (should be ~100 at 100Hz)
+        with lcm_spy._messages_lock:
+            message_count = len(lcm_spy.messages.get(joint_state_topic, []))
+
+        # Allow some tolerance (at least 50 messages in 1 second)
+        assert message_count >= 50, f"Expected ~100 messages, got {message_count}"
+
+        # Decode a message to verify structure
+        with lcm_spy._messages_lock:
+            raw_msg = lcm_spy.messages[joint_state_topic][0]
+
+        joint_state = JointState.lcm_decode(raw_msg)
+        assert len(joint_state.name) == 7
+        assert len(joint_state.position) == 7
+        assert "arm_joint1" in joint_state.name
+
+    def test_orchestrator_cancel_trajectory(self, lcm_spy, start_blueprint) -> None:
+        """Test that a running trajectory can be cancelled."""
+        lcm_spy.save_topic("/orchestrator/joint_state#sensor_msgs.JointState")
+
+        # Start orchestrator
+        start_blueprint("orchestrator-mock")
+        lcm_spy.wait_for_saved_topic(
+            "/orchestrator/joint_state#sensor_msgs.JointState", timeout=10.0
+        )
+
+        client = RPCClient(None, ControlOrchestrator)
+        try:
+            # Create a long trajectory (5 seconds)
+            trajectory = JointTrajectory(
+                joint_names=[f"arm_joint{i + 1}" for i in range(7)],
+                points=[
+                    TrajectoryPoint(
+                        time_from_start=0.0,
+                        positions=[0.0] * 7,
+                        velocities=[0.0] * 7,
+                    ),
+                    TrajectoryPoint(
+                        time_from_start=5.0,
+                        positions=[1.0] * 7,
+                        velocities=[0.0] * 7,
+                    ),
+                ],
+            )
+
+            # Start trajectory
+            result = client.execute_trajectory("traj_arm", trajectory)
+            assert result is True
+
+            # Wait a bit then cancel
+            time.sleep(0.5)
+            cancel_result = client.cancel_trajectory("traj_arm")
+            assert cancel_result is True
+
+            # Check status is ABORTED
+            status = client.get_trajectory_status("traj_arm")
+            assert status is not None
+            assert status.get("state") == TrajectoryState.ABORTED.name
+        finally:
+            client.stop_rpc_client()
+
+    def test_dual_arm_orchestrator(self, lcm_spy, start_blueprint) -> None:
+        """Test dual-arm orchestrator with independent trajectories."""
+        lcm_spy.save_topic("/orchestrator/joint_state#sensor_msgs.JointState")
+
+        # Start dual-arm mock orchestrator
+        start_blueprint("orchestrator-dual-mock")
+        lcm_spy.wait_for_saved_topic(
+            "/orchestrator/joint_state#sensor_msgs.JointState", timeout=10.0
+        )
+
+        client = RPCClient(None, ControlOrchestrator)
+        try:
+            # Verify both arms present
+            joints = client.list_joints()
+            assert "left_joint1" in joints
+            assert "right_joint1" in joints
+
+            tasks = client.list_tasks()
+            assert "traj_left" in tasks
+            assert "traj_right" in tasks
+
+            # Create trajectories for both arms
+            left_trajectory = JointTrajectory(
+                joint_names=[f"left_joint{i + 1}" for i in range(7)],
+                points=[
+                    TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 7),
+                    TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 7),
+                ],
+            )
+
+            right_trajectory = JointTrajectory(
+                joint_names=[f"right_joint{i + 1}" for i in range(6)],
+                points=[
+                    TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 6),
+                    TrajectoryPoint(time_from_start=0.5, positions=[0.3] * 6),
+                ],
+            )
+
+            # Execute both
+            assert client.execute_trajectory("traj_left", left_trajectory) is True
+            assert client.execute_trajectory("traj_right", right_trajectory) is True
+
+            # Wait for completion
+            time.sleep(1.0)
+
+            # Both should complete
+            left_status = client.get_trajectory_status("traj_left")
+            right_status = client.get_trajectory_status("traj_right")
+
+            assert left_status.get("state") == TrajectoryState.COMPLETED.name
+            assert right_status.get("state") == TrajectoryState.COMPLETED.name
+        finally:
+            client.stop_rpc_client()
diff --git a/dimos/hardware/manipulators/README.md b/dimos/hardware/manipulators/README.md
index d4bb1cdba7..d3e54d4cb0 100644
--- a/dimos/hardware/manipulators/README.md
+++ b/dimos/hardware/manipulators/README.md
@@ -1,173 +1,163 @@
 # Manipulator Drivers
 
-Component-based framework for integrating robotic manipulators into DIMOS.
+This module provides manipulator arm drivers using the **B-lite architecture**: Protocol-only with injectable backends.
 
-## Quick Start: Adding a New Manipulator
-
-Adding support for a new robot arm requires **two files**:
-1. **SDK Wrapper** (~200-500 lines) - Translates vendor SDK to standard interface
-2. **Driver** (~30-50 lines) - Assembles components and configuration
-
-## Directory Structure
+## Architecture Overview
 
 ```
-manipulators/
-├── base/                      # Framework (don't modify)
-│   ├── sdk_interface.py       # BaseManipulatorSDK abstract class
-│   ├── driver.py              # BaseManipulatorDriver base class
-│   ├── spec.py                # ManipulatorCapabilities dataclass
-│   └── components/            # Reusable standard components
-├── xarm/                      # XArm implementation (reference)
-└── piper/                     # Piper implementation (reference)
+┌─────────────────────────────────────────────────────────────┐
+│                      Driver (Module)                        │
+│  - Owns threading (control loop, monitor loop)              │
+│  - Publishes joint_state, robot_state                       │
+│  - Subscribes to joint_position_command, joint_velocity_cmd │
+│  - Exposes RPC methods (move_joint, enable_servos, etc.)    │
+└─────────────────────┬───────────────────────────────────────┘
+                      │ uses
+┌─────────────────────▼───────────────────────────────────────┐
+│              Backend (implements Protocol)                   │
+│  - Handles SDK communication                                 │
+│  - Unit conversions (radians ↔ vendor units)                │
+│  - Swappable: XArmBackend, PiperBackend, MockBackend        │
+└─────────────────────────────────────────────────────────────┘
 ```
 
-## Hardware Requirements
+## Key Benefits
 
-Your manipulator **must** support:
+- **Testable**: Inject `MockBackend` 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
 
-| Requirement | Description |
-|-------------|-------------|
-| Joint Position Feedback | Read current joint angles |
-| Joint Position Control | Command target joint positions |
-| Servo Enable/Disable | Enable and disable motor power |
-| Error Reporting | Report error codes/states |
-| Emergency Stop | Hardware or software e-stop |
+## Directory Structure
 
-**Optional:** velocity control, torque control, cartesian control, F/T sensor, gripper
+```
+manipulators/
+├── spec.py              # ManipulatorBackend Protocol + shared types
+├── mock/
+│   └── backend.py       # MockBackend for testing
+├── xarm/
+│   ├── backend.py       # XArmBackend (SDK wrapper)
+│   ├── arm.py           # XArm driver module
+│   └── blueprints.py    # Pre-configured blueprints
+└── piper/
+    ├── backend.py       # PiperBackend (SDK wrapper)
+    ├── arm.py           # Piper driver module
+    └── blueprints.py    # Pre-configured blueprints
+```
 
-## Step 1: Implement SDK Wrapper
+## Quick Start
 
-Create `your_arm/your_arm_wrapper.py` implementing `BaseManipulatorSDK`:
+### Using a Driver Directly
 
 ```python
-from dimos.hardware.manipulators.base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo
-
-class YourArmSDKWrapper(BaseManipulatorSDK):
-    def __init__(self):
-        self._sdk = None
-
-    def connect(self, config: dict) -> bool:
-        self._sdk = YourNativeSDK(config['ip'])
-        return self._sdk.connect()
-
-    def get_joint_positions(self) -> list[float]:
-        """Return positions in RADIANS."""
-        degrees = self._sdk.get_angles()
-        return [math.radians(d) for d in degrees]
+from dimos.hardware.manipulators.xarm import XArm
 
-    def set_joint_positions(self, positions: list[float],
-                           velocity: float, acceleration: float) -> bool:
-        return self._sdk.move_joints(positions, velocity)
-
-    def enable_servos(self) -> bool:
-        return self._sdk.motor_on()
-
-    # ... implement remaining required methods (see sdk_interface.py)
+arm = XArm(ip="192.168.1.185", dof=6)
+arm.start()
+arm.enable_servos()
+arm.move_joint([0, 0, 0, 0, 0, 0])
+arm.stop()
 ```
 
-### Unit Conventions
-
-**All SDK wrappers must use these standard units:**
-
-| Quantity | Unit |
-|----------|------|
-| Joint positions | radians |
-| Joint velocities | rad/s |
-| Joint accelerations | rad/s^2 |
-| Joint torques | Nm |
-| Cartesian positions | meters |
-| Forces | N |
-
-## Step 2: Create Driver Assembly
-
-Create `your_arm/your_arm_driver.py`:
+### Using Blueprints
 
 ```python
-from dimos.hardware.manipulators.base.driver import BaseManipulatorDriver
-from dimos.hardware.manipulators.base.spec import ManipulatorCapabilities
-from dimos.hardware.manipulators.base.components import (
-    StandardMotionComponent,
-    StandardServoComponent,
-    StandardStatusComponent,
-)
-from .your_arm_wrapper import YourArmSDKWrapper
-
-class YourArmDriver(BaseManipulatorDriver):
-    def __init__(self, config: dict):
-        sdk = YourArmSDKWrapper()
-
-        capabilities = ManipulatorCapabilities(
-            dof=6,
-            has_gripper=False,
-            has_force_torque=False,
-            joint_limits_lower=[-3.14, -2.09, -3.14, -3.14, -3.14, -3.14],
-            joint_limits_upper=[3.14, 2.09, 3.14, 3.14, 3.14, 3.14],
-            max_joint_velocity=[2.0] * 6,
-            max_joint_acceleration=[4.0] * 6,
-        )
-
-        components = [
-            StandardMotionComponent(),
-            StandardServoComponent(),
-            StandardStatusComponent(),
-        ]
+from dimos.hardware.manipulators.xarm.blueprints import xarm_trajectory
 
-        super().__init__(sdk, components, config, capabilities)
+coordinator = xarm_trajectory.build()
+coordinator.loop()
 ```
 
-## Component API Decorator
-
-Use `@component_api` to expose methods as RPC endpoints:
+### Testing Without Hardware
 
 ```python
-from dimos.hardware.manipulators.base.components import component_api
+from dimos.hardware.manipulators.mock import MockBackend
+from dimos.hardware.manipulators.xarm import XArm
 
-class StandardMotionComponent:
-    @component_api
-    def move_joint(self, positions: list[float], velocity: float = 1.0):
-        """Auto-exposed as driver.move_joint()"""
-        ...
+arm = XArm(backend=MockBackend(dof=6))
+arm.start()  # No hardware needed!
+arm.move_joint([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])
 ```
 
-## Threading Architecture
+## Adding a New Arm
 
-The driver runs **2 threads**:
-1. **Control Loop (100Hz)** - Processes commands, reads joint state, publishes feedback
-2. **Monitor Loop (10Hz)** - Reads robot state, errors, optional sensors
+1. **Create the backend** (`backend.py`):
 
-```
-RPC Call → Command Queue → Control Loop → SDK → Hardware
-                              ↓
-                         SharedState → LCM Publisher
+```python
+class MyArmBackend:  # 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
+
+    def connect(self) -> bool: ...
+    def disconnect(self) -> None: ...
+    def read_joint_positions(self) -> list[float]: ...
+    def write_joint_positions(self, positions: list[float], velocity: float = 1.0) -> bool: ...
+    # ... implement other Protocol methods
 ```
 
-## Testing Your Driver
+2. **Create the driver** (`arm.py`):
 
 ```python
-driver = YourArmDriver({"ip": "192.168.1.100"})
-driver.start()
-driver.enable_servo()
-driver.move_joint([0, 0, 0, 0, 0, 0], velocity=0.5)
-state = driver.get_joint_state()
-driver.stop()
+from dimos.core import Module, ModuleConfig, In, Out, rpc
+from .backend import MyArmBackend
+
+class MyArm(Module[MyArmConfig]):
+    joint_state: Out[JointState]
+    robot_state: Out[RobotState]
+    joint_position_command: In[JointCommand]
+
+    def __init__(self, backend=None, **kwargs):
+        super().__init__(**kwargs)
+        self.backend = backend or MyArmBackend(
+            ip=self.config.ip,
+            dof=self.config.dof,
+        )
+        # ... setup control loops
 ```
 
-## Common Issues
+3. **Create blueprints** (`blueprints.py`) for common configurations.
 
-| Issue | Solution |
-|-------|----------|
-| Unit mismatch | Verify wrapper converts to radians/meters |
-| Commands ignored | Ensure servos are enabled before commanding |
-| Velocity not working | Some arms need mode switch via `set_control_mode()` |
+## ManipulatorBackend Protocol
 
-## Architecture Details
+All backends must implement these core methods:
 
-For complete architecture documentation including full SDK interface specification,
-component details, and testing strategies, see:
+| Category | Methods |
+|----------|---------|
+| Connection | `connect()`, `disconnect()`, `is_connected()` |
+| Info | `get_info()`, `get_dof()`, `get_limits()` |
+| State | `read_joint_positions()`, `read_joint_velocities()`, `read_joint_efforts()` |
+| Motion | `write_joint_positions()`, `write_joint_velocities()`, `write_stop()` |
+| Servo | `write_enable()`, `read_enabled()`, `write_clear_errors()` |
+| Mode | `set_control_mode()`, `get_control_mode()` |
 
-**[component_based_architecture.md](base/component_based_architecture.md)**
+Optional methods (return `None`/`False` if unsupported):
+- `read_cartesian_position()`, `write_cartesian_position()`
+- `read_gripper_position()`, `write_gripper_position()`
+- `read_force_torque()`
 
-## Reference Implementations
+## Unit Conventions
 
-- **XArm**: [xarm/xarm_wrapper.py](xarm/xarm_wrapper.py) - Full-featured wrapper
-- **Piper**: [piper/piper_wrapper.py](piper/piper_wrapper.py) - Shows velocity workaround
+All backends convert to/from SI units:
+
+| Quantity | Unit |
+|----------|------|
+| Angles | radians |
+| Angular velocity | rad/s |
+| 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 a54a846afc..e4133dbb51 100644
--- a/dimos/hardware/manipulators/__init__.py
+++ b/dimos/hardware/manipulators/__init__.py
@@ -12,10 +12,41 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-"""
-Manipulator Hardware Drivers
+"""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
 
-Drivers for various robotic manipulator arms.
+Usage:
+    >>> from dimos.hardware.manipulators.xarm import XArm
+    >>> arm = XArm(ip="192.168.1.185")
+    >>> arm.start()
+    >>> arm.enable_servos()
+    >>> arm.move_joint([0, 0, 0, 0, 0, 0])
+
+Testing:
+    >>> from dimos.hardware.manipulators.xarm import XArm
+    >>> from dimos.hardware.manipulators.mock import MockBackend
+    >>> arm = XArm(backend=MockBackend())
+    >>> arm.start()  # No hardware needed!
 """
 
-__all__ = []
+from dimos.hardware.manipulators.spec import (
+    ControlMode,
+    DriverStatus,
+    JointLimits,
+    ManipulatorBackend,
+    ManipulatorInfo,
+)
+
+__all__ = [
+    "ControlMode",
+    "DriverStatus",
+    "JointLimits",
+    "ManipulatorBackend",
+    "ManipulatorInfo",
+]
diff --git a/dimos/hardware/manipulators/base/__init__.py b/dimos/hardware/manipulators/base/__init__.py
deleted file mode 100644
index 3ed58d9819..0000000000
--- a/dimos/hardware/manipulators/base/__init__.py
+++ /dev/null
@@ -1,44 +0,0 @@
-# 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.
-
-"""Base framework for generalized manipulator drivers.
-
-This package provides the foundation for building manipulator drivers
-that work with any robotic arm (XArm, Piper, UR, Franka, etc.).
-"""
-
-from .components import StandardMotionComponent, StandardServoComponent, StandardStatusComponent
-from .driver import BaseManipulatorDriver, Command
-from .sdk_interface import BaseManipulatorSDK, ManipulatorInfo
-from .spec import ManipulatorCapabilities, ManipulatorDriverSpec, RobotState
-from .utils import SharedState
-
-__all__ = [
-    # Driver
-    "BaseManipulatorDriver",
-    # SDK Interface
-    "BaseManipulatorSDK",
-    "Command",
-    "ManipulatorCapabilities",
-    # Spec
-    "ManipulatorDriverSpec",
-    "ManipulatorInfo",
-    "RobotState",
-    # Utils
-    "SharedState",
-    # Components
-    "StandardMotionComponent",
-    "StandardServoComponent",
-    "StandardStatusComponent",
-]
diff --git a/dimos/hardware/manipulators/base/component_based_architecture.md b/dimos/hardware/manipulators/base/component_based_architecture.md
deleted file mode 100644
index 893ebf1276..0000000000
--- a/dimos/hardware/manipulators/base/component_based_architecture.md
+++ /dev/null
@@ -1,208 +0,0 @@
-# Component-Based Architecture for Manipulator Drivers
-
-## Overview
-
-This architecture provides maximum code reuse through standardized SDK wrappers and reusable components. Each new manipulator requires only an SDK wrapper (~200-500 lines) and a thin driver assembly (~30-50 lines).
-
-## Architecture Layers
-
-```
-┌─────────────────────────────────────────────────────┐
-│                   RPC Interface                      │
-│              (Standardized across all arms)          │
-└─────────────────────────────────────────────────────┘
-                           ▲
-┌─────────────────────────────────────────────────────┐
-│              Driver Instance (XArmDriver)            │
-│          Extends DIMOS Module, assembles components  │
-└─────────────────────────────────────────────────────┘
-                           ▲
-┌─────────────────────────────────────────────────────┐
-│              Standard Components                     │
-│        (Motion, Servo, Status) - reused everywhere   │
-└─────────────────────────────────────────────────────┘
-                           ▲
-┌─────────────────────────────────────────────────────┐
-│              SDK Wrapper (XArmSDKWrapper)            │
-│        Implements BaseManipulatorSDK interface       │
-└─────────────────────────────────────────────────────┘
-                           ▲
-┌─────────────────────────────────────────────────────┐
-│              Native Vendor SDK (XArmAPI)             │
-└─────────────────────────────────────────────────────┘
-```
-
-## Core Interfaces
-
-### BaseManipulatorSDK
-
-Abstract interface that all SDK wrappers must implement. See `sdk_interface.py` for full specification.
-
-**Required methods:** `connect()`, `disconnect()`, `is_connected()`, `get_joint_positions()`, `get_joint_velocities()`, `set_joint_positions()`, `enable_servos()`, `disable_servos()`, `emergency_stop()`, `get_error_code()`, `clear_errors()`, `get_info()`
-
-**Optional methods:** `get_force_torque()`, `get_gripper_position()`, `set_cartesian_position()`, etc.
-
-### ManipulatorCapabilities
-
-Dataclass defining arm properties: DOF, joint limits, velocity limits, feature flags.
-
-## Component System
-
-### @component_api Decorator
-
-Methods marked with `@component_api` are automatically exposed as RPC endpoints on the driver:
-
-```python
-from dimos.hardware.manipulators.base.components import component_api
-
-class StandardMotionComponent:
-    @component_api
-    def move_joint(self, positions: list[float], velocity: float = 1.0) -> dict:
-        """Auto-exposed as driver.move_joint()"""
-        ...
-```
-
-### Dependency Injection
-
-Components receive dependencies via setter methods, not constructor:
-
-```python
-class StandardMotionComponent:
-    def __init__(self):
-        self.sdk = None
-        self.shared_state = None
-        self.command_queue = None
-        self.capabilities = None
-
-    def set_sdk(self, sdk): self.sdk = sdk
-    def set_shared_state(self, state): self.shared_state = state
-    def set_command_queue(self, queue): self.command_queue = queue
-    def set_capabilities(self, caps): self.capabilities = caps
-    def initialize(self): pass  # Called after all setters
-```
-
-### Standard Components
-
-| Component | Purpose | Key Methods |
-|-----------|---------|-------------|
-| `StandardMotionComponent` | Joint/cartesian motion | `move_joint()`, `move_joint_velocity()`, `get_joint_state()`, `stop_motion()` |
-| `StandardServoComponent` | Motor control | `enable_servo()`, `disable_servo()`, `emergency_stop()`, `set_control_mode()` |
-| `StandardStatusComponent` | Monitoring | `get_robot_state()`, `get_error_state()`, `get_health_metrics()` |
-
-## Threading Model
-
-The driver runs **2 threads**:
-
-1. **Control Loop (100Hz)** - Process commands, read joint state, publish feedback
-2. **Monitor Loop (10Hz)** - Read robot state, errors, optional sensors (F/T, gripper)
-
-```
-RPC Call → Command Queue → Control Loop → SDK → Hardware
-                              ↓
-                         SharedState (thread-safe)
-                              ↓
-                         LCM Publisher → External Systems
-```
-
-## DIMOS Module Integration
-
-The driver extends `Module` for pub/sub integration:
-
-```python
-class BaseManipulatorDriver(Module):
-    def __init__(self, sdk, components, config, capabilities):
-        super().__init__()
-        self.shared_state = SharedState()
-        self.command_queue = Queue(maxsize=10)
-
-        # Inject dependencies into components
-        for component in components:
-            component.set_sdk(sdk)
-            component.set_shared_state(self.shared_state)
-            component.set_command_queue(self.command_queue)
-            component.set_capabilities(capabilities)
-            component.initialize()
-
-        # Auto-expose @component_api methods
-        self._auto_expose_component_apis()
-```
-
-## Adding a New Manipulator
-
-### Step 1: SDK Wrapper
-
-```python
-class YourArmSDKWrapper(BaseManipulatorSDK):
-    def get_joint_positions(self) -> list[float]:
-        degrees = self._sdk.get_angles()
-        return [math.radians(d) for d in degrees]  # Convert to radians
-
-    def set_joint_positions(self, positions, velocity, acceleration) -> bool:
-        return self._sdk.move_joints(positions, velocity)
-
-    # ... implement remaining required methods
-```
-
-### Step 2: Driver Assembly
-
-```python
-class YourArmDriver(BaseManipulatorDriver):
-    def __init__(self, config: dict):
-        sdk = YourArmSDKWrapper()
-        capabilities = ManipulatorCapabilities(
-            dof=6,
-            joint_limits_lower=[-3.14] * 6,
-            joint_limits_upper=[3.14] * 6,
-        )
-        components = [
-            StandardMotionComponent(),
-            StandardServoComponent(),
-            StandardStatusComponent(),
-        ]
-        super().__init__(sdk, components, config, capabilities)
-```
-
-## Unit Conventions
-
-All SDK wrappers must convert to standard units:
-
-| Quantity | Unit |
-|----------|------|
-| Positions | radians |
-| Velocities | rad/s |
-| Accelerations | rad/s^2 |
-| Torques | Nm |
-| Cartesian | meters |
-
-## Testing Strategy
-
-```python
-# Test SDK wrapper with mocked native SDK
-def test_wrapper_positions():
-    mock = Mock()
-    mock.get_angles.return_value = [0, 90, 180]
-    wrapper = YourArmSDKWrapper()
-    wrapper._sdk = mock
-    assert wrapper.get_joint_positions() == [0, math.pi/2, math.pi]
-
-# Test component with mocked SDK wrapper
-def test_motion_component():
-    mock_sdk = Mock(spec=BaseManipulatorSDK)
-    component = StandardMotionComponent()
-    component.set_sdk(mock_sdk)
-    component.move_joint([0, 0, 0])
-    # Verify command was queued
-```
-
-## Advantages
-
-- **Maximum reuse**: Components tested once, used by 100+ arms
-- **Consistent behavior**: All arms identical at RPC level
-- **Centralized fixes**: Fix once in component, all arms benefit
-- **Team scalability**: Developers work on wrappers independently
-- **Strong contracts**: SDK interface defines exact requirements
-
-## Reference Implementations
-
-- **XArm**: `xarm/xarm_wrapper.py` - Full-featured, converts degrees→radians
-- **Piper**: `piper/piper_wrapper.py` - Shows velocity integration workaround
diff --git a/dimos/hardware/manipulators/base/components/__init__.py b/dimos/hardware/manipulators/base/components/__init__.py
deleted file mode 100644
index b04f60f691..0000000000
--- a/dimos/hardware/manipulators/base/components/__init__.py
+++ /dev/null
@@ -1,59 +0,0 @@
-# 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.
-
-"""Standard components for manipulator drivers."""
-
-from collections.abc import Callable
-from typing import Any, TypeVar
-
-F = TypeVar("F", bound=Callable[..., Any])
-
-
-def component_api(fn: F) -> F:
-    """Decorator to mark component methods that should be exposed as driver RPCs.
-
-    Methods decorated with @component_api will be automatically discovered by the
-    driver and exposed as @rpc methods on the driver instance. This allows external
-    code to call these methods via the standard Module RPC system.
-
-    Example:
-        class MyComponent:
-            @component_api
-            def enable_servo(self):
-                '''Enable servo motors.'''
-                return self.sdk.enable_servos()
-
-        # The driver will auto-generate:
-        # @rpc
-        # def enable_servo(self):
-        #     return component.enable_servo()
-
-        # External code can then call:
-        # driver.enable_servo()
-    """
-    fn.__component_api__ = True  # type: ignore[attr-defined]
-    return fn
-
-
-# Import components AFTER defining component_api to avoid circular imports
-from .motion import StandardMotionComponent
-from .servo import StandardServoComponent
-from .status import StandardStatusComponent
-
-__all__ = [
-    "StandardMotionComponent",
-    "StandardServoComponent",
-    "StandardStatusComponent",
-    "component_api",
-]
diff --git a/dimos/hardware/manipulators/base/components/motion.py b/dimos/hardware/manipulators/base/components/motion.py
deleted file mode 100644
index f3205acb01..0000000000
--- a/dimos/hardware/manipulators/base/components/motion.py
+++ /dev/null
@@ -1,591 +0,0 @@
-# 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.
-
-"""Standard motion control component for manipulator drivers."""
-
-import logging
-from queue import Queue
-import time
-from typing import Any
-
-from ..driver import Command
-from ..sdk_interface import BaseManipulatorSDK
-from ..spec import ManipulatorCapabilities
-from ..utils import SharedState, scale_velocities, validate_joint_limits, validate_velocity_limits
-from . import component_api
-
-
-class StandardMotionComponent:
-    """Motion control component that works with any SDK wrapper.
-
-    This component provides standard motion control methods that work
-    consistently across all manipulator types. Methods decorated with @component_api
-    are automatically exposed as RPC methods on the driver. It handles:
-    - Joint position control
-    - Joint velocity control
-    - Joint effort/torque control (if supported)
-    - Trajectory execution (if supported)
-    - Motion safety validation
-    """
-
-    def __init__(
-        self,
-        sdk: BaseManipulatorSDK | None = None,
-        shared_state: SharedState | None = None,
-        command_queue: Queue[Any] | None = None,
-        capabilities: ManipulatorCapabilities | None = None,
-    ) -> None:
-        """Initialize the motion component.
-
-        Args:
-            sdk: SDK wrapper instance (can be set later)
-            shared_state: Shared state instance (can be set later)
-            command_queue: Command queue (can be set later)
-            capabilities: Manipulator capabilities (can be set later)
-        """
-        self.sdk = sdk
-        self.shared_state = shared_state
-        self.command_queue = command_queue
-        self.capabilities = capabilities
-        self.logger = logging.getLogger(self.__class__.__name__)
-
-        # Motion limits
-        self.velocity_scale = 1.0  # Global velocity scaling (0-1)
-        self.acceleration_scale = 1.0  # Global acceleration scaling (0-1)
-
-    # ============= Initialization Methods (called by BaseDriver) =============
-
-    def set_sdk(self, sdk: BaseManipulatorSDK) -> None:
-        """Set the SDK wrapper instance."""
-        self.sdk = sdk
-
-    def set_shared_state(self, shared_state: SharedState) -> None:
-        """Set the shared state instance."""
-        self.shared_state = shared_state
-
-    def set_command_queue(self, command_queue: "Queue[Any]") -> None:
-        """Set the command queue instance."""
-        self.command_queue = command_queue
-
-    def set_capabilities(self, capabilities: ManipulatorCapabilities) -> None:
-        """Set the capabilities instance."""
-        self.capabilities = capabilities
-
-    def initialize(self) -> None:
-        """Initialize the component after all resources are set."""
-        self.logger.debug("Motion component initialized")
-
-    # ============= Component API Methods =============
-
-    @component_api
-    def move_joint(
-        self,
-        positions: list[float],
-        velocity: float = 1.0,
-        acceleration: float = 1.0,
-        wait: bool = False,
-        validate: bool = True,
-    ) -> dict[str, Any]:
-        """Move joints to target positions.
-
-        Args:
-            positions: Target joint positions in radians
-            velocity: Velocity scaling factor (0-1)
-            acceleration: Acceleration scaling factor (0-1)
-            wait: If True, block until motion completes
-            validate: If True, validate against joint limits
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            # Validate inputs
-            if validate and self.capabilities:
-                if len(positions) != self.capabilities.dof:
-                    return {
-                        "success": False,
-                        "error": f"Expected {self.capabilities.dof} positions, got {len(positions)}",
-                    }
-
-                # Check joint limits
-                if self.capabilities.joint_limits_lower and self.capabilities.joint_limits_upper:
-                    valid, error = validate_joint_limits(
-                        positions,
-                        self.capabilities.joint_limits_lower,
-                        self.capabilities.joint_limits_upper,
-                    )
-                    if not valid:
-                        return {"success": False, "error": error}
-
-            # Apply global scaling
-            velocity = velocity * self.velocity_scale
-            acceleration = acceleration * self.acceleration_scale
-
-            # Queue command for async execution
-            if self.command_queue and not wait:
-                command = Command(
-                    type="position",
-                    data={
-                        "positions": positions,
-                        "velocity": velocity,
-                        "acceleration": acceleration,
-                        "wait": False,
-                    },
-                    timestamp=time.time(),
-                )
-                self.command_queue.put(command)
-                return {"success": True, "queued": True}
-
-            # Execute directly (blocking or wait mode)
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-            success = self.sdk.set_joint_positions(positions, velocity, acceleration, wait)
-
-            if success and self.shared_state:
-                self.shared_state.set_target_joints(positions=positions)
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in move_joint: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def move_joint_velocity(
-        self, velocities: list[float], acceleration: float = 1.0, validate: bool = True
-    ) -> dict[str, Any]:
-        """Set joint velocities.
-
-        Args:
-            velocities: Target joint velocities in rad/s
-            acceleration: Acceleration scaling factor (0-1)
-            validate: If True, validate against velocity limits
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            # Validate inputs
-            if validate and self.capabilities:
-                if len(velocities) != self.capabilities.dof:
-                    return {
-                        "success": False,
-                        "error": f"Expected {self.capabilities.dof} velocities, got {len(velocities)}",
-                    }
-
-                # Check velocity limits
-                if self.capabilities.max_joint_velocity:
-                    valid, _error = validate_velocity_limits(
-                        velocities, self.capabilities.max_joint_velocity, self.velocity_scale
-                    )
-                    if not valid:
-                        # Scale velocities to stay within limits
-                        velocities = scale_velocities(
-                            velocities, self.capabilities.max_joint_velocity, self.velocity_scale
-                        )
-                        self.logger.warning("Velocities scaled to stay within limits")
-
-            # Queue command for async execution
-            if self.command_queue:
-                command = Command(
-                    type="velocity", data={"velocities": velocities}, timestamp=time.time()
-                )
-                self.command_queue.put(command)
-                return {"success": True, "queued": True}
-
-            # Execute directly
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-            success = self.sdk.set_joint_velocities(velocities)
-
-            if success and self.shared_state:
-                self.shared_state.set_target_joints(velocities=velocities)
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in move_joint_velocity: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def move_joint_effort(self, efforts: list[float], validate: bool = True) -> dict[str, Any]:
-        """Set joint efforts/torques.
-
-        Args:
-            efforts: Target joint efforts in Nm
-            validate: If True, validate inputs
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            # Check if effort control is supported
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-            if not hasattr(self.sdk, "set_joint_efforts"):
-                return {"success": False, "error": "Effort control not supported"}
-
-            # Validate inputs
-            if validate and self.capabilities:
-                if len(efforts) != self.capabilities.dof:
-                    return {
-                        "success": False,
-                        "error": f"Expected {self.capabilities.dof} efforts, got {len(efforts)}",
-                    }
-
-            # Queue command for async execution
-            if self.command_queue:
-                command = Command(type="effort", data={"efforts": efforts}, timestamp=time.time())
-                self.command_queue.put(command)
-                return {"success": True, "queued": True}
-
-            # Execute directly
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-            success = self.sdk.set_joint_efforts(efforts)
-
-            if success and self.shared_state:
-                self.shared_state.set_target_joints(efforts=efforts)
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in move_joint_effort: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def stop_motion(self) -> dict[str, Any]:
-        """Stop all ongoing motion immediately.
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            # Queue stop command with high priority
-            if self.command_queue:
-                command = Command(type="stop", data={}, timestamp=time.time())
-                # Clear queue and add stop command
-                while not self.command_queue.empty():
-                    try:
-                        self.command_queue.get_nowait()
-                    except:
-                        break
-                self.command_queue.put(command)
-
-            # Also execute directly for immediate stop
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-            success = self.sdk.stop_motion()
-
-            # Clear targets
-            if self.shared_state:
-                self.shared_state.set_target_joints(positions=None, velocities=None, efforts=None)
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in stop_motion: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def get_joint_state(self) -> dict[str, Any]:
-        """Get current joint state.
-
-        Returns:
-            Dict with joint positions, velocities, efforts, and timestamp
-        """
-        try:
-            if self.shared_state:
-                # Get from shared state (updated by reader thread)
-                positions, velocities, efforts = self.shared_state.get_joint_state()
-            else:
-                # Get directly from SDK
-                if self.sdk is None:
-                    return {"success": False, "error": "SDK not configured"}
-                positions = self.sdk.get_joint_positions()
-                velocities = self.sdk.get_joint_velocities()
-                efforts = self.sdk.get_joint_efforts()
-
-            return {
-                "positions": positions,
-                "velocities": velocities,
-                "efforts": efforts,
-                "timestamp": time.time(),
-                "success": True,
-            }
-
-        except Exception as e:
-            self.logger.error(f"Error in get_joint_state: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def get_joint_limits(self) -> dict[str, Any]:
-        """Get joint position limits.
-
-        Returns:
-            Dict with lower and upper limits in radians
-        """
-        try:
-            if self.capabilities:
-                return {
-                    "lower": self.capabilities.joint_limits_lower,
-                    "upper": self.capabilities.joint_limits_upper,
-                    "success": True,
-                }
-            else:
-                if self.sdk is None:
-                    return {"success": False, "error": "SDK not configured"}
-                lower, upper = self.sdk.get_joint_limits()
-                return {"lower": lower, "upper": upper, "success": True}
-
-        except Exception as e:
-            self.logger.error(f"Error in get_joint_limits: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def get_velocity_limits(self) -> dict[str, Any]:
-        """Get joint velocity limits.
-
-        Returns:
-            Dict with maximum velocities in rad/s
-        """
-        try:
-            if self.capabilities:
-                return {"limits": self.capabilities.max_joint_velocity, "success": True}
-            else:
-                if self.sdk is None:
-                    return {"success": False, "error": "SDK not configured"}
-                limits = self.sdk.get_velocity_limits()
-                return {"limits": limits, "success": True}
-
-        except Exception as e:
-            self.logger.error(f"Error in get_velocity_limits: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def set_velocity_scale(self, scale: float) -> dict[str, Any]:
-        """Set global velocity scaling factor.
-
-        Args:
-            scale: Velocity scale factor (0-1)
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            if scale <= 0 or scale > 1:
-                return {"success": False, "error": f"Invalid scale {scale}, must be in (0, 1]"}
-
-            self.velocity_scale = scale
-            return {"success": True, "scale": scale}
-
-        except Exception as e:
-            self.logger.error(f"Error in set_velocity_scale: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def set_acceleration_scale(self, scale: float) -> dict[str, Any]:
-        """Set global acceleration scaling factor.
-
-        Args:
-            scale: Acceleration scale factor (0-1)
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            if scale <= 0 or scale > 1:
-                return {"success": False, "error": f"Invalid scale {scale}, must be in (0, 1]"}
-
-            self.acceleration_scale = scale
-            return {"success": True, "scale": scale}
-
-        except Exception as e:
-            self.logger.error(f"Error in set_acceleration_scale: {e}")
-            return {"success": False, "error": str(e)}
-
-    # ============= Cartesian Control (Optional) =============
-
-    @component_api
-    def move_cartesian(
-        self,
-        pose: dict[str, float],
-        velocity: float = 1.0,
-        acceleration: float = 1.0,
-        wait: bool = False,
-    ) -> dict[str, Any]:
-        """Move end-effector to target pose.
-
-        Args:
-            pose: Target pose with keys: x, y, z (meters), roll, pitch, yaw (radians)
-            velocity: Velocity scaling factor (0-1)
-            acceleration: Acceleration scaling factor (0-1)
-            wait: If True, block until motion completes
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            # Check if Cartesian control is supported
-            if not self.capabilities or not self.capabilities.has_cartesian_control:
-                return {"success": False, "error": "Cartesian control not supported"}
-
-            # Apply global scaling
-            velocity = velocity * self.velocity_scale
-            acceleration = acceleration * self.acceleration_scale
-
-            # Queue command for async execution
-            if self.command_queue and not wait:
-                command = Command(
-                    type="cartesian",
-                    data={
-                        "pose": pose,
-                        "velocity": velocity,
-                        "acceleration": acceleration,
-                        "wait": False,
-                    },
-                    timestamp=time.time(),
-                )
-                self.command_queue.put(command)
-                return {"success": True, "queued": True}
-
-            # Execute directly
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-            success = self.sdk.set_cartesian_position(pose, velocity, acceleration, wait)
-
-            if success and self.shared_state:
-                self.shared_state.target_cartesian_position = pose
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in move_cartesian: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def get_cartesian_state(self) -> dict[str, Any]:
-        """Get current end-effector pose.
-
-        Returns:
-            Dict with pose (x, y, z, roll, pitch, yaw) and timestamp
-        """
-        try:
-            # Check if Cartesian control is supported
-            if not self.capabilities or not self.capabilities.has_cartesian_control:
-                return {"success": False, "error": "Cartesian control not supported"}
-
-            pose: dict[str, float] | None = None
-            if self.shared_state and self.shared_state.cartesian_position:
-                # Get from shared state
-                pose = self.shared_state.cartesian_position
-            else:
-                # Get directly from SDK
-                if self.sdk is None:
-                    return {"success": False, "error": "SDK not configured"}
-                pose = self.sdk.get_cartesian_position()
-
-            if pose:
-                return {"pose": pose, "timestamp": time.time(), "success": True}
-            else:
-                return {"success": False, "error": "Failed to get Cartesian state"}
-
-        except Exception as e:
-            self.logger.error(f"Error in get_cartesian_state: {e}")
-            return {"success": False, "error": str(e)}
-
-    # ============= Trajectory Execution (Optional) =============
-
-    @component_api
-    def execute_trajectory(
-        self, trajectory: list[dict[str, Any]], wait: bool = True
-    ) -> dict[str, Any]:
-        """Execute a joint trajectory.
-
-        Args:
-            trajectory: List of waypoints, each with:
-                       - 'positions': list[float] in radians
-                       - 'velocities': Optional list[float] in rad/s
-                       - 'time': float seconds from start
-            wait: If True, block until trajectory completes
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            # Check if trajectory execution is supported
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-            if not hasattr(self.sdk, "execute_trajectory"):
-                return {"success": False, "error": "Trajectory execution not supported"}
-
-            # Validate trajectory if capabilities available
-            if self.capabilities:
-                from ..utils import validate_trajectory
-
-                # Only validate if all required capability fields are present
-                jl_lower = self.capabilities.joint_limits_lower
-                jl_upper = self.capabilities.joint_limits_upper
-                max_vel = self.capabilities.max_joint_velocity
-                max_acc = self.capabilities.max_joint_acceleration
-
-                if (
-                    jl_lower is not None
-                    and jl_upper is not None
-                    and max_vel is not None
-                    and max_acc is not None
-                ):
-                    valid, error = validate_trajectory(
-                        trajectory,
-                        jl_lower,
-                        jl_upper,
-                        max_vel,
-                        max_acc,
-                    )
-                    if not valid:
-                        return {"success": False, "error": error}
-                else:
-                    self.logger.debug("Skipping trajectory validation; capabilities incomplete")
-
-            # Execute trajectory
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-            success = self.sdk.execute_trajectory(trajectory, wait)
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in execute_trajectory: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def stop_trajectory(self) -> dict[str, Any]:
-        """Stop any executing trajectory.
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            # Check if trajectory execution is supported
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-            if not hasattr(self.sdk, "stop_trajectory"):
-                return {"success": False, "error": "Trajectory execution not supported"}
-
-            success = self.sdk.stop_trajectory()
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in stop_trajectory: {e}")
-            return {"success": False, "error": str(e)}
diff --git a/dimos/hardware/manipulators/base/components/servo.py b/dimos/hardware/manipulators/base/components/servo.py
deleted file mode 100644
index c773f10723..0000000000
--- a/dimos/hardware/manipulators/base/components/servo.py
+++ /dev/null
@@ -1,522 +0,0 @@
-# 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.
-
-"""Standard servo control component for manipulator drivers."""
-
-import logging
-import time
-from typing import Any
-
-from ..sdk_interface import BaseManipulatorSDK
-from ..spec import ManipulatorCapabilities
-from ..utils import SharedState
-from . import component_api
-
-
-class StandardServoComponent:
-    """Servo control component that works with any SDK wrapper.
-
-    This component provides standard servo/motor control methods that work
-    consistently across all manipulator types. Methods decorated with @component_api
-    are automatically exposed as RPC methods on the driver. It handles:
-    - Servo enable/disable
-    - Control mode switching
-    - Emergency stop
-    - Error recovery
-    - Homing operations
-    """
-
-    def __init__(
-        self,
-        sdk: BaseManipulatorSDK | None = None,
-        shared_state: SharedState | None = None,
-        capabilities: ManipulatorCapabilities | None = None,
-    ):
-        """Initialize the servo component.
-
-        Args:
-            sdk: SDK wrapper instance (can be set later)
-            shared_state: Shared state instance (can be set later)
-            capabilities: Manipulator capabilities (can be set later)
-        """
-        self.sdk = sdk
-        self.shared_state = shared_state
-        self.capabilities = capabilities
-        self.logger = logging.getLogger(self.__class__.__name__)
-
-        # State tracking
-        self.last_enable_time = 0.0
-        self.last_disable_time = 0.0
-
-    # ============= Initialization Methods (called by BaseDriver) =============
-
-    def set_sdk(self, sdk: BaseManipulatorSDK) -> None:
-        """Set the SDK wrapper instance."""
-        self.sdk = sdk
-
-    def set_shared_state(self, shared_state: SharedState) -> None:
-        """Set the shared state instance."""
-        self.shared_state = shared_state
-
-    def set_capabilities(self, capabilities: ManipulatorCapabilities) -> None:
-        """Set the capabilities instance."""
-        self.capabilities = capabilities
-
-    def initialize(self) -> None:
-        """Initialize the component after all resources are set."""
-        self.logger.debug("Servo component initialized")
-
-    # ============= Component API Methods =============
-
-    @component_api
-    def enable_servo(self, check_errors: bool = True) -> dict[str, Any]:
-        """Enable servo/motor control.
-
-        Args:
-            check_errors: If True, check for errors before enabling
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            # Check if already enabled
-            if self.sdk.are_servos_enabled():
-                return {"success": True, "message": "Servos already enabled"}
-
-            # Check for errors if requested
-            if check_errors:
-                error_code = self.sdk.get_error_code()
-                if error_code != 0:
-                    error_msg = self.sdk.get_error_message()
-                    return {
-                        "success": False,
-                        "error": f"Cannot enable servos with active error: {error_msg} (code: {error_code})",
-                    }
-
-            # Enable servos
-            success = self.sdk.enable_servos()
-
-            if success:
-                self.last_enable_time = time.time()
-                if self.shared_state:
-                    self.shared_state.is_enabled = True
-                self.logger.info("Servos enabled successfully")
-            else:
-                self.logger.error("Failed to enable servos")
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in enable_servo: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def disable_servo(self, stop_motion: bool = True) -> dict[str, Any]:
-        """Disable servo/motor control.
-
-        Args:
-            stop_motion: If True, stop any ongoing motion first
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            # Check if already disabled
-            if not self.sdk.are_servos_enabled():
-                return {"success": True, "message": "Servos already disabled"}
-
-            # Stop motion if requested
-            if stop_motion:
-                self.sdk.stop_motion()
-                time.sleep(0.1)  # Brief delay to ensure motion stopped
-
-            # Disable servos
-            success = self.sdk.disable_servos()
-
-            if success:
-                self.last_disable_time = time.time()
-                if self.shared_state:
-                    self.shared_state.is_enabled = False
-                self.logger.info("Servos disabled successfully")
-            else:
-                self.logger.error("Failed to disable servos")
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in disable_servo: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def toggle_servo(self) -> dict[str, Any]:
-        """Toggle servo enable/disable state.
-
-        Returns:
-            Dict with 'success', 'enabled' state, and optional 'error' keys
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            current_state = self.sdk.are_servos_enabled()
-
-            if current_state:
-                result = self.disable_servo()
-            else:
-                result = self.enable_servo()
-
-            if result["success"]:
-                result["enabled"] = not current_state
-
-            return result
-
-        except Exception as e:
-            self.logger.error(f"Error in toggle_servo: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def get_servo_state(self) -> dict[str, Any]:
-        """Get current servo state.
-
-        Returns:
-            Dict with servo state information
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            enabled = self.sdk.are_servos_enabled()
-            robot_state = self.sdk.get_robot_state()
-
-            return {
-                "enabled": enabled,
-                "mode": robot_state.get("mode", 0),
-                "state": robot_state.get("state", 0),
-                "is_moving": robot_state.get("is_moving", False),
-                "last_enable_time": self.last_enable_time,
-                "last_disable_time": self.last_disable_time,
-                "success": True,
-            }
-
-        except Exception as e:
-            self.logger.error(f"Error in get_servo_state: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def emergency_stop(self) -> dict[str, Any]:
-        """Execute emergency stop.
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            # Execute e-stop
-            success = self.sdk.emergency_stop()
-
-            if success:
-                # Update shared state
-                if self.shared_state:
-                    self.shared_state.update_robot_state(state=3)  # 3 = e-stop state
-                    self.shared_state.is_enabled = False
-                    self.shared_state.is_moving = False
-
-                self.logger.warning("Emergency stop executed")
-            else:
-                self.logger.error("Failed to execute emergency stop")
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in emergency_stop: {e}")
-            # Try to stop motion as fallback
-            try:
-                if self.sdk is not None:
-                    self.sdk.stop_motion()
-            except:
-                pass
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def reset_emergency_stop(self) -> dict[str, Any]:
-        """Reset from emergency stop state.
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            # Clear errors first
-            self.sdk.clear_errors()
-
-            # Re-enable servos
-            success = self.sdk.enable_servos()
-
-            if success:
-                if self.shared_state:
-                    self.shared_state.update_robot_state(state=0)  # 0 = idle
-                    self.shared_state.is_enabled = True
-
-                self.logger.info("Emergency stop reset successfully")
-            else:
-                self.logger.error("Failed to reset emergency stop")
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in reset_emergency_stop: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def set_control_mode(self, mode: str) -> dict[str, Any]:
-        """Set control mode.
-
-        Args:
-            mode: Control mode ('position', 'velocity', 'torque', 'impedance')
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            # Validate mode
-            valid_modes = ["position", "velocity", "torque", "impedance"]
-            if mode not in valid_modes:
-                return {
-                    "success": False,
-                    "error": f"Invalid mode '{mode}'. Valid modes: {valid_modes}",
-                }
-
-            # Check if mode is supported
-            if mode == "impedance" and self.capabilities:
-                if not self.capabilities.has_impedance_control:
-                    return {"success": False, "error": "Impedance control not supported"}
-
-            # Set control mode
-            success = self.sdk.set_control_mode(mode)
-
-            if success:
-                # Map mode string to integer
-                mode_map = {"position": 0, "velocity": 1, "torque": 2, "impedance": 3}
-                if self.shared_state:
-                    self.shared_state.update_robot_state(mode=mode_map.get(mode, 0))
-
-                self.logger.info(f"Control mode set to '{mode}'")
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in set_control_mode: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def get_control_mode(self) -> dict[str, Any]:
-        """Get current control mode.
-
-        Returns:
-            Dict with current mode and success status
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            mode = self.sdk.get_control_mode()
-
-            if mode:
-                return {"mode": mode, "success": True}
-            else:
-                # Try to get from robot state
-                robot_state = self.sdk.get_robot_state()
-                mode_int = robot_state.get("mode", 0)
-
-                # Map integer to string
-                mode_map = {0: "position", 1: "velocity", 2: "torque", 3: "impedance"}
-                mode_str = mode_map.get(mode_int, "unknown")
-
-                return {"mode": mode_str, "success": True}
-
-        except Exception as e:
-            self.logger.error(f"Error in get_control_mode: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def clear_errors(self) -> dict[str, Any]:
-        """Clear any error states.
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            # Clear errors via SDK
-            success = self.sdk.clear_errors()
-
-            if success:
-                # Update shared state
-                if self.shared_state:
-                    self.shared_state.clear_errors()
-
-                self.logger.info("Errors cleared successfully")
-            else:
-                self.logger.error("Failed to clear errors")
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in clear_errors: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def reset_fault(self) -> dict[str, Any]:
-        """Reset from fault state.
-
-        This typically involves:
-        1. Clearing errors
-        2. Disabling servos
-        3. Brief delay
-        4. Re-enabling servos
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            self.logger.info("Resetting fault state...")
-
-            # Step 1: Clear errors
-            if not self.sdk.clear_errors():
-                return {"success": False, "error": "Failed to clear errors"}
-
-            # Step 2: Disable servos if enabled
-            if self.sdk.are_servos_enabled():
-                if not self.sdk.disable_servos():
-                    return {"success": False, "error": "Failed to disable servos"}
-
-            # Step 3: Brief delay
-            time.sleep(0.5)
-
-            # Step 4: Re-enable servos
-            if not self.sdk.enable_servos():
-                return {"success": False, "error": "Failed to re-enable servos"}
-
-            # Update shared state
-            if self.shared_state:
-                self.shared_state.update_robot_state(
-                    state=0,  # idle
-                    error_code=0,
-                    error_message="",
-                )
-                self.shared_state.is_enabled = True
-
-            self.logger.info("Fault reset successfully")
-            return {"success": True}
-
-        except Exception as e:
-            self.logger.error(f"Error in reset_fault: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def home_robot(self, position: list[float] | None = None) -> dict[str, Any]:
-        """Move robot to home position.
-
-        Args:
-            position: Optional home position in radians.
-                     If None, uses zero position or configured home.
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            # Determine home position
-            if position is None:
-                # Use configured home or zero position
-                if self.capabilities:
-                    position = [0.0] * self.capabilities.dof
-                else:
-                    # Get current DOF from joint state
-                    current = self.sdk.get_joint_positions()
-                    position = [0.0] * len(current)
-
-            # Enable servos if needed
-            if not self.sdk.are_servos_enabled():
-                if not self.sdk.enable_servos():
-                    return {"success": False, "error": "Failed to enable servos"}
-
-            # Move to home position
-            success = self.sdk.set_joint_positions(
-                position,
-                velocity=0.3,  # Slower speed for homing
-                acceleration=0.3,
-                wait=True,  # Wait for completion
-            )
-
-            if success:
-                if self.shared_state:
-                    self.shared_state.is_homed = True
-                self.logger.info("Robot homed successfully")
-
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in home_robot: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def brake_release(self) -> dict[str, Any]:
-        """Release motor brakes (if applicable).
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            # This is typically the same as enabling servos
-            return self.enable_servo()
-
-        except Exception as e:
-            self.logger.error(f"Error in brake_release: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def brake_engage(self) -> dict[str, Any]:
-        """Engage motor brakes (if applicable).
-
-        Returns:
-            Dict with 'success' and optional 'error' keys
-        """
-        try:
-            # This is typically the same as disabling servos
-            return self.disable_servo()
-
-        except Exception as e:
-            self.logger.error(f"Error in brake_engage: {e}")
-            return {"success": False, "error": str(e)}
diff --git a/dimos/hardware/manipulators/base/components/status.py b/dimos/hardware/manipulators/base/components/status.py
deleted file mode 100644
index b20897ac65..0000000000
--- a/dimos/hardware/manipulators/base/components/status.py
+++ /dev/null
@@ -1,595 +0,0 @@
-# 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.
-
-"""Standard status monitoring component for manipulator drivers."""
-
-from collections import deque
-from dataclasses import dataclass
-import logging
-import time
-from typing import Any
-
-from ..sdk_interface import BaseManipulatorSDK
-from ..spec import ManipulatorCapabilities
-from ..utils import SharedState
-from . import component_api
-
-
-@dataclass
-class HealthMetrics:
-    """Health metrics for monitoring."""
-
-    update_rate: float = 0.0  # Hz
-    command_rate: float = 0.0  # Hz
-    error_rate: float = 0.0  # errors/minute
-    uptime: float = 0.0  # seconds
-    total_errors: int = 0
-    total_commands: int = 0
-    total_updates: int = 0
-
-
-class StandardStatusComponent:
-    """Status monitoring component that works with any SDK wrapper.
-
-    This component provides standard status monitoring methods that work
-    consistently across all manipulator types. Methods decorated with @component_api
-    are automatically exposed as RPC methods on the driver. It handles:
-    - Robot state queries
-    - Error monitoring
-    - Health metrics
-    - System information
-    - Force/torque monitoring (if supported)
-    - Temperature monitoring (if supported)
-    """
-
-    def __init__(
-        self,
-        sdk: BaseManipulatorSDK | None = None,
-        shared_state: SharedState | None = None,
-        capabilities: ManipulatorCapabilities | None = None,
-    ):
-        """Initialize the status component.
-
-        Args:
-            sdk: SDK wrapper instance (can be set later)
-            shared_state: Shared state instance (can be set later)
-            capabilities: Manipulator capabilities (can be set later)
-        """
-        self.sdk = sdk
-        self.shared_state = shared_state
-        self.capabilities = capabilities
-        self.logger = logging.getLogger(self.__class__.__name__)
-
-        # Health monitoring
-        self.start_time = time.time()
-        self.health_metrics = HealthMetrics()
-
-        # Rate calculation
-        self.update_timestamps: deque[float] = deque(maxlen=100)
-        self.command_timestamps: deque[float] = deque(maxlen=100)
-        self.error_timestamps: deque[float] = deque(maxlen=100)
-
-        # Error history
-        self.error_history: deque[dict[str, Any]] = deque(maxlen=50)
-
-    # ============= Initialization Methods (called by BaseDriver) =============
-
-    def set_sdk(self, sdk: BaseManipulatorSDK) -> None:
-        """Set the SDK wrapper instance."""
-        self.sdk = sdk
-
-    def set_shared_state(self, shared_state: SharedState) -> None:
-        """Set the shared state instance."""
-        self.shared_state = shared_state
-
-    def set_capabilities(self, capabilities: ManipulatorCapabilities) -> None:
-        """Set the capabilities instance."""
-        self.capabilities = capabilities
-
-    def initialize(self) -> None:
-        """Initialize the component after all resources are set."""
-        self.start_time = time.time()
-        self.logger.debug("Status component initialized")
-
-    def publish_state(self) -> None:
-        """Called periodically to update metrics (by publisher thread)."""
-        current_time = time.time()
-        self.update_timestamps.append(current_time)
-        self._update_health_metrics()
-
-    # ============= Component API Methods =============
-
-    @component_api
-    def get_robot_state(self) -> dict[str, Any]:
-        """Get comprehensive robot state.
-
-        Returns:
-            Dict with complete state information
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            current_time = time.time()
-
-            # Get state from SDK
-            robot_state = self.sdk.get_robot_state()
-
-            # Get additional info
-            error_msg = (
-                self.sdk.get_error_message() if robot_state.get("error_code", 0) != 0 else ""
-            )
-
-            # Map state integer to string
-            state_map = {0: "idle", 1: "moving", 2: "error", 3: "emergency_stop"}
-            state_str = state_map.get(robot_state.get("state", 0), "unknown")
-
-            # Map mode integer to string
-            mode_map = {0: "position", 1: "velocity", 2: "torque", 3: "impedance"}
-            mode_str = mode_map.get(robot_state.get("mode", 0), "unknown")
-
-            result = {
-                "state": state_str,
-                "state_code": robot_state.get("state", 0),
-                "mode": mode_str,
-                "mode_code": robot_state.get("mode", 0),
-                "error_code": robot_state.get("error_code", 0),
-                "error_message": error_msg,
-                "is_moving": robot_state.get("is_moving", False),
-                "is_connected": self.sdk.is_connected(),
-                "is_enabled": self.sdk.are_servos_enabled(),
-                "timestamp": current_time,
-                "success": True,
-            }
-
-            # Add shared state info if available
-            if self.shared_state:
-                result["is_homed"] = self.shared_state.is_homed
-                result["last_update"] = self.shared_state.last_state_update
-                result["last_command"] = self.shared_state.last_command_sent
-
-            return result
-
-        except Exception as e:
-            self.logger.error(f"Error in get_robot_state: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def get_system_info(self) -> dict[str, Any]:
-        """Get system information.
-
-        Returns:
-            Dict with system information
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            # Get manipulator info
-            info = self.sdk.get_info()
-
-            result = {
-                "vendor": info.vendor,
-                "model": info.model,
-                "dof": info.dof,
-                "firmware_version": info.firmware_version,
-                "serial_number": info.serial_number,
-                "success": True,
-            }
-
-            # Add capabilities if available
-            if self.capabilities:
-                result["capabilities"] = {
-                    "dof": self.capabilities.dof,
-                    "has_gripper": self.capabilities.has_gripper,
-                    "has_force_torque": self.capabilities.has_force_torque,
-                    "has_impedance_control": self.capabilities.has_impedance_control,
-                    "has_cartesian_control": self.capabilities.has_cartesian_control,
-                    "payload_mass": self.capabilities.payload_mass,
-                    "reach": self.capabilities.reach,
-                }
-
-            return result
-
-        except Exception as e:
-            self.logger.error(f"Error in get_system_info: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def get_capabilities(self) -> dict[str, Any]:
-        """Get manipulator capabilities.
-
-        Returns:
-            Dict with capability information
-        """
-        try:
-            if not self.capabilities:
-                return {"success": False, "error": "Capabilities not available"}
-
-            return {
-                "dof": self.capabilities.dof,
-                "has_gripper": self.capabilities.has_gripper,
-                "has_force_torque": self.capabilities.has_force_torque,
-                "has_impedance_control": self.capabilities.has_impedance_control,
-                "has_cartesian_control": self.capabilities.has_cartesian_control,
-                "joint_limits_lower": self.capabilities.joint_limits_lower,
-                "joint_limits_upper": self.capabilities.joint_limits_upper,
-                "max_joint_velocity": self.capabilities.max_joint_velocity,
-                "max_joint_acceleration": self.capabilities.max_joint_acceleration,
-                "payload_mass": self.capabilities.payload_mass,
-                "reach": self.capabilities.reach,
-                "success": True,
-            }
-
-        except Exception as e:
-            self.logger.error(f"Error in get_capabilities: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def get_error_state(self) -> dict[str, Any]:
-        """Get detailed error state.
-
-        Returns:
-            Dict with error information
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            error_code = self.sdk.get_error_code()
-            error_msg = self.sdk.get_error_message()
-
-            result = {
-                "has_error": error_code != 0,
-                "error_code": error_code,
-                "error_message": error_msg,
-                "error_history": list(self.error_history),
-                "total_errors": self.health_metrics.total_errors,
-                "success": True,
-            }
-
-            # Add last error time from shared state
-            if self.shared_state:
-                result["last_error_time"] = self.shared_state.last_error_time
-
-            return result
-
-        except Exception as e:
-            self.logger.error(f"Error in get_error_state: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def get_health_metrics(self) -> dict[str, Any]:
-        """Get health metrics.
-
-        Returns:
-            Dict with health metrics
-        """
-        try:
-            self._update_health_metrics()
-
-            return {
-                "uptime": self.health_metrics.uptime,
-                "update_rate": self.health_metrics.update_rate,
-                "command_rate": self.health_metrics.command_rate,
-                "error_rate": self.health_metrics.error_rate,
-                "total_updates": self.health_metrics.total_updates,
-                "total_commands": self.health_metrics.total_commands,
-                "total_errors": self.health_metrics.total_errors,
-                "is_healthy": self._is_healthy(),
-                "timestamp": time.time(),
-                "success": True,
-            }
-
-        except Exception as e:
-            self.logger.error(f"Error in get_health_metrics: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def get_statistics(self) -> dict[str, Any]:
-        """Get operation statistics.
-
-        Returns:
-            Dict with statistics
-        """
-        try:
-            stats = {}
-
-            # Get stats from shared state
-            if self.shared_state:
-                stats.update(self.shared_state.get_statistics())
-
-            # Add component stats
-            stats["uptime"] = time.time() - self.start_time
-            stats["health_metrics"] = {
-                "update_rate": self.health_metrics.update_rate,
-                "command_rate": self.health_metrics.command_rate,
-                "error_rate": self.health_metrics.error_rate,
-            }
-
-            stats["success"] = True
-            return stats
-
-        except Exception as e:
-            self.logger.error(f"Error in get_statistics: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def check_connection(self) -> dict[str, Any]:
-        """Check connection status.
-
-        Returns:
-            Dict with connection status
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            connected = self.sdk.is_connected()
-
-            result: dict[str, Any] = {
-                "connected": connected,
-                "timestamp": time.time(),
-                "success": True,
-            }
-
-            # Try to get more info if connected
-            if connected:
-                try:
-                    # Try a simple query to verify connection
-                    self.sdk.get_error_code()
-                    result["verified"] = True
-                except:
-                    result["verified"] = False
-                    result["message"] = "Connected but cannot communicate"
-
-            return result
-
-        except Exception as e:
-            self.logger.error(f"Error in check_connection: {e}")
-            return {"success": False, "error": str(e)}
-
-    # ============= Force/Torque Monitoring (Optional) =============
-
-    @component_api
-    def get_force_torque(self) -> dict[str, Any]:
-        """Get force/torque sensor data.
-
-        Returns:
-            Dict with F/T data if available
-        """
-        try:
-            # Check if F/T is supported
-            if not self.capabilities or not self.capabilities.has_force_torque:
-                return {"success": False, "error": "Force/torque sensor not available"}
-
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            ft_data = self.sdk.get_force_torque()
-
-            if ft_data:
-                return {
-                    "force": ft_data[:3] if len(ft_data) >= 3 else None,  # [fx, fy, fz]
-                    "torque": ft_data[3:6] if len(ft_data) >= 6 else None,  # [tx, ty, tz]
-                    "data": ft_data,
-                    "timestamp": time.time(),
-                    "success": True,
-                }
-            else:
-                return {"success": False, "error": "Failed to read F/T sensor"}
-
-        except Exception as e:
-            self.logger.error(f"Error in get_force_torque: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def zero_force_torque(self) -> dict[str, Any]:
-        """Zero the force/torque sensor.
-
-        Returns:
-            Dict with success status
-        """
-        try:
-            # Check if F/T is supported
-            if not self.capabilities or not self.capabilities.has_force_torque:
-                return {"success": False, "error": "Force/torque sensor not available"}
-
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            success = self.sdk.zero_force_torque()
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in zero_force_torque: {e}")
-            return {"success": False, "error": str(e)}
-
-    # ============= I/O Monitoring (Optional) =============
-
-    @component_api
-    def get_digital_inputs(self) -> dict[str, Any]:
-        """Get digital input states.
-
-        Returns:
-            Dict with digital input states
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            inputs = self.sdk.get_digital_inputs()
-
-            if inputs is not None:
-                return {"inputs": inputs, "timestamp": time.time(), "success": True}
-            else:
-                return {"success": False, "error": "Digital inputs not available"}
-
-        except Exception as e:
-            self.logger.error(f"Error in get_digital_inputs: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def set_digital_outputs(self, outputs: dict[str, bool]) -> dict[str, Any]:
-        """Set digital output states.
-
-        Args:
-            outputs: Dict of output_id: bool
-
-        Returns:
-            Dict with success status
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            success = self.sdk.set_digital_outputs(outputs)
-            return {"success": success}
-
-        except Exception as e:
-            self.logger.error(f"Error in set_digital_outputs: {e}")
-            return {"success": False, "error": str(e)}
-
-    @component_api
-    def get_analog_inputs(self) -> dict[str, Any]:
-        """Get analog input values.
-
-        Returns:
-            Dict with analog input values
-        """
-        try:
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            inputs = self.sdk.get_analog_inputs()
-
-            if inputs is not None:
-                return {"inputs": inputs, "timestamp": time.time(), "success": True}
-            else:
-                return {"success": False, "error": "Analog inputs not available"}
-
-        except Exception as e:
-            self.logger.error(f"Error in get_analog_inputs: {e}")
-            return {"success": False, "error": str(e)}
-
-    # ============= Gripper Status (Optional) =============
-
-    @component_api
-    def get_gripper_state(self) -> dict[str, Any]:
-        """Get gripper state.
-
-        Returns:
-            Dict with gripper state
-        """
-        try:
-            # Check if gripper is supported
-            if not self.capabilities or not self.capabilities.has_gripper:
-                return {"success": False, "error": "Gripper not available"}
-
-            if self.sdk is None:
-                return {"success": False, "error": "SDK not configured"}
-
-            position = self.sdk.get_gripper_position()
-
-            if position is not None:
-                result: dict[str, Any] = {
-                    "position": position,  # meters
-                    "timestamp": time.time(),
-                    "success": True,
-                }
-
-                # Add from shared state if available
-                if self.shared_state and self.shared_state.gripper_force is not None:
-                    result["force"] = self.shared_state.gripper_force
-
-                return result
-            else:
-                return {"success": False, "error": "Failed to get gripper state"}
-
-        except Exception as e:
-            self.logger.error(f"Error in get_gripper_state: {e}")
-            return {"success": False, "error": str(e)}
-
-    # ============= Helper Methods =============
-
-    def _update_health_metrics(self) -> None:
-        """Update health metrics based on recent data."""
-        current_time = time.time()
-
-        # Update uptime
-        self.health_metrics.uptime = current_time - self.start_time
-
-        # Calculate update rate
-        if len(self.update_timestamps) > 1:
-            time_span = self.update_timestamps[-1] - self.update_timestamps[0]
-            if time_span > 0:
-                self.health_metrics.update_rate = len(self.update_timestamps) / time_span
-
-        # Calculate command rate
-        if len(self.command_timestamps) > 1:
-            time_span = self.command_timestamps[-1] - self.command_timestamps[0]
-            if time_span > 0:
-                self.health_metrics.command_rate = len(self.command_timestamps) / time_span
-
-        # Calculate error rate (errors per minute)
-        recent_errors = [t for t in self.error_timestamps if current_time - t < 60]
-        self.health_metrics.error_rate = len(recent_errors)
-
-        # Update totals from shared state
-        if self.shared_state:
-            stats = self.shared_state.get_statistics()
-            self.health_metrics.total_updates = stats.get("state_read_count", 0)
-            self.health_metrics.total_commands = stats.get("command_sent_count", 0)
-            self.health_metrics.total_errors = stats.get("error_count", 0)
-
-    def _is_healthy(self) -> bool:
-        """Check if system is healthy based on metrics."""
-        # Check update rate (should be > 10 Hz)
-        if self.health_metrics.update_rate < 10:
-            return False
-
-        # Check error rate (should be < 10 per minute)
-        if self.health_metrics.error_rate > 10:
-            return False
-
-        # Check SDK is configured
-        if self.sdk is None:
-            return False
-
-        # Check connection
-        if not self.sdk.is_connected():
-            return False
-
-        # Check for persistent errors
-        if self.sdk.get_error_code() != 0:
-            return False
-
-        return True
-
-    def record_error(self, error_code: int, error_msg: str) -> None:
-        """Record an error occurrence.
-
-        Args:
-            error_code: Error code
-            error_msg: Error message
-        """
-        current_time = time.time()
-        self.error_timestamps.append(current_time)
-        self.error_history.append(
-            {"code": error_code, "message": error_msg, "timestamp": current_time}
-        )
-
-    def record_command(self) -> None:
-        """Record a command occurrence."""
-        self.command_timestamps.append(time.time())
diff --git a/dimos/hardware/manipulators/base/driver.py b/dimos/hardware/manipulators/base/driver.py
deleted file mode 100644
index be68be5a23..0000000000
--- a/dimos/hardware/manipulators/base/driver.py
+++ /dev/null
@@ -1,637 +0,0 @@
-# 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.
-
-"""Base manipulator driver with threading and component management."""
-
-from dataclasses import dataclass
-import logging
-from queue import Empty, Queue
-from threading import Event, Thread
-import time
-from typing import Any
-
-from dimos.core import In, Module, Out, rpc
-from dimos.msgs.geometry_msgs import WrenchStamped
-from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState
-
-from .sdk_interface import BaseManipulatorSDK
-from .spec import ManipulatorCapabilities
-from .utils import SharedState
-
-
-@dataclass
-class Command:
-    """Command to be sent to the manipulator."""
-
-    type: str  # 'position', 'velocity', 'effort', 'cartesian', etc.
-    data: Any
-    timestamp: float = 0.0
-
-
-class BaseManipulatorDriver(Module):
-    """Base driver providing threading and component management.
-
-    This class handles:
-    - Thread management (state reader, command sender, state publisher)
-    - Component registration and lifecycle
-    - RPC method registration
-    - Shared state management
-    - Error handling and recovery
-    - Pub/Sub with LCM transport for real-time control
-    """
-
-    # Input topics (commands from controllers - initialized by Module)
-    joint_position_command: In[JointCommand] = None  # type: ignore[assignment]
-    joint_velocity_command: In[JointCommand] = None  # type: ignore[assignment]
-
-    # Output topics (state publishing - initialized by Module)
-    joint_state: Out[JointState] = None  # type: ignore[assignment]
-    robot_state: Out[RobotState] = None  # type: ignore[assignment]
-    ft_sensor: Out[WrenchStamped] = None  # type: ignore[assignment]
-
-    def __init__(
-        self,
-        sdk: BaseManipulatorSDK,
-        components: list[Any],
-        config: dict[str, Any],
-        name: str | None = None,
-        *args: Any,
-        **kwargs: Any,
-    ) -> None:
-        """Initialize the base manipulator driver.
-
-        Args:
-            sdk: SDK wrapper instance
-            components: List of component instances
-            config: Configuration dictionary
-            name: Optional driver name for logging
-            *args, **kwargs: Additional arguments for Module
-        """
-        # Initialize Module parent class
-        super().__init__(*args, **kwargs)
-
-        self.sdk = sdk
-        self.components = components
-        self.config: Any = config  # Config dict accessed as object
-        self.name = name or self.__class__.__name__
-
-        # Logging
-        self.logger = logging.getLogger(self.name)
-
-        # Shared state
-        self.shared_state = SharedState()
-
-        # Threading
-        self.stop_event = Event()
-        self.threads: list[Thread] = []
-        self.command_queue: Queue[Any] = Queue(maxsize=10)
-
-        # RPC registry
-        self.rpc_methods: dict[str, Any] = {}
-        self._exposed_component_apis: set[str] = set()  # Track auto-exposed method names
-
-        # Capabilities
-        self.capabilities = self._get_capabilities()
-
-        # Rate control
-        self.control_rate = config.get("control_rate", 100)  # Hz - control loop + joint feedback
-        self.monitor_rate = config.get("monitor_rate", 10)  # Hz - robot state monitoring
-
-        # Pre-allocate reusable objects (optimization: avoid per-cycle allocation)
-        # Note: _joint_names is populated after _get_capabilities() sets self.capabilities
-        self._joint_names: list[str] = [f"joint{i + 1}" for i in range(self.capabilities.dof)]
-
-        # Initialize components with shared resources
-        self._initialize_components()
-
-        # Auto-expose component API methods as RPCs on the driver
-        self._auto_expose_component_apis()
-
-        # Connect to hardware
-        self._connect()
-
-    def _get_capabilities(self) -> ManipulatorCapabilities:
-        """Get manipulator capabilities from config or SDK.
-
-        Returns:
-            ManipulatorCapabilities instance
-        """
-        # Try to get from SDK info
-        info = self.sdk.get_info()
-
-        # Get joint limits
-        lower_limits, upper_limits = self.sdk.get_joint_limits()
-        velocity_limits = self.sdk.get_velocity_limits()
-        acceleration_limits = self.sdk.get_acceleration_limits()
-
-        return ManipulatorCapabilities(
-            dof=info.dof,
-            has_gripper=self.config.get("has_gripper", False),
-            has_force_torque=self.config.get("has_force_torque", False),
-            has_impedance_control=self.config.get("has_impedance_control", False),
-            has_cartesian_control=self.config.get("has_cartesian_control", False),
-            max_joint_velocity=velocity_limits,
-            max_joint_acceleration=acceleration_limits,
-            joint_limits_lower=lower_limits,
-            joint_limits_upper=upper_limits,
-            payload_mass=self.config.get("payload_mass", 0.0),
-            reach=self.config.get("reach", 0.0),
-        )
-
-    def _initialize_components(self) -> None:
-        """Initialize components with shared resources."""
-        for component in self.components:
-            # Provide access to shared state
-            if hasattr(component, "set_shared_state"):
-                component.set_shared_state(self.shared_state)
-
-            # Provide access to SDK
-            if hasattr(component, "set_sdk"):
-                component.set_sdk(self.sdk)
-
-            # Provide access to command queue
-            if hasattr(component, "set_command_queue"):
-                component.set_command_queue(self.command_queue)
-
-            # Provide access to capabilities
-            if hasattr(component, "set_capabilities"):
-                component.set_capabilities(self.capabilities)
-
-            # Initialize component
-            if hasattr(component, "initialize"):
-                component.initialize()
-
-    def _auto_expose_component_apis(self) -> None:
-        """Auto-expose @component_api methods from components as RPC methods on the driver.
-
-        This scans all components for methods decorated with @component_api and creates
-        corresponding @rpc wrapper methods on the driver instance. This allows external
-        code to call these methods via the standard Module RPC system.
-
-        Example:
-            # Component defines:
-            @component_api
-            def enable_servo(self): ...
-
-            # Driver auto-generates an RPC wrapper, so external code can call:
-            driver.enable_servo()
-
-            # And the method is discoverable via:
-            driver.rpcs  # Lists 'enable_servo' among available RPCs
-        """
-        for component in self.components:
-            for method_name in dir(component):
-                if method_name.startswith("_"):
-                    continue
-
-                method = getattr(component, method_name, None)
-                if not callable(method) or not getattr(method, "__component_api__", False):
-                    continue
-
-                # Skip if driver already has a non-wrapper method with this name
-                existing = getattr(self, method_name, None)
-                if existing is not None and not getattr(
-                    existing, "__component_api_wrapper__", False
-                ):
-                    self.logger.warning(
-                        f"Driver already has method '{method_name}', skipping component API"
-                    )
-                    continue
-
-                # Create RPC wrapper - use factory to properly capture method reference
-                wrapper = self._create_component_api_wrapper(method)
-
-                # Attach to driver instance
-                setattr(self, method_name, wrapper)
-
-                # Store in rpc_methods dict for backward compatibility
-                self.rpc_methods[method_name] = wrapper
-
-                # Track exposed method name for cleanup
-                self._exposed_component_apis.add(method_name)
-
-                self.logger.debug(f"Exposed component API as RPC: {method_name}")
-
-    def _create_component_api_wrapper(self, component_method: Any) -> Any:
-        """Create an RPC wrapper for a component API method.
-
-        Args:
-            component_method: The component method to wrap
-
-        Returns:
-            RPC-decorated wrapper function
-        """
-        import functools
-
-        @rpc
-        @functools.wraps(component_method)
-        def wrapper(*args: Any, **kwargs: Any) -> Any:
-            return component_method(*args, **kwargs)
-
-        wrapper.__component_api_wrapper__ = True  # type: ignore[attr-defined]
-        return wrapper
-
-    def _connect(self) -> None:
-        """Connect to the manipulator hardware."""
-        self.logger.info(f"Connecting to {self.name}...")
-
-        # Connect via SDK
-        if not self.sdk.connect(self.config):
-            raise RuntimeError(f"Failed to connect to {self.name}")
-
-        self.shared_state.is_connected = True
-        self.logger.info(f"Successfully connected to {self.name}")
-
-        # Get initial state
-        self._update_joint_state()
-        self._update_robot_state()
-
-    def _update_joint_state(self) -> None:
-        """Update joint state from hardware (high frequency - 100Hz).
-
-        Reads joint positions, velocities, efforts and publishes to LCM immediately.
-        """
-        try:
-            # Get joint state feedback
-            positions = self.sdk.get_joint_positions()
-            velocities = self.sdk.get_joint_velocities()
-            efforts = self.sdk.get_joint_efforts()
-
-            self.shared_state.update_joint_state(
-                positions=positions, velocities=velocities, efforts=efforts
-            )
-
-            # Publish joint state immediately at control rate
-            if self.joint_state and hasattr(self.joint_state, "publish"):
-                joint_state_msg = JointState(
-                    ts=time.time(),
-                    frame_id="joint-state",
-                    name=self._joint_names,  # Pre-allocated list (optimization)
-                    position=positions or [0.0] * self.capabilities.dof,
-                    velocity=velocities or [0.0] * self.capabilities.dof,
-                    effort=efforts or [0.0] * self.capabilities.dof,
-                )
-                self.joint_state.publish(joint_state_msg)
-
-        except Exception as e:
-            self.logger.error(f"Error updating joint state: {e}")
-
-    def _update_robot_state(self) -> None:
-        """Update robot state from hardware (low frequency - 10Hz).
-
-        Reads robot mode, errors, warnings, optional states and publishes to LCM immediately.
-        """
-        try:
-            # Get robot state (mode, errors, warnings)
-            robot_state = self.sdk.get_robot_state()
-            self.shared_state.update_robot_state(
-                state=robot_state.get("state", 0),
-                mode=robot_state.get("mode", 0),
-                error_code=robot_state.get("error_code", 0),
-                error_message=self.sdk.get_error_message(),
-            )
-
-            # Update status flags
-            self.shared_state.is_moving = robot_state.get("is_moving", False)
-            self.shared_state.is_enabled = self.sdk.are_servos_enabled()
-
-            # Get optional states (cartesian, force/torque, gripper)
-            if self.capabilities.has_cartesian_control:
-                cart_pos = self.sdk.get_cartesian_position()
-                if cart_pos:
-                    self.shared_state.cartesian_position = cart_pos
-
-            if self.capabilities.has_force_torque:
-                ft = self.sdk.get_force_torque()
-                if ft:
-                    self.shared_state.force_torque = ft
-
-            if self.capabilities.has_gripper:
-                gripper_pos = self.sdk.get_gripper_position()
-                if gripper_pos is not None:
-                    self.shared_state.gripper_position = gripper_pos
-
-            # Publish robot state immediately at monitor rate
-            if self.robot_state and hasattr(self.robot_state, "publish"):
-                robot_state_msg = RobotState(
-                    state=self.shared_state.robot_state,
-                    mode=self.shared_state.control_mode,
-                    error_code=self.shared_state.error_code,
-                    warn_code=0,
-                )
-                self.robot_state.publish(robot_state_msg)
-
-            # Publish force/torque if available
-            if (
-                self.ft_sensor
-                and hasattr(self.ft_sensor, "publish")
-                and self.capabilities.has_force_torque
-            ):
-                if self.shared_state.force_torque:
-                    ft_msg = WrenchStamped.from_force_torque_array(
-                        ft_data=self.shared_state.force_torque,
-                        frame_id="ft_sensor",
-                        ts=time.time(),
-                    )
-                    self.ft_sensor.publish(ft_msg)
-
-        except Exception as e:
-            self.logger.error(f"Error updating robot state: {e}")
-            self.shared_state.update_robot_state(error_code=999, error_message=str(e))
-
-    # ============= Threading =============
-
-    @rpc
-    def start(self) -> None:
-        """Start all driver threads and subscribe to input topics."""
-        super().start()
-        self.logger.info(f"Starting {self.name} driver threads...")
-
-        # Subscribe to input topics if they have transports
-        try:
-            if self.joint_position_command and hasattr(self.joint_position_command, "subscribe"):
-                self.joint_position_command.subscribe(self._on_joint_position_command)
-                self.logger.debug("Subscribed to joint_position_command")
-        except (AttributeError, ValueError) as e:
-            self.logger.debug(f"joint_position_command transport not configured: {e}")
-
-        try:
-            if self.joint_velocity_command and hasattr(self.joint_velocity_command, "subscribe"):
-                self.joint_velocity_command.subscribe(self._on_joint_velocity_command)
-                self.logger.debug("Subscribed to joint_velocity_command")
-        except (AttributeError, ValueError) as e:
-            self.logger.debug(f"joint_velocity_command transport not configured: {e}")
-
-        self.threads = [
-            Thread(target=self._control_loop_thread, name=f"{self.name}-ControlLoop", daemon=True),
-            Thread(
-                target=self._robot_state_monitor_thread,
-                name=f"{self.name}-StateMonitor",
-                daemon=True,
-            ),
-        ]
-
-        for thread in self.threads:
-            thread.start()
-            self.logger.debug(f"Started thread: {thread.name}")
-
-        self.logger.info(f"{self.name} driver started successfully")
-
-    def _control_loop_thread(self) -> None:
-        """Control loop: send commands AND read joint feedback (100Hz).
-
-        This tight loop ensures synchronized command/feedback for real-time control.
-        """
-        self.logger.debug("Control loop thread started")
-        period = 1.0 / self.control_rate
-        next_time = time.perf_counter() + period  # perf_counter for precise timing
-
-        while not self.stop_event.is_set():
-            try:
-                # 1. Process all pending commands (non-blocking)
-                while True:
-                    try:
-                        command = self.command_queue.get_nowait()  # Non-blocking (optimization)
-                        self._process_command(command)
-                    except Empty:
-                        break  # No more commands
-
-                # 2. Read joint state feedback (critical for control)
-                self._update_joint_state()
-
-            except Exception as e:
-                self.logger.error(f"Control loop error: {e}")
-
-            # Rate control - maintain precise timing
-            next_time += period
-            sleep_time = next_time - time.perf_counter()
-            if sleep_time > 0:
-                time.sleep(sleep_time)
-            else:
-                # Fell behind - reset timing
-                next_time = time.perf_counter() + period
-                if sleep_time < -period:
-                    self.logger.warning(f"Control loop fell behind by {-sleep_time:.3f}s")
-
-        self.logger.debug("Control loop thread stopped")
-
-    def _robot_state_monitor_thread(self) -> None:
-        """Monitor robot state: mode, errors, warnings (10-20Hz).
-
-        Lower frequency monitoring for high-level planning and error handling.
-        """
-        self.logger.debug("Robot state monitor thread started")
-        period = 1.0 / self.monitor_rate
-        next_time = time.perf_counter() + period  # perf_counter for precise timing
-
-        while not self.stop_event.is_set():
-            try:
-                # Read robot state, mode, errors, optional states
-                self._update_robot_state()
-            except Exception as e:
-                self.logger.error(f"Robot state monitor error: {e}")
-
-            # Rate control
-            next_time += period
-            sleep_time = next_time - time.perf_counter()
-            if sleep_time > 0:
-                time.sleep(sleep_time)
-            else:
-                next_time = time.perf_counter() + period
-
-        self.logger.debug("Robot state monitor thread stopped")
-
-    def _process_command(self, command: Command) -> None:
-        """Process a command from the queue.
-
-        Args:
-            command: Command to process
-        """
-        try:
-            if command.type == "position":
-                success = self.sdk.set_joint_positions(
-                    command.data["positions"],
-                    command.data.get("velocity", 1.0),
-                    command.data.get("acceleration", 1.0),
-                    command.data.get("wait", False),
-                )
-                if success:
-                    self.shared_state.target_positions = command.data["positions"]
-
-            elif command.type == "velocity":
-                success = self.sdk.set_joint_velocities(command.data["velocities"])
-                if success:
-                    self.shared_state.target_velocities = command.data["velocities"]
-
-            elif command.type == "effort":
-                success = self.sdk.set_joint_efforts(command.data["efforts"])
-                if success:
-                    self.shared_state.target_efforts = command.data["efforts"]
-
-            elif command.type == "cartesian":
-                success = self.sdk.set_cartesian_position(
-                    command.data["pose"],
-                    command.data.get("velocity", 1.0),
-                    command.data.get("acceleration", 1.0),
-                    command.data.get("wait", False),
-                )
-                if success:
-                    self.shared_state.target_cartesian_position = command.data["pose"]
-
-            elif command.type == "stop":
-                self.sdk.stop_motion()
-
-            else:
-                self.logger.warning(f"Unknown command type: {command.type}")
-
-        except Exception as e:
-            self.logger.error(f"Error processing command {command.type}: {e}")
-
-    # ============= Input Callbacks =============
-
-    def _on_joint_position_command(self, cmd_msg: JointCommand) -> None:
-        """Callback when joint position command is received.
-
-        Args:
-            cmd_msg: JointCommand message containing positions
-        """
-        command = Command(
-            type="position", data={"positions": list(cmd_msg.positions)}, timestamp=time.time()
-        )
-        try:
-            self.command_queue.put_nowait(command)
-        except:
-            self.logger.warning("Command queue full, dropping position command")
-
-    def _on_joint_velocity_command(self, cmd_msg: JointCommand) -> None:
-        """Callback when joint velocity command is received.
-
-        Args:
-            cmd_msg: JointCommand message containing velocities
-        """
-        command = Command(
-            type="velocity",
-            data={"velocities": list(cmd_msg.positions)},  # JointCommand uses 'positions' field
-            timestamp=time.time(),
-        )
-        try:
-            self.command_queue.put_nowait(command)
-        except:
-            self.logger.warning("Command queue full, dropping velocity command")
-
-    # ============= Lifecycle Management =============
-
-    @rpc
-    def stop(self) -> None:
-        """Stop all threads and disconnect from hardware."""
-        self.logger.info(f"Stopping {self.name} driver...")
-
-        # Signal threads to stop
-        self.stop_event.set()
-
-        # Stop any ongoing motion
-        try:
-            self.sdk.stop_motion()
-        except:
-            pass
-
-        # Wait for threads to stop
-        for thread in self.threads:
-            thread.join(timeout=2.0)
-            if thread.is_alive():
-                self.logger.warning(f"Thread {thread.name} did not stop cleanly")
-
-        # Disconnect from hardware
-        try:
-            self.sdk.disconnect()
-        except:
-            pass
-
-        self.shared_state.is_connected = False
-        self.logger.info(f"{self.name} driver stopped")
-
-        # Call Module's stop
-        super().stop()
-
-    def __del__(self) -> None:
-        """Cleanup on deletion."""
-        if self.shared_state.is_connected:
-            self.stop()
-
-    # ============= RPC Method Access =============
-
-    def get_rpc_method(self, method_name: str) -> Any:
-        """Get an RPC method by name.
-
-        Args:
-            method_name: Name of the RPC method
-
-        Returns:
-            The method if found, None otherwise
-        """
-        return self.rpc_methods.get(method_name)
-
-    def list_rpc_methods(self) -> list[str]:
-        """List all available RPC methods.
-
-        Returns:
-            List of RPC method names
-        """
-        return list(self.rpc_methods.keys())
-
-    # ============= Component Access =============
-
-    def get_component(self, component_type: type[Any]) -> Any:
-        """Get a component by type.
-
-        Args:
-            component_type: Type of component to find
-
-        Returns:
-            The component if found, None otherwise
-        """
-        for component in self.components:
-            if isinstance(component, component_type):
-                return component
-        return None
-
-    def add_component(self, component: Any) -> None:
-        """Add a component at runtime.
-
-        Args:
-            component: Component instance to add
-        """
-        self.components.append(component)
-        self._initialize_components()
-        self._auto_expose_component_apis()
-
-    def remove_component(self, component: Any) -> None:
-        """Remove a component at runtime.
-
-        Args:
-            component: Component instance to remove
-        """
-        if component in self.components:
-            self.components.remove(component)
-            # Clean up old exposed methods and re-expose for remaining components
-            self._cleanup_exposed_component_apis()
-            self._auto_expose_component_apis()
-
-    def _cleanup_exposed_component_apis(self) -> None:
-        """Remove all auto-exposed component API methods from the driver."""
-        for method_name in self._exposed_component_apis:
-            if hasattr(self, method_name):
-                delattr(self, method_name)
-        self._exposed_component_apis.clear()
-        self.rpc_methods.clear()
diff --git a/dimos/hardware/manipulators/base/sdk_interface.py b/dimos/hardware/manipulators/base/sdk_interface.py
deleted file mode 100644
index f20d35bd50..0000000000
--- a/dimos/hardware/manipulators/base/sdk_interface.py
+++ /dev/null
@@ -1,471 +0,0 @@
-# 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.
-
-"""Base SDK interface that all manipulator SDK wrappers must implement.
-
-This interface defines the standard methods and units that all SDK wrappers
-must provide, ensuring consistent behavior across different manipulator types.
-
-Standard Units:
-- Angles: radians
-- Angular velocity: rad/s
-- Linear position: meters
-- Linear velocity: m/s
-- Force: Newtons
-- Torque: Nm
-- Time: seconds
-"""
-
-from abc import ABC, abstractmethod
-from dataclasses import dataclass
-from typing import Any
-
-
-@dataclass
-class ManipulatorInfo:
-    """Information about the manipulator."""
-
-    vendor: str
-    model: str
-    dof: int
-    firmware_version: str | None = None
-    serial_number: str | None = None
-
-
-class BaseManipulatorSDK(ABC):
-    """Abstract base class for manipulator SDK wrappers.
-
-    All SDK wrappers must implement this interface to ensure compatibility
-    with the standard components. Methods should handle unit conversions
-    internally to always work with standard units.
-    """
-
-    # ============= Connection Management =============
-
-    @abstractmethod
-    def connect(self, config: dict[str, Any]) -> bool:
-        """Establish connection to the manipulator.
-
-        Args:
-            config: Configuration dict with connection parameters
-                   (e.g., ip, port, can_interface, etc.)
-
-        Returns:
-            True if connection successful, False otherwise
-        """
-        pass
-
-    @abstractmethod
-    def disconnect(self) -> None:
-        """Disconnect from the manipulator.
-
-        Should cleanly close all connections and free resources.
-        """
-        pass
-
-    @abstractmethod
-    def is_connected(self) -> bool:
-        """Check if currently connected to the manipulator.
-
-        Returns:
-            True if connected, False otherwise
-        """
-        pass
-
-    # ============= Joint State Query =============
-
-    @abstractmethod
-    def get_joint_positions(self) -> list[float]:
-        """Get current joint positions.
-
-        Returns:
-            Joint positions in RADIANS
-        """
-        pass
-
-    @abstractmethod
-    def get_joint_velocities(self) -> list[float]:
-        """Get current joint velocities.
-
-        Returns:
-            Joint velocities in RAD/S
-        """
-        pass
-
-    @abstractmethod
-    def get_joint_efforts(self) -> list[float]:
-        """Get current joint efforts/torques.
-
-        Returns:
-            Joint efforts in Nm (torque) or N (force)
-        """
-        pass
-
-    # ============= Joint Motion Control =============
-
-    @abstractmethod
-    def set_joint_positions(
-        self,
-        positions: list[float],
-        velocity: float = 1.0,
-        acceleration: float = 1.0,
-        wait: bool = False,
-    ) -> bool:
-        """Move joints to target positions.
-
-        Args:
-            positions: Target positions in RADIANS
-            velocity: Max velocity as fraction of maximum (0-1)
-            acceleration: Max acceleration as fraction of maximum (0-1)
-            wait: If True, block until motion completes
-
-        Returns:
-            True if command accepted, False otherwise
-        """
-        pass
-
-    @abstractmethod
-    def set_joint_velocities(self, velocities: list[float]) -> bool:
-        """Set joint velocity targets.
-
-        Args:
-            velocities: Target velocities in RAD/S
-
-        Returns:
-            True if command accepted, False otherwise
-        """
-        pass
-
-    @abstractmethod
-    def set_joint_efforts(self, efforts: list[float]) -> bool:
-        """Set joint effort/torque targets.
-
-        Args:
-            efforts: Target efforts in Nm (torque) or N (force)
-
-        Returns:
-            True if command accepted, False otherwise
-        """
-        pass
-
-    @abstractmethod
-    def stop_motion(self) -> bool:
-        """Stop all ongoing motion immediately.
-
-        Returns:
-            True if stop successful, False otherwise
-        """
-        pass
-
-    # ============= Servo Control =============
-
-    @abstractmethod
-    def enable_servos(self) -> bool:
-        """Enable motor control (servos/brakes released).
-
-        Returns:
-            True if servos enabled, False otherwise
-        """
-        pass
-
-    @abstractmethod
-    def disable_servos(self) -> bool:
-        """Disable motor control (servos/brakes engaged).
-
-        Returns:
-            True if servos disabled, False otherwise
-        """
-        pass
-
-    @abstractmethod
-    def are_servos_enabled(self) -> bool:
-        """Check if servos are currently enabled.
-
-        Returns:
-            True if enabled, False if disabled
-        """
-        pass
-
-    # ============= System State =============
-
-    @abstractmethod
-    def get_robot_state(self) -> dict[str, Any]:
-        """Get current robot state information.
-
-        Returns:
-            Dict with at least these keys:
-            - 'state': int (0=idle, 1=moving, 2=error, 3=e-stop)
-            - 'mode': int (0=position, 1=velocity, 2=torque)
-            - 'error_code': int (0 = no error)
-            - 'is_moving': bool
-        """
-        pass
-
-    @abstractmethod
-    def get_error_code(self) -> int:
-        """Get current error code.
-
-        Returns:
-            Error code (0 = no error)
-        """
-        pass
-
-    @abstractmethod
-    def get_error_message(self) -> str:
-        """Get human-readable error message.
-
-        Returns:
-            Error message string (empty if no error)
-        """
-        pass
-
-    @abstractmethod
-    def clear_errors(self) -> bool:
-        """Clear any error states.
-
-        Returns:
-            True if errors cleared, False otherwise
-        """
-        pass
-
-    @abstractmethod
-    def emergency_stop(self) -> bool:
-        """Execute emergency stop.
-
-        Returns:
-            True if e-stop executed, False otherwise
-        """
-        pass
-
-    # ============= Information =============
-
-    @abstractmethod
-    def get_info(self) -> ManipulatorInfo:
-        """Get manipulator information.
-
-        Returns:
-            ManipulatorInfo object with vendor, model, DOF, etc.
-        """
-        pass
-
-    @abstractmethod
-    def get_joint_limits(self) -> tuple[list[float], list[float]]:
-        """Get joint position limits.
-
-        Returns:
-            Tuple of (lower_limits, upper_limits) in RADIANS
-        """
-        pass
-
-    @abstractmethod
-    def get_velocity_limits(self) -> list[float]:
-        """Get joint velocity limits.
-
-        Returns:
-            Maximum velocities in RAD/S
-        """
-        pass
-
-    @abstractmethod
-    def get_acceleration_limits(self) -> list[float]:
-        """Get joint acceleration limits.
-
-        Returns:
-            Maximum accelerations in RAD/S²
-        """
-        pass
-
-    # ============= Optional Methods (Override if Supported) =============
-    # These have default implementations that indicate feature not available
-
-    def get_cartesian_position(self) -> dict[str, float] | None:
-        """Get current end-effector pose.
-
-        Returns:
-            Dict with keys: x, y, z (meters), roll, pitch, yaw (radians)
-            None if not supported
-        """
-        return None
-
-    def set_cartesian_position(
-        self,
-        pose: dict[str, float],
-        velocity: float = 1.0,
-        acceleration: float = 1.0,
-        wait: bool = False,
-    ) -> bool:
-        """Move end-effector to target pose.
-
-        Args:
-            pose: Target pose with keys: x, y, z (meters), roll, pitch, yaw (radians)
-            velocity: Max velocity as fraction (0-1)
-            acceleration: Max acceleration as fraction (0-1)
-            wait: If True, block until motion completes
-
-        Returns:
-            False (not supported by default)
-        """
-        return False
-
-    def get_cartesian_velocity(self) -> dict[str, float] | None:
-        """Get current end-effector velocity.
-
-        Returns:
-            Dict with keys: vx, vy, vz (m/s), wx, wy, wz (rad/s)
-            None if not supported
-        """
-        return None
-
-    def set_cartesian_velocity(self, twist: dict[str, float]) -> bool:
-        """Set end-effector velocity.
-
-        Args:
-            twist: Velocity with keys: vx, vy, vz (m/s), wx, wy, wz (rad/s)
-
-        Returns:
-            False (not supported by default)
-        """
-        return False
-
-    def get_force_torque(self) -> list[float] | None:
-        """Get force/torque sensor reading.
-
-        Returns:
-            List of [fx, fy, fz (N), tx, ty, tz (Nm)]
-            None if not supported
-        """
-        return None
-
-    def zero_force_torque(self) -> bool:
-        """Zero the force/torque sensor.
-
-        Returns:
-            False (not supported by default)
-        """
-        return False
-
-    def set_impedance_parameters(self, stiffness: list[float], damping: list[float]) -> bool:
-        """Set impedance control parameters.
-
-        Args:
-            stiffness: Stiffness values [x, y, z, rx, ry, rz]
-            damping: Damping values [x, y, z, rx, ry, rz]
-
-        Returns:
-            False (not supported by default)
-        """
-        return False
-
-    def get_digital_inputs(self) -> dict[str, bool] | None:
-        """Get digital input states.
-
-        Returns:
-            Dict of input_id: bool
-            None if not supported
-        """
-        return None
-
-    def set_digital_outputs(self, outputs: dict[str, bool]) -> bool:
-        """Set digital output states.
-
-        Args:
-            outputs: Dict of output_id: bool
-
-        Returns:
-            False (not supported by default)
-        """
-        return False
-
-    def get_analog_inputs(self) -> dict[str, float] | None:
-        """Get analog input values.
-
-        Returns:
-            Dict of input_id: float
-            None if not supported
-        """
-        return None
-
-    def set_analog_outputs(self, outputs: dict[str, float]) -> bool:
-        """Set analog output values.
-
-        Args:
-            outputs: Dict of output_id: float
-
-        Returns:
-            False (not supported by default)
-        """
-        return False
-
-    def execute_trajectory(self, trajectory: list[dict[str, Any]], wait: bool = True) -> bool:
-        """Execute a joint trajectory.
-
-        Args:
-            trajectory: List of waypoints, each with:
-                       - 'positions': list[float] in radians
-                       - 'velocities': Optional list[float] in rad/s
-                       - 'time': float seconds from start
-            wait: If True, block until trajectory completes
-
-        Returns:
-            False (not supported by default)
-        """
-        return False
-
-    def stop_trajectory(self) -> bool:
-        """Stop any executing trajectory.
-
-        Returns:
-            False (not supported by default)
-        """
-        return False
-
-    def get_gripper_position(self) -> float | None:
-        """Get gripper position.
-
-        Returns:
-            Position in meters (0=closed, max=fully open)
-            None if no gripper
-        """
-        return None
-
-    def set_gripper_position(self, position: float, force: float = 1.0) -> bool:
-        """Set gripper position.
-
-        Args:
-            position: Target position in meters
-            force: Gripping force as fraction (0-1)
-
-        Returns:
-            False (not supported by default)
-        """
-        return False
-
-    def set_control_mode(self, mode: str) -> bool:
-        """Set control mode.
-
-        Args:
-            mode: One of 'position', 'velocity', 'torque', 'impedance'
-
-        Returns:
-            False (not supported by default)
-        """
-        return False
-
-    def get_control_mode(self) -> str | None:
-        """Get current control mode.
-
-        Returns:
-            Current mode string or None if not supported
-        """
-        return None
diff --git a/dimos/hardware/manipulators/base/spec.py b/dimos/hardware/manipulators/base/spec.py
deleted file mode 100644
index 8a0722cf09..0000000000
--- a/dimos/hardware/manipulators/base/spec.py
+++ /dev/null
@@ -1,195 +0,0 @@
-# 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.
-
-from dataclasses import dataclass
-from typing import Any, Protocol
-
-from dimos.core import In, Out
-from dimos.msgs.geometry_msgs import WrenchStamped
-from dimos.msgs.sensor_msgs import JointCommand, JointState
-
-
-@dataclass
-class RobotState:
-    """Universal robot state compatible with all manipulators."""
-
-    # Core state fields (all manipulators must provide these)
-    state: int = 0  # 0: idle, 1: moving, 2: error, 3: e-stop
-    mode: int = 0  # 0: position, 1: velocity, 2: torque, 3: impedance
-    error_code: int = 0  # Standardized error codes across all arms
-    warn_code: int = 0  # Standardized warning codes
-
-    # Extended state (optional, arm-specific)
-    is_connected: bool = False
-    is_enabled: bool = False
-    is_moving: bool = False
-    is_collision: bool = False
-
-    # Vendor-specific data (if needed)
-    vendor_data: dict[str, Any] | None = None
-
-
-@dataclass
-class ManipulatorCapabilities:
-    """Describes what a manipulator can do."""
-
-    dof: int  # Degrees of freedom
-    has_gripper: bool = False
-    has_force_torque: bool = False
-    has_impedance_control: bool = False
-    has_cartesian_control: bool = False
-    max_joint_velocity: list[float] | None = None  # rad/s
-    max_joint_acceleration: list[float] | None = None  # rad/s²
-    joint_limits_lower: list[float] | None = None  # rad
-    joint_limits_upper: list[float] | None = None  # rad
-    payload_mass: float = 0.0  # kg
-    reach: float = 0.0  # meters
-
-
-class ManipulatorDriverSpec(Protocol):
-    """Universal protocol specification for ALL manipulator drivers.
-
-    This defines the standard interface that every manipulator driver
-    must implement, regardless of the underlying hardware (XArm, Piper,
-    UR, Franka, etc.).
-
-    ## Component-Based Architecture
-
-    Drivers use a **component-based architecture** where functionality is provided
-    by composable components:
-
-    - **StandardMotionComponent**: Joint/cartesian motion, trajectory execution
-    - **StandardServoComponent**: Servo control, modes, emergency stop, error handling
-    - **StandardStatusComponent**: State monitoring, capabilities, diagnostics
-
-    RPC methods are provided by components and registered with the driver.
-    Access them via:
-
-    ```python
-    # Method 1: Via component (direct access)
-    motion = driver.get_component(StandardMotionComponent)
-    motion.rpc_move_joint(positions=[0, 0, 0, 0, 0, 0])
-
-    # Method 2: Via driver's RPC registry
-    move_fn = driver.get_rpc_method('rpc_move_joint')
-    move_fn(positions=[0, 0, 0, 0, 0, 0])
-
-    # Method 3: Via blueprints (recommended - automatic routing)
-    # Commands sent to input topics are automatically routed to components
-    driver.joint_position_command.publish(JointCommand(positions=[0, 0, 0, 0, 0, 0]))
-    ```
-
-    ## Required Components
-
-    Every driver must include these standard components:
-    - `StandardMotionComponent` - Provides motion control RPC methods
-    - `StandardServoComponent` - Provides servo control RPC methods
-    - `StandardStatusComponent` - Provides status monitoring RPC methods
-
-    ## Available RPC Methods (via Components)
-
-    ### Motion Control (StandardMotionComponent)
-    - `rpc_move_joint()` - Move to joint positions
-    - `rpc_move_joint_velocity()` - Set joint velocities
-    - `rpc_move_joint_effort()` - Set joint efforts (optional)
-    - `rpc_stop_motion()` - Stop all motion
-    - `rpc_get_joint_state()` - Get current joint state
-    - `rpc_get_joint_limits()` - Get joint limits
-    - `rpc_move_cartesian()` - Cartesian motion (optional)
-    - `rpc_execute_trajectory()` - Execute trajectory (optional)
-
-    ### Servo Control (StandardServoComponent)
-    - `rpc_enable_servo()` - Enable motor control
-    - `rpc_disable_servo()` - Disable motor control
-    - `rpc_set_control_mode()` - Set control mode
-    - `rpc_emergency_stop()` - Execute emergency stop
-    - `rpc_clear_errors()` - Clear error states
-    - `rpc_home_robot()` - Home the robot
-
-    ### Status Monitoring (StandardStatusComponent)
-    - `rpc_get_robot_state()` - Get robot state
-    - `rpc_get_capabilities()` - Get capabilities
-    - `rpc_get_system_info()` - Get system information
-    - `rpc_check_connection()` - Check connection status
-
-    ## Standardized Units
-
-    All units are standardized:
-    - Angles: radians
-    - Angular velocity: rad/s
-    - Linear position: meters
-    - Linear velocity: m/s
-    - Force: Newtons
-    - Torque: Nm
-    - Time: seconds
-    """
-
-    # ============= Capabilities Declaration =============
-    capabilities: ManipulatorCapabilities
-
-    # ============= Input Topics (Commands) =============
-    # Core control inputs (all manipulators must support these)
-    joint_position_command: In[JointCommand]  # Target joint positions (rad)
-    joint_velocity_command: In[JointCommand]  # Target joint velocities (rad/s)
-
-    # ============= Output Topics (Feedback) =============
-    # Core feedback (all manipulators must provide these)
-    joint_state: Out[JointState]  # Current positions, velocities, efforts
-    robot_state: Out[RobotState]  # System state and health
-
-    # Optional feedback (capability-dependent)
-    ft_sensor: Out[WrenchStamped] | None  # Force/torque sensor data
-
-    # ============= Component Access =============
-    def get_component(self, component_type: type) -> Any:
-        """Get a component by type.
-
-        Args:
-            component_type: Type of component to retrieve
-
-        Returns:
-            Component instance if found, None otherwise
-
-        Example:
-            motion = driver.get_component(StandardMotionComponent)
-            motion.rpc_move_joint([0, 0, 0, 0, 0, 0])
-        """
-        pass
-
-    def get_rpc_method(self, method_name: str) -> Any:
-        """Get an RPC method by name.
-
-        Args:
-            method_name: Name of the RPC method (e.g., 'rpc_move_joint')
-
-        Returns:
-            Callable method if found, None otherwise
-
-        Example:
-            move_fn = driver.get_rpc_method('rpc_move_joint')
-            result = move_fn(positions=[0, 0, 0, 0, 0, 0])
-        """
-        ...
-
-    def list_rpc_methods(self) -> list[str]:
-        """List all available RPC methods from all components.
-
-        Returns:
-            List of RPC method names
-
-        Example:
-            methods = driver.list_rpc_methods()
-            # ['rpc_move_joint', 'rpc_enable_servo', 'rpc_get_robot_state', ...]
-        """
-        ...
diff --git a/dimos/hardware/manipulators/base/tests/conftest.py b/dimos/hardware/manipulators/base/tests/conftest.py
deleted file mode 100644
index d3e6a4c66d..0000000000
--- a/dimos/hardware/manipulators/base/tests/conftest.py
+++ /dev/null
@@ -1,362 +0,0 @@
-# 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.
-
-"""Pytest fixtures and mocks for manipulator driver tests.
-
-This module contains MockSDK which implements BaseManipulatorSDK with controllable
-behavior for testing driver logic without requiring hardware.
-
-Features:
-- Configurable initial state (positions, DOF, vendor, model)
-- Call tracking for verification
-- Configurable error injection
-- Simulated behavior (e.g., position updates)
-"""
-
-from dataclasses import dataclass, field
-import math
-
-import pytest
-
-from ..sdk_interface import BaseManipulatorSDK, ManipulatorInfo
-
-
-@dataclass
-class MockSDKConfig:
-    """Configuration for MockSDK behavior."""
-
-    dof: int = 6
-    vendor: str = "Mock"
-    model: str = "TestArm"
-    initial_positions: list[float] | None = None
-    initial_velocities: list[float] | None = None
-    initial_efforts: list[float] | None = None
-
-    # Error injection
-    connect_fails: bool = False
-    enable_fails: bool = False
-    motion_fails: bool = False
-    error_code: int = 0
-
-    # Behavior options
-    simulate_motion: bool = False  # If True, set_joint_positions updates internal state
-
-
-@dataclass
-class CallRecord:
-    """Record of a method call for verification."""
-
-    method: str
-    args: tuple = field(default_factory=tuple)
-    kwargs: dict = field(default_factory=dict)
-
-
-class MockSDK(BaseManipulatorSDK):
-    """Mock SDK for unit testing. Implements BaseManipulatorSDK interface.
-
-    Usage:
-        # Basic usage
-        mock = MockSDK()
-        driver = create_driver_with_sdk(mock)
-        driver.enable_servo()
-        assert mock.enable_servos_called
-
-        # With custom config
-        config = MockSDKConfig(dof=7, connect_fails=True)
-        mock = MockSDK(config=config)
-
-        # With initial positions
-        mock = MockSDK(positions=[0.1, 0.2, 0.3, 0.4, 0.5, 0.6])
-
-        # Verify calls
-        mock.set_joint_positions([0.1] * 6)
-        assert mock.was_called("set_joint_positions")
-        assert mock.call_count("set_joint_positions") == 1
-    """
-
-    def __init__(
-        self,
-        config: MockSDKConfig | None = None,
-        *,
-        dof: int = 6,
-        vendor: str = "Mock",
-        model: str = "TestArm",
-        positions: list[float] | None = None,
-    ):
-        """Initialize MockSDK.
-
-        Args:
-            config: Full configuration object (takes precedence)
-            dof: Degrees of freedom (ignored if config provided)
-            vendor: Vendor name (ignored if config provided)
-            model: Model name (ignored if config provided)
-            positions: Initial joint positions (ignored if config provided)
-        """
-        if config is None:
-            config = MockSDKConfig(
-                dof=dof,
-                vendor=vendor,
-                model=model,
-                initial_positions=positions,
-            )
-
-        self._config = config
-        self._dof = config.dof
-        self._vendor = config.vendor
-        self._model = config.model
-
-        # State
-        self._connected = False
-        self._servos_enabled = False
-        self._positions = list(config.initial_positions or [0.0] * self._dof)
-        self._velocities = list(config.initial_velocities or [0.0] * self._dof)
-        self._efforts = list(config.initial_efforts or [0.0] * self._dof)
-        self._mode = 0
-        self._state = 0
-        self._error_code = config.error_code
-
-        # Call tracking
-        self._calls: list[CallRecord] = []
-
-        # Convenience flags for simple assertions
-        self.connect_called = False
-        self.disconnect_called = False
-        self.enable_servos_called = False
-        self.disable_servos_called = False
-        self.set_joint_positions_called = False
-        self.set_joint_velocities_called = False
-        self.stop_motion_called = False
-        self.emergency_stop_called = False
-        self.clear_errors_called = False
-
-    def _record_call(self, method: str, *args, **kwargs):
-        """Record a method call."""
-        self._calls.append(CallRecord(method=method, args=args, kwargs=kwargs))
-
-    def was_called(self, method: str) -> bool:
-        """Check if a method was called."""
-        return any(c.method == method for c in self._calls)
-
-    def call_count(self, method: str) -> int:
-        """Get the number of times a method was called."""
-        return sum(1 for c in self._calls if c.method == method)
-
-    def get_calls(self, method: str) -> list[CallRecord]:
-        """Get all calls to a specific method."""
-        return [c for c in self._calls if c.method == method]
-
-    def get_last_call(self, method: str) -> CallRecord | None:
-        """Get the last call to a specific method."""
-        calls = self.get_calls(method)
-        return calls[-1] if calls else None
-
-    def reset_calls(self):
-        """Reset call tracking."""
-        self._calls.clear()
-        self.connect_called = False
-        self.disconnect_called = False
-        self.enable_servos_called = False
-        self.disable_servos_called = False
-        self.set_joint_positions_called = False
-        self.set_joint_velocities_called = False
-        self.stop_motion_called = False
-        self.emergency_stop_called = False
-        self.clear_errors_called = False
-
-    # ============= State Manipulation (for test setup) =============
-
-    def set_positions(self, positions: list[float]):
-        """Set internal positions (test helper)."""
-        self._positions = list(positions)
-
-    def set_error(self, code: int, message: str = ""):
-        """Inject an error state (test helper)."""
-        self._error_code = code
-
-    def set_enabled(self, enabled: bool):
-        """Set servo enabled state (test helper)."""
-        self._servos_enabled = enabled
-
-    # ============= BaseManipulatorSDK Implementation =============
-
-    def connect(self, config: dict) -> bool:
-        self._record_call("connect", config)
-        self.connect_called = True
-
-        if self._config.connect_fails:
-            return False
-
-        self._connected = True
-        return True
-
-    def disconnect(self) -> None:
-        self._record_call("disconnect")
-        self.disconnect_called = True
-        self._connected = False
-
-    def is_connected(self) -> bool:
-        self._record_call("is_connected")
-        return self._connected
-
-    def get_joint_positions(self) -> list[float]:
-        self._record_call("get_joint_positions")
-        return self._positions.copy()
-
-    def get_joint_velocities(self) -> list[float]:
-        self._record_call("get_joint_velocities")
-        return self._velocities.copy()
-
-    def get_joint_efforts(self) -> list[float]:
-        self._record_call("get_joint_efforts")
-        return self._efforts.copy()
-
-    def set_joint_positions(
-        self,
-        positions: list[float],
-        velocity: float = 1.0,
-        acceleration: float = 1.0,
-        wait: bool = False,
-    ) -> bool:
-        self._record_call(
-            "set_joint_positions",
-            positions,
-            velocity=velocity,
-            acceleration=acceleration,
-            wait=wait,
-        )
-        self.set_joint_positions_called = True
-
-        if self._config.motion_fails:
-            return False
-
-        if not self._servos_enabled:
-            return False
-
-        if self._config.simulate_motion:
-            self._positions = list(positions)
-
-        return True
-
-    def set_joint_velocities(self, velocities: list[float]) -> bool:
-        self._record_call("set_joint_velocities", velocities)
-        self.set_joint_velocities_called = True
-
-        if self._config.motion_fails:
-            return False
-
-        if not self._servos_enabled:
-            return False
-
-        self._velocities = list(velocities)
-        return True
-
-    def set_joint_efforts(self, efforts: list[float]) -> bool:
-        self._record_call("set_joint_efforts", efforts)
-        return False  # Not supported in mock
-
-    def stop_motion(self) -> bool:
-        self._record_call("stop_motion")
-        self.stop_motion_called = True
-        self._velocities = [0.0] * self._dof
-        return True
-
-    def enable_servos(self) -> bool:
-        self._record_call("enable_servos")
-        self.enable_servos_called = True
-
-        if self._config.enable_fails:
-            return False
-
-        self._servos_enabled = True
-        return True
-
-    def disable_servos(self) -> bool:
-        self._record_call("disable_servos")
-        self.disable_servos_called = True
-        self._servos_enabled = False
-        return True
-
-    def are_servos_enabled(self) -> bool:
-        self._record_call("are_servos_enabled")
-        return self._servos_enabled
-
-    def get_robot_state(self) -> dict:
-        self._record_call("get_robot_state")
-        return {
-            "state": self._state,
-            "mode": self._mode,
-            "error_code": self._error_code,
-            "is_moving": any(v != 0 for v in self._velocities),
-        }
-
-    def get_error_code(self) -> int:
-        self._record_call("get_error_code")
-        return self._error_code
-
-    def get_error_message(self) -> str:
-        self._record_call("get_error_message")
-        return "" if self._error_code == 0 else f"Mock error {self._error_code}"
-
-    def clear_errors(self) -> bool:
-        self._record_call("clear_errors")
-        self.clear_errors_called = True
-        self._error_code = 0
-        return True
-
-    def emergency_stop(self) -> bool:
-        self._record_call("emergency_stop")
-        self.emergency_stop_called = True
-        self._velocities = [0.0] * self._dof
-        self._servos_enabled = False
-        return True
-
-    def get_info(self) -> ManipulatorInfo:
-        self._record_call("get_info")
-        return ManipulatorInfo(
-            vendor=self._vendor,
-            model=f"{self._model} (Mock)",
-            dof=self._dof,
-            firmware_version="mock-1.0.0",
-            serial_number="MOCK-001",
-        )
-
-    def get_joint_limits(self) -> tuple[list[float], list[float]]:
-        self._record_call("get_joint_limits")
-        lower = [-2 * math.pi] * self._dof
-        upper = [2 * math.pi] * self._dof
-        return lower, upper
-
-    def get_velocity_limits(self) -> list[float]:
-        self._record_call("get_velocity_limits")
-        return [math.pi] * self._dof
-
-    def get_acceleration_limits(self) -> list[float]:
-        self._record_call("get_acceleration_limits")
-        return [math.pi * 2] * self._dof
-
-
-# ============= Pytest Fixtures =============
-
-
-@pytest.fixture
-def mock_sdk():
-    """Create a basic MockSDK."""
-    return MockSDK(dof=6)
-
-
-@pytest.fixture
-def mock_sdk_with_positions():
-    """Create MockSDK with initial positions."""
-    positions = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
-    return MockSDK(positions=positions)
diff --git a/dimos/hardware/manipulators/base/tests/test_driver_unit.py b/dimos/hardware/manipulators/base/tests/test_driver_unit.py
deleted file mode 100644
index b305d8cd15..0000000000
--- a/dimos/hardware/manipulators/base/tests/test_driver_unit.py
+++ /dev/null
@@ -1,577 +0,0 @@
-# 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.
-
-"""Unit tests for BaseManipulatorDriver.
-
-These tests use MockSDK to test driver logic in isolation without hardware.
-Run with: pytest dimos/hardware/manipulators/base/tests/test_driver_unit.py -v
-"""
-
-import math
-import time
-
-import pytest
-
-from ..components import (
-    StandardMotionComponent,
-    StandardServoComponent,
-    StandardStatusComponent,
-)
-from ..driver import BaseManipulatorDriver
-from .conftest import MockSDK, MockSDKConfig
-
-# =============================================================================
-# Fixtures
-# =============================================================================
-# Note: mock_sdk and mock_sdk_with_positions fixtures are defined in conftest.py
-
-
-@pytest.fixture
-def standard_components():
-    """Create standard component set."""
-    return [
-        StandardMotionComponent(),
-        StandardServoComponent(),
-        StandardStatusComponent(),
-    ]
-
-
-@pytest.fixture
-def driver(mock_sdk, standard_components):
-    """Create a driver with MockSDK and standard components."""
-    config = {"dof": 6}
-    driver = BaseManipulatorDriver(
-        sdk=mock_sdk,
-        components=standard_components,
-        config=config,
-        name="TestDriver",
-    )
-    yield driver
-    # Cleanup - stop driver if running
-    try:
-        driver.stop()
-    except Exception:
-        pass
-
-
-@pytest.fixture
-def started_driver(driver):
-    """Create and start a driver."""
-    driver.start()
-    time.sleep(0.05)  # Allow threads to start
-    yield driver
-
-
-# =============================================================================
-# Connection Tests
-# =============================================================================
-
-
-class TestConnection:
-    """Tests for driver connection behavior."""
-
-    def test_driver_connects_on_init(self, mock_sdk, standard_components):
-        """Driver should connect to SDK during initialization."""
-        config = {"dof": 6}
-        driver = BaseManipulatorDriver(
-            sdk=mock_sdk,
-            components=standard_components,
-            config=config,
-            name="TestDriver",
-        )
-
-        assert mock_sdk.connect_called
-        assert mock_sdk.is_connected()
-        assert driver.shared_state.is_connected
-
-        driver.stop()
-
-    @pytest.mark.skip(
-        reason="Driver init failure leaks LCM threads - needs cleanup fix in Module base class"
-    )
-    def test_connection_failure_raises(self, standard_components):
-        """Driver should raise if SDK connection fails."""
-        config_fail = MockSDKConfig(connect_fails=True)
-        mock_sdk = MockSDK(config=config_fail)
-
-        with pytest.raises(RuntimeError, match="Failed to connect"):
-            BaseManipulatorDriver(
-                sdk=mock_sdk,
-                components=standard_components,
-                config={"dof": 6},
-                name="TestDriver",
-            )
-
-    def test_disconnect_on_stop(self, started_driver, mock_sdk):
-        """Driver should disconnect SDK on stop."""
-        started_driver.stop()
-
-        assert mock_sdk.disconnect_called
-        assert not started_driver.shared_state.is_connected
-
-
-# =============================================================================
-# Joint State Tests
-# =============================================================================
-
-
-class TestJointState:
-    """Tests for joint state reading."""
-
-    def test_get_joint_state_returns_positions(self, driver):
-        """get_joint_state should return current positions."""
-        result = driver.get_joint_state()
-
-        assert result["success"] is True
-        assert len(result["positions"]) == 6
-        assert len(result["velocities"]) == 6
-        assert len(result["efforts"]) == 6
-
-    def test_get_joint_state_with_custom_positions(self, standard_components):
-        """get_joint_state should return SDK positions."""
-        expected_positions = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
-        mock_sdk = MockSDK(positions=expected_positions)
-
-        driver = BaseManipulatorDriver(
-            sdk=mock_sdk,
-            components=standard_components,
-            config={"dof": 6},
-            name="TestDriver",
-        )
-
-        result = driver.get_joint_state()
-
-        assert result["positions"] == expected_positions
-
-        driver.stop()
-
-    def test_shared_state_updated_on_joint_read(self, driver):
-        """Shared state should be updated when reading joints."""
-        # Manually trigger joint state update
-        driver._update_joint_state()
-
-        assert driver.shared_state.joint_positions is not None
-        assert len(driver.shared_state.joint_positions) == 6
-
-
-# =============================================================================
-# Servo Control Tests
-# =============================================================================
-
-
-class TestServoControl:
-    """Tests for servo enable/disable."""
-
-    def test_enable_servo_calls_sdk(self, driver, mock_sdk):
-        """enable_servo should call SDK's enable_servos."""
-        result = driver.enable_servo()
-
-        assert result["success"] is True
-        assert mock_sdk.enable_servos_called
-
-    def test_enable_servo_updates_shared_state(self, driver):
-        """enable_servo should update shared state."""
-        driver.enable_servo()
-
-        # Trigger state update to sync
-        driver._update_robot_state()
-
-        assert driver.shared_state.is_enabled is True
-
-    def test_disable_servo_calls_sdk(self, driver, mock_sdk):
-        """disable_servo should call SDK's disable_servos."""
-        driver.enable_servo()  # Enable first
-        result = driver.disable_servo()
-
-        assert result["success"] is True
-        assert mock_sdk.disable_servos_called
-
-    def test_enable_fails_with_error(self, standard_components):
-        """enable_servo should return failure when SDK fails."""
-        config = MockSDKConfig(enable_fails=True)
-        mock_sdk = MockSDK(config=config)
-
-        driver = BaseManipulatorDriver(
-            sdk=mock_sdk,
-            components=standard_components,
-            config={"dof": 6},
-            name="TestDriver",
-        )
-
-        result = driver.enable_servo()
-
-        assert result["success"] is False
-
-        driver.stop()
-
-
-# =============================================================================
-# Motion Control Tests
-# =============================================================================
-
-
-class TestMotionControl:
-    """Tests for motion commands."""
-
-    def test_move_joint_blocking_calls_sdk(self, driver, mock_sdk):
-        """move_joint with wait=True should call SDK directly."""
-        # Enable servos first (required for motion)
-        driver.enable_servo()
-
-        target = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
-        # Use wait=True to bypass queue and call SDK directly
-        result = driver.move_joint(target, velocity=0.5, wait=True)
-
-        assert result["success"] is True
-        assert mock_sdk.set_joint_positions_called
-
-        # Verify arguments
-        call = mock_sdk.get_last_call("set_joint_positions")
-        assert call is not None
-        assert list(call.args[0]) == target
-        assert call.kwargs["velocity"] == 0.5
-
-    def test_move_joint_async_queues_command(self, driver, mock_sdk):
-        """move_joint with wait=False should queue command."""
-        # Enable servos first
-        driver.enable_servo()
-
-        target = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
-        # Default wait=False queues command
-        result = driver.move_joint(target, velocity=0.5)
-
-        assert result["success"] is True
-        assert result.get("queued") is True
-        # SDK not called yet (command is in queue)
-        assert not mock_sdk.set_joint_positions_called
-        # But command is in the queue
-        assert not driver.command_queue.empty()
-
-    def test_move_joint_fails_without_enable(self, driver, mock_sdk):
-        """move_joint should fail if servos not enabled (blocking mode)."""
-        target = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
-        # Use wait=True to test synchronous failure
-        result = driver.move_joint(target, wait=True)
-
-        assert result["success"] is False
-
-    def test_move_joint_with_simulated_motion(self, standard_components):
-        """With simulate_motion, positions should update (blocking mode)."""
-        config = MockSDKConfig(simulate_motion=True)
-        mock_sdk = MockSDK(config=config)
-
-        driver = BaseManipulatorDriver(
-            sdk=mock_sdk,
-            components=standard_components,
-            config={"dof": 6},
-            name="TestDriver",
-        )
-
-        driver.enable_servo()
-        target = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
-        # Use wait=True to execute directly
-        driver.move_joint(target, wait=True)
-
-        # Check SDK internal state updated
-        assert mock_sdk.get_joint_positions() == target
-
-        driver.stop()
-
-    def test_stop_motion_calls_sdk(self, driver, mock_sdk):
-        """stop_motion should call SDK's stop_motion."""
-        result = driver.stop_motion()
-
-        # stop_motion may return success=False if not moving, but should not error
-        assert result is not None
-        assert mock_sdk.stop_motion_called
-
-    def test_process_command_calls_sdk(self, driver, mock_sdk):
-        """_process_command should execute queued commands."""
-        from ..driver import Command
-
-        driver.enable_servo()
-
-        # Create a position command directly
-        command = Command(
-            type="position",
-            data={"positions": [0.1, 0.2, 0.3, 0.4, 0.5, 0.6], "velocity": 0.5},
-        )
-
-        # Process it directly
-        driver._process_command(command)
-
-        assert mock_sdk.set_joint_positions_called
-
-
-# =============================================================================
-# Robot State Tests
-# =============================================================================
-
-
-class TestRobotState:
-    """Tests for robot state reading."""
-
-    def test_get_robot_state_returns_state(self, driver):
-        """get_robot_state should return state info."""
-        result = driver.get_robot_state()
-
-        assert result["success"] is True
-        assert "state" in result
-        assert "mode" in result
-        assert "error_code" in result
-
-    def test_get_robot_state_with_error(self, standard_components):
-        """get_robot_state should report errors from SDK."""
-        config = MockSDKConfig(error_code=42)
-        mock_sdk = MockSDK(config=config)
-
-        driver = BaseManipulatorDriver(
-            sdk=mock_sdk,
-            components=standard_components,
-            config={"dof": 6},
-            name="TestDriver",
-        )
-
-        result = driver.get_robot_state()
-
-        assert result["error_code"] == 42
-
-        driver.stop()
-
-    def test_clear_errors_calls_sdk(self, driver, mock_sdk):
-        """clear_errors should call SDK's clear_errors."""
-        result = driver.clear_errors()
-
-        assert result["success"] is True
-        assert mock_sdk.clear_errors_called
-
-
-# =============================================================================
-# Joint Limits Tests
-# =============================================================================
-
-
-class TestJointLimits:
-    """Tests for joint limit queries."""
-
-    def test_get_joint_limits_returns_limits(self, driver):
-        """get_joint_limits should return lower and upper limits."""
-        result = driver.get_joint_limits()
-
-        assert result["success"] is True
-        assert len(result["lower"]) == 6
-        assert len(result["upper"]) == 6
-
-    def test_joint_limits_are_reasonable(self, driver):
-        """Joint limits should be reasonable values."""
-        result = driver.get_joint_limits()
-
-        for lower, upper in zip(result["lower"], result["upper"], strict=False):
-            assert lower < upper
-            assert lower >= -2 * math.pi
-            assert upper <= 2 * math.pi
-
-
-# =============================================================================
-# Capabilities Tests
-# =============================================================================
-
-
-class TestCapabilities:
-    """Tests for driver capabilities."""
-
-    def test_capabilities_from_sdk(self, driver):
-        """Driver should get capabilities from SDK."""
-        assert driver.capabilities.dof == 6
-        assert len(driver.capabilities.max_joint_velocity) == 6
-        assert len(driver.capabilities.joint_limits_lower) == 6
-
-    def test_capabilities_with_different_dof(self, standard_components):
-        """Driver should support different DOF arms."""
-        mock_sdk = MockSDK(dof=7)
-
-        driver = BaseManipulatorDriver(
-            sdk=mock_sdk,
-            components=standard_components,
-            config={"dof": 7},
-            name="TestDriver",
-        )
-
-        assert driver.capabilities.dof == 7
-        assert len(driver.capabilities.max_joint_velocity) == 7
-
-        driver.stop()
-
-
-# =============================================================================
-# Component API Exposure Tests
-# =============================================================================
-
-
-class TestComponentAPIExposure:
-    """Tests for auto-exposed component APIs."""
-
-    def test_motion_component_api_exposed(self, driver):
-        """Motion component APIs should be exposed on driver."""
-        assert hasattr(driver, "move_joint")
-        assert hasattr(driver, "stop_motion")
-        assert callable(driver.move_joint)
-
-    def test_servo_component_api_exposed(self, driver):
-        """Servo component APIs should be exposed on driver."""
-        assert hasattr(driver, "enable_servo")
-        assert hasattr(driver, "disable_servo")
-        assert callable(driver.enable_servo)
-
-    def test_status_component_api_exposed(self, driver):
-        """Status component APIs should be exposed on driver."""
-        assert hasattr(driver, "get_joint_state")
-        assert hasattr(driver, "get_robot_state")
-        assert hasattr(driver, "get_joint_limits")
-        assert callable(driver.get_joint_state)
-
-
-# =============================================================================
-# Threading Tests
-# =============================================================================
-
-
-class TestThreading:
-    """Tests for driver threading behavior."""
-
-    def test_start_creates_threads(self, driver):
-        """start() should create control threads."""
-        driver.start()
-        time.sleep(0.05)
-
-        assert len(driver.threads) >= 2
-        assert all(t.is_alive() for t in driver.threads)
-
-        driver.stop()
-
-    def test_stop_terminates_threads(self, started_driver):
-        """stop() should terminate all threads."""
-        started_driver.stop()
-        time.sleep(0.1)
-
-        assert all(not t.is_alive() for t in started_driver.threads)
-
-    def test_stop_calls_sdk_stop_motion(self, started_driver, mock_sdk):
-        """stop() should call SDK stop_motion."""
-        started_driver.stop()
-
-        assert mock_sdk.stop_motion_called
-
-
-# =============================================================================
-# Call Verification Tests (MockSDK features)
-# =============================================================================
-
-
-class TestMockSDKCallTracking:
-    """Tests for MockSDK call tracking features."""
-
-    def test_call_count(self, mock_sdk):
-        """MockSDK should count method calls."""
-        mock_sdk.get_joint_positions()
-        mock_sdk.get_joint_positions()
-        mock_sdk.get_joint_positions()
-
-        assert mock_sdk.call_count("get_joint_positions") == 3
-
-    def test_was_called(self, mock_sdk):
-        """MockSDK.was_called should report if method called."""
-        assert not mock_sdk.was_called("enable_servos")
-
-        mock_sdk.enable_servos()
-
-        assert mock_sdk.was_called("enable_servos")
-
-    def test_get_last_call_args(self, mock_sdk):
-        """MockSDK should record call arguments."""
-        positions = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]
-        mock_sdk.enable_servos()
-        mock_sdk.set_joint_positions(positions, velocity=0.5, wait=True)
-
-        call = mock_sdk.get_last_call("set_joint_positions")
-
-        assert call is not None
-        assert list(call.args[0]) == positions
-        assert call.kwargs["velocity"] == 0.5
-        assert call.kwargs["wait"] is True
-
-    def test_reset_calls(self, mock_sdk):
-        """MockSDK.reset_calls should clear call history."""
-        mock_sdk.enable_servos()
-        mock_sdk.get_joint_positions()
-
-        mock_sdk.reset_calls()
-
-        assert mock_sdk.call_count("enable_servos") == 0
-        assert mock_sdk.call_count("get_joint_positions") == 0
-        assert not mock_sdk.enable_servos_called
-
-
-# =============================================================================
-# Edge Case Tests
-# =============================================================================
-
-
-class TestEdgeCases:
-    """Tests for edge cases and error handling."""
-
-    def test_multiple_enable_calls_optimized(self, driver):
-        """Multiple enable calls should only call SDK once (optimization)."""
-        result1 = driver.enable_servo()
-        result2 = driver.enable_servo()
-        result3 = driver.enable_servo()
-
-        # All calls succeed
-        assert result1["success"] is True
-        assert result2["success"] is True
-        assert result3["success"] is True
-
-        # But SDK only called once (component optimizes redundant calls)
-        assert driver.sdk.call_count("enable_servos") == 1
-
-        # Second and third calls should indicate already enabled
-        assert result2.get("message") == "Servos already enabled"
-        assert result3.get("message") == "Servos already enabled"
-
-    def test_disable_when_already_disabled(self, driver):
-        """Disable when already disabled should return success without SDK call."""
-        # MockSDK starts with servos disabled
-        result = driver.disable_servo()
-
-        assert result["success"] is True
-        assert result.get("message") == "Servos already disabled"
-        # SDK not called since already disabled
-        assert not driver.sdk.disable_servos_called
-
-    def test_disable_after_enable(self, driver):
-        """Disable after enable should call SDK."""
-        driver.enable_servo()
-        result = driver.disable_servo()
-
-        assert result["success"] is True
-        assert driver.sdk.disable_servos_called
-
-    def test_emergency_stop(self, driver):
-        """emergency_stop should disable servos."""
-        driver.enable_servo()
-
-        driver.sdk.emergency_stop()
-
-        assert driver.sdk.emergency_stop_called
-        assert not driver.sdk.are_servos_enabled()
diff --git a/dimos/hardware/manipulators/base/utils/__init__.py b/dimos/hardware/manipulators/base/utils/__init__.py
deleted file mode 100644
index a2dcb2f82e..0000000000
--- a/dimos/hardware/manipulators/base/utils/__init__.py
+++ /dev/null
@@ -1,40 +0,0 @@
-# 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.
-
-"""Shared utilities for manipulator drivers."""
-
-from .converters import degrees_to_radians, meters_to_mm, mm_to_meters, radians_to_degrees
-from .shared_state import SharedState
-from .validators import (
-    clamp_positions,
-    scale_velocities,
-    validate_acceleration_limits,
-    validate_joint_limits,
-    validate_trajectory,
-    validate_velocity_limits,
-)
-
-__all__ = [
-    "SharedState",
-    "clamp_positions",
-    "degrees_to_radians",
-    "meters_to_mm",
-    "mm_to_meters",
-    "radians_to_degrees",
-    "scale_velocities",
-    "validate_acceleration_limits",
-    "validate_joint_limits",
-    "validate_trajectory",
-    "validate_velocity_limits",
-]
diff --git a/dimos/hardware/manipulators/base/utils/converters.py b/dimos/hardware/manipulators/base/utils/converters.py
deleted file mode 100644
index dff5956f8e..0000000000
--- a/dimos/hardware/manipulators/base/utils/converters.py
+++ /dev/null
@@ -1,266 +0,0 @@
-# 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.
-
-"""Unit conversion utilities for manipulator drivers."""
-
-import math
-
-
-def degrees_to_radians(degrees: float | list[float]) -> float | list[float]:
-    """Convert degrees to radians.
-
-    Args:
-        degrees: Angle(s) in degrees
-
-    Returns:
-        Angle(s) in radians
-    """
-    if isinstance(degrees, list):
-        return [math.radians(d) for d in degrees]
-    return math.radians(degrees)
-
-
-def radians_to_degrees(radians: float | list[float]) -> float | list[float]:
-    """Convert radians to degrees.
-
-    Args:
-        radians: Angle(s) in radians
-
-    Returns:
-        Angle(s) in degrees
-    """
-    if isinstance(radians, list):
-        return [math.degrees(r) for r in radians]
-    return math.degrees(radians)
-
-
-def mm_to_meters(mm: float | list[float]) -> float | list[float]:
-    """Convert millimeters to meters.
-
-    Args:
-        mm: Distance(s) in millimeters
-
-    Returns:
-        Distance(s) in meters
-    """
-    if isinstance(mm, list):
-        return [m / 1000.0 for m in mm]
-    return mm / 1000.0
-
-
-def meters_to_mm(meters: float | list[float]) -> float | list[float]:
-    """Convert meters to millimeters.
-
-    Args:
-        meters: Distance(s) in meters
-
-    Returns:
-        Distance(s) in millimeters
-    """
-    if isinstance(meters, list):
-        return [m * 1000.0 for m in meters]
-    return meters * 1000.0
-
-
-def rpm_to_rad_per_sec(rpm: float | list[float]) -> float | list[float]:
-    """Convert RPM to rad/s.
-
-    Args:
-        rpm: Angular velocity in RPM
-
-    Returns:
-        Angular velocity in rad/s
-    """
-    factor = (2 * math.pi) / 60.0
-    if isinstance(rpm, list):
-        return [r * factor for r in rpm]
-    return rpm * factor
-
-
-def rad_per_sec_to_rpm(rad_per_sec: float | list[float]) -> float | list[float]:
-    """Convert rad/s to RPM.
-
-    Args:
-        rad_per_sec: Angular velocity in rad/s
-
-    Returns:
-        Angular velocity in RPM
-    """
-    factor = 60.0 / (2 * math.pi)
-    if isinstance(rad_per_sec, list):
-        return [r * factor for r in rad_per_sec]
-    return rad_per_sec * factor
-
-
-def quaternion_to_euler(qx: float, qy: float, qz: float, qw: float) -> tuple[float, float, float]:
-    """Convert quaternion to Euler angles (roll, pitch, yaw).
-
-    Args:
-        qx, qy, qz, qw: Quaternion components
-
-    Returns:
-        Tuple of (roll, pitch, yaw) in radians
-    """
-    # Roll (x-axis rotation)
-    sinr_cosp = 2 * (qw * qx + qy * qz)
-    cosr_cosp = 1 - 2 * (qx * qx + qy * qy)
-    roll = math.atan2(sinr_cosp, cosr_cosp)
-
-    # Pitch (y-axis rotation)
-    sinp = 2 * (qw * qy - qz * qx)
-    if abs(sinp) >= 1:
-        pitch = math.copysign(math.pi / 2, sinp)  # Use 90 degrees if out of range
-    else:
-        pitch = math.asin(sinp)
-
-    # Yaw (z-axis rotation)
-    siny_cosp = 2 * (qw * qz + qx * qy)
-    cosy_cosp = 1 - 2 * (qy * qy + qz * qz)
-    yaw = math.atan2(siny_cosp, cosy_cosp)
-
-    return roll, pitch, yaw
-
-
-def euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
-    """Convert Euler angles to quaternion.
-
-    Args:
-        roll, pitch, yaw: Euler angles in radians
-
-    Returns:
-        Tuple of (qx, qy, qz, qw) quaternion components
-    """
-    cy = math.cos(yaw * 0.5)
-    sy = math.sin(yaw * 0.5)
-    cp = math.cos(pitch * 0.5)
-    sp = math.sin(pitch * 0.5)
-    cr = math.cos(roll * 0.5)
-    sr = math.sin(roll * 0.5)
-
-    qw = cr * cp * cy + sr * sp * sy
-    qx = sr * cp * cy - cr * sp * sy
-    qy = cr * sp * cy + sr * cp * sy
-    qz = cr * cp * sy - sr * sp * cy
-
-    return qx, qy, qz, qw
-
-
-def pose_dict_to_list(pose: dict[str, float]) -> list[float]:
-    """Convert pose dictionary to list format.
-
-    Args:
-        pose: Dict with keys: x, y, z, roll, pitch, yaw
-
-    Returns:
-        List [x, y, z, roll, pitch, yaw]
-    """
-    return [
-        pose.get("x", 0.0),
-        pose.get("y", 0.0),
-        pose.get("z", 0.0),
-        pose.get("roll", 0.0),
-        pose.get("pitch", 0.0),
-        pose.get("yaw", 0.0),
-    ]
-
-
-def pose_list_to_dict(pose: list[float]) -> dict[str, float]:
-    """Convert pose list to dictionary format.
-
-    Args:
-        pose: List [x, y, z, roll, pitch, yaw]
-
-    Returns:
-        Dict with keys: x, y, z, roll, pitch, yaw
-    """
-    if len(pose) < 6:
-        raise ValueError(f"Pose list must have 6 elements, got {len(pose)}")
-
-    return {
-        "x": pose[0],
-        "y": pose[1],
-        "z": pose[2],
-        "roll": pose[3],
-        "pitch": pose[4],
-        "yaw": pose[5],
-    }
-
-
-def twist_dict_to_list(twist: dict[str, float]) -> list[float]:
-    """Convert twist dictionary to list format.
-
-    Args:
-        twist: Dict with keys: vx, vy, vz, wx, wy, wz
-
-    Returns:
-        List [vx, vy, vz, wx, wy, wz]
-    """
-    return [
-        twist.get("vx", 0.0),
-        twist.get("vy", 0.0),
-        twist.get("vz", 0.0),
-        twist.get("wx", 0.0),
-        twist.get("wy", 0.0),
-        twist.get("wz", 0.0),
-    ]
-
-
-def twist_list_to_dict(twist: list[float]) -> dict[str, float]:
-    """Convert twist list to dictionary format.
-
-    Args:
-        twist: List [vx, vy, vz, wx, wy, wz]
-
-    Returns:
-        Dict with keys: vx, vy, vz, wx, wy, wz
-    """
-    if len(twist) < 6:
-        raise ValueError(f"Twist list must have 6 elements, got {len(twist)}")
-
-    return {
-        "vx": twist[0],
-        "vy": twist[1],
-        "vz": twist[2],
-        "wx": twist[3],
-        "wy": twist[4],
-        "wz": twist[5],
-    }
-
-
-def normalize_angle(angle: float) -> float:
-    """Normalize angle to [-pi, pi].
-
-    Args:
-        angle: Angle in radians
-
-    Returns:
-        Normalized angle in [-pi, pi]
-    """
-    while angle > math.pi:
-        angle -= 2 * math.pi
-    while angle < -math.pi:
-        angle += 2 * math.pi
-    return angle
-
-
-def normalize_angles(angles: list[float]) -> list[float]:
-    """Normalize angles to [-pi, pi].
-
-    Args:
-        angles: Angles in radians
-
-    Returns:
-        Normalized angles in [-pi, pi]
-    """
-    return [normalize_angle(a) for a in angles]
diff --git a/dimos/hardware/manipulators/base/utils/shared_state.py b/dimos/hardware/manipulators/base/utils/shared_state.py
deleted file mode 100644
index 8af275ea17..0000000000
--- a/dimos/hardware/manipulators/base/utils/shared_state.py
+++ /dev/null
@@ -1,255 +0,0 @@
-# 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.
-
-"""Thread-safe shared state for manipulator drivers."""
-
-from dataclasses import dataclass, field
-from threading import Lock
-import time
-from typing import Any
-
-
-@dataclass
-class SharedState:
-    """Thread-safe shared state for manipulator drivers.
-
-    This class holds the current state of the manipulator that needs to be
-    shared between multiple threads (state reader, command sender, publisher).
-    All access should be protected by the lock.
-    """
-
-    # Thread synchronization
-    lock: Lock = field(default_factory=Lock)
-
-    # Joint state (current values from hardware)
-    joint_positions: list[float] | None = None  # radians
-    joint_velocities: list[float] | None = None  # rad/s
-    joint_efforts: list[float] | None = None  # Nm
-
-    # Joint targets (commanded values)
-    target_positions: list[float] | None = None  # radians
-    target_velocities: list[float] | None = None  # rad/s
-    target_efforts: list[float] | None = None  # Nm
-
-    # Cartesian state (if available)
-    cartesian_position: dict[str, float] | None = None  # x,y,z,roll,pitch,yaw
-    cartesian_velocity: dict[str, float] | None = None  # vx,vy,vz,wx,wy,wz
-
-    # Cartesian targets
-    target_cartesian_position: dict[str, float] | None = None
-    target_cartesian_velocity: dict[str, float] | None = None
-
-    # Force/torque sensor (if available)
-    force_torque: list[float] | None = None  # [fx,fy,fz,tx,ty,tz]
-
-    # System state
-    robot_state: int = 0  # 0=idle, 1=moving, 2=error, 3=e-stop
-    control_mode: int = 0  # 0=position, 1=velocity, 2=torque
-    error_code: int = 0  # 0 = no error
-    error_message: str = ""  # Human-readable error
-
-    # Connection and enable status
-    is_connected: bool = False
-    is_enabled: bool = False
-    is_moving: bool = False
-    is_homed: bool = False
-
-    # Gripper state (if available)
-    gripper_position: float | None = None  # meters
-    gripper_force: float | None = None  # Newtons
-
-    # Timestamps
-    last_state_update: float = 0.0
-    last_command_sent: float = 0.0
-    last_error_time: float = 0.0
-
-    # Statistics
-    state_read_count: int = 0
-    command_sent_count: int = 0
-    error_count: int = 0
-
-    def update_joint_state(
-        self,
-        positions: list[float] | None = None,
-        velocities: list[float] | None = None,
-        efforts: list[float] | None = None,
-    ) -> None:
-        """Thread-safe update of joint state.
-
-        Args:
-            positions: Joint positions in radians
-            velocities: Joint velocities in rad/s
-            efforts: Joint efforts in Nm
-        """
-        with self.lock:
-            if positions is not None:
-                self.joint_positions = positions
-            if velocities is not None:
-                self.joint_velocities = velocities
-            if efforts is not None:
-                self.joint_efforts = efforts
-            self.last_state_update = time.time()
-            self.state_read_count += 1
-
-    def update_robot_state(
-        self,
-        state: int | None = None,
-        mode: int | None = None,
-        error_code: int | None = None,
-        error_message: str | None = None,
-    ) -> None:
-        """Thread-safe update of robot state.
-
-        Args:
-            state: Robot state code
-            mode: Control mode code
-            error_code: Error code (0 = no error)
-            error_message: Human-readable error message
-        """
-        with self.lock:
-            if state is not None:
-                self.robot_state = state
-            if mode is not None:
-                self.control_mode = mode
-            if error_code is not None:
-                self.error_code = error_code
-                if error_code != 0:
-                    self.error_count += 1
-                    self.last_error_time = time.time()
-            if error_message is not None:
-                self.error_message = error_message
-
-    def update_cartesian_state(
-        self, position: dict[str, float] | None = None, velocity: dict[str, float] | None = None
-    ) -> None:
-        """Thread-safe update of Cartesian state.
-
-        Args:
-            position: End-effector pose (x,y,z,roll,pitch,yaw)
-            velocity: End-effector twist (vx,vy,vz,wx,wy,wz)
-        """
-        with self.lock:
-            if position is not None:
-                self.cartesian_position = position
-            if velocity is not None:
-                self.cartesian_velocity = velocity
-
-    def set_target_joints(
-        self,
-        positions: list[float] | None = None,
-        velocities: list[float] | None = None,
-        efforts: list[float] | None = None,
-    ) -> None:
-        """Thread-safe update of joint targets.
-
-        Args:
-            positions: Target positions in radians
-            velocities: Target velocities in rad/s
-            efforts: Target efforts in Nm
-        """
-        with self.lock:
-            if positions is not None:
-                self.target_positions = positions
-            if velocities is not None:
-                self.target_velocities = velocities
-            if efforts is not None:
-                self.target_efforts = efforts
-            self.last_command_sent = time.time()
-            self.command_sent_count += 1
-
-    def get_joint_state(
-        self,
-    ) -> tuple[list[float] | None, list[float] | None, list[float] | None]:
-        """Thread-safe read of joint state.
-
-        Returns:
-            Tuple of (positions, velocities, efforts)
-        """
-        with self.lock:
-            return (
-                self.joint_positions.copy() if self.joint_positions else None,
-                self.joint_velocities.copy() if self.joint_velocities else None,
-                self.joint_efforts.copy() if self.joint_efforts else None,
-            )
-
-    def get_robot_state(self) -> dict[str, Any]:
-        """Thread-safe read of robot state.
-
-        Returns:
-            Dict with state information
-        """
-        with self.lock:
-            return {
-                "state": self.robot_state,
-                "mode": self.control_mode,
-                "error_code": self.error_code,
-                "error_message": self.error_message,
-                "is_connected": self.is_connected,
-                "is_enabled": self.is_enabled,
-                "is_moving": self.is_moving,
-                "last_update": self.last_state_update,
-            }
-
-    def get_statistics(self) -> dict[str, Any]:
-        """Get statistics about state updates.
-
-        Returns:
-            Dict with statistics
-        """
-        with self.lock:
-            return {
-                "state_read_count": self.state_read_count,
-                "command_sent_count": self.command_sent_count,
-                "error_count": self.error_count,
-                "last_state_update": self.last_state_update,
-                "last_command_sent": self.last_command_sent,
-                "last_error_time": self.last_error_time,
-            }
-
-    def clear_errors(self) -> None:
-        """Clear error state."""
-        with self.lock:
-            self.error_code = 0
-            self.error_message = ""
-
-    def reset(self) -> None:
-        """Reset all state to initial values."""
-        with self.lock:
-            self.joint_positions = None
-            self.joint_velocities = None
-            self.joint_efforts = None
-            self.target_positions = None
-            self.target_velocities = None
-            self.target_efforts = None
-            self.cartesian_position = None
-            self.cartesian_velocity = None
-            self.target_cartesian_position = None
-            self.target_cartesian_velocity = None
-            self.force_torque = None
-            self.robot_state = 0
-            self.control_mode = 0
-            self.error_code = 0
-            self.error_message = ""
-            self.is_connected = False
-            self.is_enabled = False
-            self.is_moving = False
-            self.is_homed = False
-            self.gripper_position = None
-            self.gripper_force = None
-            self.last_state_update = 0.0
-            self.last_command_sent = 0.0
-            self.last_error_time = 0.0
-            self.state_read_count = 0
-            self.command_sent_count = 0
-            self.error_count = 0
diff --git a/dimos/hardware/manipulators/base/utils/validators.py b/dimos/hardware/manipulators/base/utils/validators.py
deleted file mode 100644
index 3fabdcd306..0000000000
--- a/dimos/hardware/manipulators/base/utils/validators.py
+++ /dev/null
@@ -1,254 +0,0 @@
-# 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.
-
-"""Validation utilities for manipulator drivers."""
-
-from typing import cast
-
-
-def validate_joint_limits(
-    positions: list[float],
-    lower_limits: list[float],
-    upper_limits: list[float],
-    tolerance: float = 0.0,
-) -> tuple[bool, str | None]:
-    """Validate joint positions are within limits.
-
-    Args:
-        positions: Joint positions to validate (radians)
-        lower_limits: Lower joint limits (radians)
-        upper_limits: Upper joint limits (radians)
-        tolerance: Optional tolerance for soft limits (radians)
-
-    Returns:
-        Tuple of (is_valid, error_message)
-        If valid, error_message is None
-    """
-    if len(positions) != len(lower_limits) or len(positions) != len(upper_limits):
-        return False, f"Dimension mismatch: {len(positions)} positions, {len(lower_limits)} limits"
-
-    for i, pos in enumerate(positions):
-        lower = lower_limits[i] - tolerance
-        upper = upper_limits[i] + tolerance
-
-        if pos < lower:
-            return False, f"Joint {i} position {pos:.3f} below limit {lower_limits[i]:.3f}"
-
-        if pos > upper:
-            return False, f"Joint {i} position {pos:.3f} above limit {upper_limits[i]:.3f}"
-
-    return True, None
-
-
-def validate_velocity_limits(
-    velocities: list[float], max_velocities: list[float], scale_factor: float = 1.0
-) -> tuple[bool, str | None]:
-    """Validate joint velocities are within limits.
-
-    Args:
-        velocities: Joint velocities to validate (rad/s)
-        max_velocities: Maximum allowed velocities (rad/s)
-        scale_factor: Optional scaling factor (0-1) to reduce max velocity
-
-    Returns:
-        Tuple of (is_valid, error_message)
-        If valid, error_message is None
-    """
-    if len(velocities) != len(max_velocities):
-        return (
-            False,
-            f"Dimension mismatch: {len(velocities)} velocities, {len(max_velocities)} limits",
-        )
-
-    if scale_factor <= 0 or scale_factor > 1:
-        return False, f"Invalid scale factor: {scale_factor} (must be in (0, 1])"
-
-    for i, vel in enumerate(velocities):
-        max_vel = max_velocities[i] * scale_factor
-
-        if abs(vel) > max_vel:
-            return False, f"Joint {i} velocity {abs(vel):.3f} exceeds limit {max_vel:.3f}"
-
-    return True, None
-
-
-def validate_acceleration_limits(
-    accelerations: list[float], max_accelerations: list[float], scale_factor: float = 1.0
-) -> tuple[bool, str | None]:
-    """Validate joint accelerations are within limits.
-
-    Args:
-        accelerations: Joint accelerations to validate (rad/s²)
-        max_accelerations: Maximum allowed accelerations (rad/s²)
-        scale_factor: Optional scaling factor (0-1) to reduce max acceleration
-
-    Returns:
-        Tuple of (is_valid, error_message)
-        If valid, error_message is None
-    """
-    if len(accelerations) != len(max_accelerations):
-        return (
-            False,
-            f"Dimension mismatch: {len(accelerations)} accelerations, {len(max_accelerations)} limits",
-        )
-
-    if scale_factor <= 0 or scale_factor > 1:
-        return False, f"Invalid scale factor: {scale_factor} (must be in (0, 1])"
-
-    for i, acc in enumerate(accelerations):
-        max_acc = max_accelerations[i] * scale_factor
-
-        if abs(acc) > max_acc:
-            return False, f"Joint {i} acceleration {abs(acc):.3f} exceeds limit {max_acc:.3f}"
-
-    return True, None
-
-
-def validate_trajectory(
-    trajectory: list[dict[str, float | list[float]]],
-    lower_limits: list[float],
-    upper_limits: list[float],
-    max_velocities: list[float] | None = None,
-    max_accelerations: list[float] | None = None,
-) -> tuple[bool, str | None]:
-    """Validate a joint trajectory.
-
-    Args:
-        trajectory: List of waypoints, each with:
-                   - 'positions': list[float] in radians
-                   - 'velocities': Optional list[float] in rad/s
-                   - 'time': float seconds from start
-        lower_limits: Lower joint limits (radians)
-        upper_limits: Upper joint limits (radians)
-        max_velocities: Optional maximum velocities (rad/s)
-        max_accelerations: Optional maximum accelerations (rad/s²)
-
-    Returns:
-        Tuple of (is_valid, error_message)
-        If valid, error_message is None
-    """
-    if not trajectory:
-        return False, "Empty trajectory"
-
-    # Check first waypoint starts at time 0
-    if trajectory[0].get("time", 0) != 0:
-        return False, "Trajectory must start at time 0"
-
-    # Check waypoints are time-ordered
-    prev_time: float = -1.0
-    for i, waypoint in enumerate(trajectory):
-        curr_time = cast("float", waypoint.get("time", 0))
-        if curr_time <= prev_time:
-            return False, f"Waypoint {i} time {curr_time} not after previous {prev_time}"
-        prev_time = curr_time
-
-    # Validate each waypoint
-    for i, waypoint in enumerate(trajectory):
-        # Check required fields
-        if "positions" not in waypoint:
-            return False, f"Waypoint {i} missing positions"
-
-        positions = cast("list[float]", waypoint["positions"])
-
-        # Validate position limits
-        valid, error = validate_joint_limits(positions, lower_limits, upper_limits)
-        if not valid:
-            return False, f"Waypoint {i}: {error}"
-
-        # Validate velocity limits if provided
-        if "velocities" in waypoint and max_velocities:
-            velocities = cast("list[float]", waypoint["velocities"])
-            valid, error = validate_velocity_limits(velocities, max_velocities)
-            if not valid:
-                return False, f"Waypoint {i}: {error}"
-
-    # Check acceleration limits between waypoints
-    if max_accelerations and len(trajectory) > 1:
-        for i in range(1, len(trajectory)):
-            prev = trajectory[i - 1]
-            curr = trajectory[i]
-
-            dt = cast("float", curr["time"]) - cast("float", prev["time"])
-            if dt <= 0:
-                continue
-
-            # Estimate acceleration from position change
-            prev_pos = cast("list[float]", prev["positions"])
-            curr_pos = cast("list[float]", curr["positions"])
-            for j in range(len(prev_pos)):
-                pos_change = curr_pos[j] - prev_pos[j]
-                pos_change / dt
-
-                # If velocities provided, use them for better estimate
-                if "velocities" in prev and "velocities" in curr:
-                    prev_vel = cast("list[float]", prev["velocities"])
-                    curr_vel = cast("list[float]", curr["velocities"])
-                    vel_change = curr_vel[j] - prev_vel[j]
-                    acc = vel_change / dt
-                    if abs(acc) > max_accelerations[j]:
-                        return (
-                            False,
-                            f"Acceleration between waypoint {i - 1} and {i} joint {j}: {abs(acc):.3f} exceeds limit {max_accelerations[j]:.3f}",
-                        )
-
-    return True, None
-
-
-def scale_velocities(
-    velocities: list[float], max_velocities: list[float], scale_factor: float = 0.8
-) -> list[float]:
-    """Scale velocities to stay within limits.
-
-    Args:
-        velocities: Desired velocities (rad/s)
-        max_velocities: Maximum allowed velocities (rad/s)
-        scale_factor: Safety factor (0-1) to stay below limits
-
-    Returns:
-        Scaled velocities that respect limits
-    """
-    if not velocities or not max_velocities:
-        return velocities
-
-    # Find the joint that requires most scaling
-    max_scale = 1.0
-    for vel, max_vel in zip(velocities, max_velocities, strict=False):
-        if max_vel > 0 and abs(vel) > 0:
-            required_scale = abs(vel) / (max_vel * scale_factor)
-            max_scale = max(max_scale, required_scale)
-
-    # Apply uniform scaling to maintain direction
-    if max_scale > 1.0:
-        return [v / max_scale for v in velocities]
-
-    return velocities
-
-
-def clamp_positions(
-    positions: list[float], lower_limits: list[float], upper_limits: list[float]
-) -> list[float]:
-    """Clamp positions to stay within limits.
-
-    Args:
-        positions: Desired positions (radians)
-        lower_limits: Lower joint limits (radians)
-        upper_limits: Upper joint limits (radians)
-
-    Returns:
-        Clamped positions within limits
-    """
-    clamped = []
-    for pos, lower, upper in zip(positions, lower_limits, upper_limits, strict=False):
-        clamped.append(max(lower, min(upper, pos)))
-    return clamped
diff --git a/dimos/hardware/manipulators/mock/__init__.py b/dimos/hardware/manipulators/mock/__init__.py
new file mode 100644
index 0000000000..87428973a4
--- /dev/null
+++ b/dimos/hardware/manipulators/mock/__init__.py
@@ -0,0 +1,28 @@
+# 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.
+
+"""Mock backend 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())
+    >>> 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]
+"""
+
+from dimos.hardware.manipulators.mock.backend import MockBackend
+
+__all__ = ["MockBackend"]
diff --git a/dimos/hardware/manipulators/mock/backend.py b/dimos/hardware/manipulators/mock/backend.py
new file mode 100644
index 0000000000..80b3543739
--- /dev/null
+++ b/dimos/hardware/manipulators/mock/backend.py
@@ -0,0 +1,250 @@
+# 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.
+
+"""Mock backend for testing - no hardware required.
+
+Usage:
+    >>> from dimos.hardware.manipulators.xarm import XArm
+    >>> from dimos.hardware.manipulators.mock import MockBackend
+    >>> arm = XArm(backend=MockBackend())
+    >>> arm.start()  # No hardware!
+"""
+
+import math
+
+from dimos.hardware.manipulators.spec import (
+    ControlMode,
+    JointLimits,
+    ManipulatorInfo,
+)
+
+
+class MockBackend:
+    """Fake backend for unit tests.
+
+    Implements ManipulatorBackend 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:
+        self._dof = dof
+        self._positions = [0.0] * dof
+        self._velocities = [0.0] * dof
+        self._efforts = [0.0] * dof
+        self._enabled = False
+        self._connected = False
+        self._control_mode = ControlMode.POSITION
+        self._cartesian_position: dict[str, float] = {
+            "x": 0.3,
+            "y": 0.0,
+            "z": 0.3,
+            "roll": 0.0,
+            "pitch": 0.0,
+            "yaw": 0.0,
+        }
+        self._gripper_position: float = 0.0
+        self._error_code: int = 0
+        self._error_message: str = ""
+
+    # =========================================================================
+    # Connection
+    # =========================================================================
+
+    def connect(self) -> bool:
+        """Simulate connection."""
+        self._connected = True
+        return True
+
+    def disconnect(self) -> None:
+        """Simulate disconnection."""
+        self._connected = False
+
+    def is_connected(self) -> bool:
+        """Check mock connection status."""
+        return self._connected
+
+    # =========================================================================
+    # Info
+    # =========================================================================
+
+    def get_info(self) -> ManipulatorInfo:
+        """Return mock info."""
+        return ManipulatorInfo(
+            vendor="Mock",
+            model="MockArm",
+            dof=self._dof,
+            firmware_version="1.0.0",
+            serial_number="MOCK-001",
+        )
+
+    def get_dof(self) -> int:
+        """Return DOF."""
+        return self._dof
+
+    def get_limits(self) -> JointLimits:
+        """Return mock joint limits."""
+        return JointLimits(
+            position_lower=[-math.pi] * self._dof,
+            position_upper=[math.pi] * self._dof,
+            velocity_max=[1.0] * self._dof,
+        )
+
+    # =========================================================================
+    # Control Mode
+    # =========================================================================
+
+    def set_control_mode(self, mode: ControlMode) -> bool:
+        """Set mock control mode."""
+        self._control_mode = mode
+        return True
+
+    def get_control_mode(self) -> ControlMode:
+        """Get mock control mode."""
+        return self._control_mode
+
+    # =========================================================================
+    # State Reading
+    # =========================================================================
+
+    def read_joint_positions(self) -> list[float]:
+        """Return mock joint positions."""
+        return self._positions.copy()
+
+    def read_joint_velocities(self) -> list[float]:
+        """Return mock joint velocities."""
+        return self._velocities.copy()
+
+    def read_joint_efforts(self) -> list[float]:
+        """Return mock joint efforts."""
+        return self._efforts.copy()
+
+    def read_state(self) -> dict[str, int]:
+        """Return mock state."""
+        # Use index of control mode as int (0=position, 1=velocity, etc.)
+        mode_int = list(ControlMode).index(self._control_mode)
+        return {
+            "state": 0 if self._enabled else 1,
+            "mode": mode_int,
+        }
+
+    def read_error(self) -> tuple[int, str]:
+        """Return mock error."""
+        return self._error_code, self._error_message
+
+    # =========================================================================
+    # Motion Control
+    # =========================================================================
+
+    def write_joint_positions(
+        self,
+        positions: list[float],
+        velocity: float = 1.0,
+    ) -> bool:
+        """Set mock joint positions (instant move)."""
+        if len(positions) != self._dof:
+            return False
+        self._positions = list(positions)
+        return True
+
+    def write_joint_velocities(self, velocities: list[float]) -> bool:
+        """Set mock joint velocities."""
+        if len(velocities) != self._dof:
+            return False
+        self._velocities = list(velocities)
+        return True
+
+    def write_stop(self) -> bool:
+        """Stop mock motion."""
+        self._velocities = [0.0] * self._dof
+        return True
+
+    # =========================================================================
+    # Servo Control
+    # =========================================================================
+
+    def write_enable(self, enable: bool) -> bool:
+        """Enable/disable mock servos."""
+        self._enabled = enable
+        return True
+
+    def read_enabled(self) -> bool:
+        """Check mock servo state."""
+        return self._enabled
+
+    def write_clear_errors(self) -> bool:
+        """Clear mock errors."""
+        self._error_code = 0
+        self._error_message = ""
+        return True
+
+    # =========================================================================
+    # Cartesian Control (Optional)
+    # =========================================================================
+
+    def read_cartesian_position(self) -> dict[str, float] | None:
+        """Return mock cartesian position."""
+        return self._cartesian_position.copy()
+
+    def write_cartesian_position(
+        self,
+        pose: dict[str, float],
+        velocity: float = 1.0,
+    ) -> bool:
+        """Set mock cartesian position."""
+        self._cartesian_position.update(pose)
+        return True
+
+    # =========================================================================
+    # Gripper (Optional)
+    # =========================================================================
+
+    def read_gripper_position(self) -> float | None:
+        """Return mock gripper position."""
+        return self._gripper_position
+
+    def write_gripper_position(self, position: float) -> bool:
+        """Set mock gripper position."""
+        self._gripper_position = position
+        return True
+
+    # =========================================================================
+    # Force/Torque (Optional)
+    # =========================================================================
+
+    def read_force_torque(self) -> list[float] | None:
+        """Return mock F/T sensor data (not supported in mock)."""
+        return None
+
+    # =========================================================================
+    # Test Helpers (not part of Protocol)
+    # =========================================================================
+
+    def set_error(self, code: int, message: str) -> None:
+        """Inject an error for testing error handling."""
+        self._error_code = code
+        self._error_message = message
+
+    def set_positions(self, positions: list[float]) -> None:
+        """Set positions directly for testing."""
+        self._positions = list(positions)
+
+    def set_efforts(self, efforts: list[float]) -> None:
+        """Set efforts directly for testing."""
+        self._efforts = list(efforts)
+
+
+__all__ = ["MockBackend"]
diff --git a/dimos/hardware/manipulators/piper/README.md b/dimos/hardware/manipulators/piper/README.md
deleted file mode 100644
index 89ff2161ac..0000000000
--- a/dimos/hardware/manipulators/piper/README.md
+++ /dev/null
@@ -1,35 +0,0 @@
-# Piper Driver
-
-Driver for the Piper 6-DOF manipulator with CAN bus communication.
-
-## Supported Features
-
-✅ **Joint Control**
-- Position control
-- Velocity control (integration-based)
-- Joint state feedback at 100Hz
-
-✅ **System Control**
-- Enable/disable motors
-- Emergency stop
-- Error recovery
-
-✅ **Gripper Control**
-- Position and force control
-- Gripper state feedback
-
-## Cartesian Control Limitation
-
-⚠️ **Cartesian control is currently NOT available for the Piper arm.**
-
-### Why?
-The Piper SDK doesn't expose an inverse kinematics (IK) solver that can be called without moving the robot. While the robot can execute Cartesian commands internally, we cannot:
-- Pre-compute joint trajectories for Cartesian paths
-- Validate if a pose is reachable without trying to move there
-- Plan complex Cartesian trajectories offline
-
-### Future Solution
-We will implement a universal IK solver that sits outside the driver layer and works with all arms (XArm, Piper, and future robots), regardless of whether they expose internal IK.
-
-### Current Workaround
-Use joint-space control for now. If you need Cartesian planning, consider using external IK libraries like ikpy or robotics-toolbox-python with the Piper's URDF file.
diff --git a/dimos/hardware/manipulators/piper/__init__.py b/dimos/hardware/manipulators/piper/__init__.py
index acead9f7fb..16c6e451cd 100644
--- a/dimos/hardware/manipulators/piper/__init__.py
+++ b/dimos/hardware/manipulators/piper/__init__.py
@@ -12,21 +12,15 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-"""
-Piper Arm Driver
+"""Piper manipulator hardware backend.
 
-Real-time driver for Piper manipulator with CAN bus communication.
+Usage:
+    >>> from dimos.hardware.manipulators.piper import PiperBackend
+    >>> backend = PiperBackend(can_port="can0")
+    >>> backend.connect()
+    >>> positions = backend.read_joint_positions()
 """
 
-from .piper_blueprints import piper_cartesian, piper_servo, piper_trajectory
-from .piper_driver import PiperDriver, piper_driver
-from .piper_wrapper import PiperSDKWrapper
+from dimos.hardware.manipulators.piper.backend import PiperBackend
 
-__all__ = [
-    "PiperDriver",
-    "PiperSDKWrapper",
-    "piper_cartesian",
-    "piper_driver",
-    "piper_servo",
-    "piper_trajectory",
-]
+__all__ = ["PiperBackend"]
diff --git a/dimos/hardware/manipulators/piper/backend.py b/dimos/hardware/manipulators/piper/backend.py
new file mode 100644
index 0000000000..1ce91dccd1
--- /dev/null
+++ b/dimos/hardware/manipulators/piper/backend.py
@@ -0,0 +1,505 @@
+# 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.
+
+"""Piper backend - implements ManipulatorBackend protocol.
+
+Handles all Piper SDK communication and unit conversion.
+"""
+
+import math
+import time
+from typing import Any
+
+from dimos.hardware.manipulators.spec import (
+    ControlMode,
+    JointLimits,
+    ManipulatorBackend,
+    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
+
+
+class PiperBackend(ManipulatorBackend):
+    """Piper-specific backend.
+
+    Implements ManipulatorBackend protocol via duck typing.
+    No inheritance required - just matching method signatures.
+
+    Unit conversions:
+    - Angles: Piper uses 0.001 degrees, we use radians
+    - Velocities: Piper uses internal units, we use rad/s
+    """
+
+    def __init__(self, can_port: str = "can0", dof: int = 6) -> None:
+        if dof != 6:
+            raise ValueError(f"PiperBackend only supports 6 DOF (got {dof})")
+        self._can_port = can_port
+        self._dof = dof
+        self._sdk: Any = None
+        self._connected: bool = False
+        self._enabled: bool = False
+        self._control_mode: ControlMode = ControlMode.POSITION
+
+    # =========================================================================
+    # Connection
+    # =========================================================================
+
+    def connect(self) -> bool:
+        """Connect to Piper via CAN bus."""
+        try:
+            from piper_sdk import C_PiperInterface_V2
+
+            self._sdk = C_PiperInterface_V2(
+                can_name=self._can_port,
+                judge_flag=True,  # Enable safety checks
+                can_auto_init=True,  # Let SDK handle CAN initialization
+                dh_is_offset=False,
+            )
+
+            # Connect to CAN port
+            self._sdk.ConnectPort(piper_init=True, start_thread=True)
+
+            # Wait for initialization
+            time.sleep(0.025)
+
+            # Check connection by trying to get status
+            status = self._sdk.GetArmStatus()
+            if status is not None:
+                self._connected = True
+                print(f"Piper connected via CAN port {self._can_port}")
+                return True
+            else:
+                print(f"ERROR: Failed to connect to Piper on {self._can_port} - no status received")
+                return False
+
+        except ImportError:
+            print("ERROR: Piper SDK not installed. Please install piper_sdk")
+            return False
+        except Exception as e:
+            print(f"ERROR: Failed to connect to Piper on {self._can_port}: {e}")
+            return False
+
+    def disconnect(self) -> None:
+        """Disconnect from Piper."""
+        if self._sdk:
+            try:
+                if self._enabled:
+                    self._sdk.DisablePiper()
+                    self._enabled = False
+                self._sdk.DisconnectPort()
+            except Exception:
+                pass
+            finally:
+                self._sdk = None
+                self._connected = False
+
+    def is_connected(self) -> bool:
+        """Check if connected to Piper."""
+        if not self._connected or not self._sdk:
+            return False
+
+        try:
+            status = self._sdk.GetArmStatus()
+            return status is not None
+        except Exception:
+            return False
+
+    # =========================================================================
+    # Info
+    # =========================================================================
+
+    def get_info(self) -> ManipulatorInfo:
+        """Get Piper information."""
+        firmware_version = None
+        if self._sdk:
+            try:
+                firmware_version = self._sdk.GetPiperFirmwareVersion()
+            except Exception:
+                pass
+
+        return ManipulatorInfo(
+            vendor="Agilex",
+            model="Piper",
+            dof=self._dof,
+            firmware_version=firmware_version,
+        )
+
+    def get_dof(self) -> int:
+        """Get degrees of freedom."""
+        return self._dof
+
+    def get_limits(self) -> JointLimits:
+        """Get joint limits."""
+        # Piper joint limits (approximate, in radians)
+        lower = [-3.14, -2.35, -2.35, -3.14, -2.35, -3.14]
+        upper = [3.14, 2.35, 2.35, 3.14, 2.35, 3.14]
+        max_vel = [math.pi] * self._dof  # ~180 deg/s
+
+        return JointLimits(
+            position_lower=lower,
+            position_upper=upper,
+            velocity_max=max_vel,
+        )
+
+    # =========================================================================
+    # Control Mode
+    # =========================================================================
+
+    def set_control_mode(self, mode: ControlMode) -> bool:
+        """Set Piper control mode via MotionCtrl_2."""
+        if not self._sdk:
+            return False
+
+        # Piper move modes: 0x01=position, 0x02=velocity
+        # SERVO_POSITION uses position mode for high-freq streaming
+        move_mode = 0x01  # Default position mode
+        if mode == ControlMode.VELOCITY:
+            move_mode = 0x02
+
+        try:
+            self._sdk.MotionCtrl_2(
+                ctrl_mode=0x01,  # CAN control mode
+                move_mode=move_mode,
+                move_spd_rate_ctrl=50,  # Speed rate (0-100)
+                is_mit_mode=0x00,  # Not MIT mode
+            )
+            self._control_mode = mode
+            return True
+        except Exception:
+            return False
+
+    def get_control_mode(self) -> ControlMode:
+        """Get current control mode."""
+        return self._control_mode
+
+    # =========================================================================
+    # State Reading
+    # =========================================================================
+
+    def read_joint_positions(self) -> list[float]:
+        """Read joint positions (Piper units -> radians)."""
+        if not self._sdk:
+            raise RuntimeError("Not connected")
+
+        joint_msgs = self._sdk.GetArmJointMsgs()
+        if not joint_msgs or not joint_msgs.joint_state:
+            raise RuntimeError("Failed to read joint positions")
+
+        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,
+        ]
+
+    def read_joint_velocities(self) -> list[float]:
+        """Read joint velocities.
+
+        Note: Piper doesn't provide real-time velocity feedback.
+        Returns zeros. For velocity estimation, use finite differences.
+        """
+        return [0.0] * self._dof
+
+    def read_joint_efforts(self) -> list[float]:
+        """Read joint efforts/torques.
+
+        Note: Piper doesn't provide torque feedback by default.
+        """
+        return [0.0] * self._dof
+
+    def read_state(self) -> dict[str, int]:
+        """Read robot state."""
+        if not self._sdk:
+            return {"state": 0, "mode": 0}
+
+        try:
+            status = self._sdk.GetArmStatus()
+            if status and status.arm_status:
+                arm_status = status.arm_status
+                error_code = getattr(arm_status, "err_code", 0)
+                state = 2 if error_code != 0 else 0  # 2=error, 0=idle
+                return {
+                    "state": state,
+                    "mode": 0,  # Piper doesn't expose mode
+                    "error_code": error_code,
+                }
+        except Exception:
+            pass
+
+        return {"state": 0, "mode": 0}
+
+    def read_error(self) -> tuple[int, str]:
+        """Read error code and message."""
+        if not self._sdk:
+            return 0, ""
+
+        try:
+            status = self._sdk.GetArmStatus()
+            if status and status.arm_status:
+                error_code = getattr(status.arm_status, "err_code", 0)
+                if error_code == 0:
+                    return 0, ""
+
+                # Piper error codes
+                error_map = {
+                    1: "Communication error",
+                    2: "Motor error",
+                    3: "Encoder error",
+                    4: "Overtemperature",
+                    5: "Overcurrent",
+                    6: "Joint limit error",
+                    7: "Emergency stop",
+                    8: "Power error",
+                }
+                return error_code, error_map.get(error_code, f"Unknown error {error_code}")
+        except Exception:
+            pass
+
+        return 0, ""
+
+    # =========================================================================
+    # Motion Control (Joint Space)
+    # =========================================================================
+
+    def write_joint_positions(
+        self,
+        positions: list[float],
+        velocity: float = 1.0,
+    ) -> bool:
+        """Write joint positions (radians -> Piper units).
+
+        Args:
+            positions: Target positions in radians
+            velocity: Speed as fraction of max (0-1)
+        """
+        if not self._sdk:
+            return False
+
+        # Convert radians to Piper units (0.001 degrees)
+        piper_joints = [round(rad * RAD_TO_PIPER) for rad in positions]
+
+        # Set speed rate if not full speed
+        if velocity < 1.0:
+            speed_rate = int(velocity * 100)
+            try:
+                self._sdk.MotionCtrl_2(
+                    ctrl_mode=0x01,
+                    move_mode=0x01,
+                    move_spd_rate_ctrl=speed_rate,
+                    is_mit_mode=0x00,
+                )
+            except Exception:
+                pass
+
+        try:
+            self._sdk.JointCtrl(
+                piper_joints[0],
+                piper_joints[1],
+                piper_joints[2],
+                piper_joints[3],
+                piper_joints[4],
+                piper_joints[5],
+            )
+            return True
+        except Exception as e:
+            print(f"Piper joint control error: {e}")
+            return False
+
+    def write_joint_velocities(self, velocities: list[float]) -> bool:
+        """Write joint velocities.
+
+        Note: Piper doesn't have native velocity control at SDK level.
+        Returns False - the driver should implement this via position integration.
+        """
+        return False
+
+    def write_stop(self) -> bool:
+        """Emergency stop."""
+        if not self._sdk:
+            return False
+
+        try:
+            if hasattr(self._sdk, "EmergencyStop"):
+                self._sdk.EmergencyStop()
+                return True
+        except Exception:
+            pass
+
+        # Fallback: disable arm
+        return self.write_enable(False)
+
+    # =========================================================================
+    # Servo Control
+    # =========================================================================
+
+    def write_enable(self, enable: bool) -> bool:
+        """Enable or disable servos."""
+        if not self._sdk:
+            return False
+
+        try:
+            if enable:
+                # Enable with retries (500ms max)
+                attempts = 0
+                max_attempts = 50
+                success = False
+                while attempts < max_attempts:
+                    if self._sdk.EnablePiper():
+                        success = True
+                        break
+                    time.sleep(0.01)
+                    attempts += 1
+
+                if success:
+                    self._enabled = True
+                    # Set control mode
+                    self._sdk.MotionCtrl_2(
+                        ctrl_mode=0x01,
+                        move_mode=0x01,
+                        move_spd_rate_ctrl=30,
+                        is_mit_mode=0x00,
+                    )
+                    return True
+                return False
+            else:
+                self._sdk.DisablePiper()
+                self._enabled = False
+                return True
+        except Exception:
+            return False
+
+    def read_enabled(self) -> bool:
+        """Check if servos are enabled."""
+        return self._enabled
+
+    def write_clear_errors(self) -> bool:
+        """Clear error state."""
+        if not self._sdk:
+            return False
+
+        try:
+            if hasattr(self._sdk, "ClearError"):
+                self._sdk.ClearError()
+                return True
+        except Exception:
+            pass
+
+        # Alternative: disable and re-enable
+        self.write_enable(False)
+        time.sleep(0.1)
+        return self.write_enable(True)
+
+    # =========================================================================
+    # Cartesian Control (Optional)
+    # =========================================================================
+
+    def read_cartesian_position(self) -> dict[str, float] | None:
+        """Read end-effector pose.
+
+        Note: Piper may not support direct cartesian feedback.
+        Returns None if not available.
+        """
+        if not self._sdk:
+            return None
+
+        try:
+            if hasattr(self._sdk, "GetArmEndPoseMsgs"):
+                pose_msgs = self._sdk.GetArmEndPoseMsgs()
+                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,
+                    }
+        except Exception:
+            pass
+
+        return None
+
+    def write_cartesian_position(
+        self,
+        pose: dict[str, float],
+        velocity: float = 1.0,
+    ) -> bool:
+        """Write end-effector pose.
+
+        Note: Piper may not support direct cartesian control.
+        """
+        # Cartesian control not commonly supported in Piper SDK
+        return False
+
+    # =========================================================================
+    # Gripper (Optional)
+    # =========================================================================
+
+    def read_gripper_position(self) -> float | None:
+        """Read gripper position (percentage -> meters)."""
+        if not self._sdk:
+            return None
+
+        try:
+            if hasattr(self._sdk, "GetArmGripperMsgs"):
+                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
+        except Exception:
+            pass
+
+        return None
+
+    def write_gripper_position(self, position: float) -> bool:
+        """Write gripper position (meters -> percentage)."""
+        if not self._sdk:
+            return False
+
+        try:
+            if hasattr(self._sdk, "GripperCtrl"):
+                # Convert meters to percentage (0-100)
+                # Assume max opening 0.08m
+                percentage = int((position / 0.08) * 100)
+                percentage = max(0, min(100, percentage))
+                self._sdk.GripperCtrl(percentage, 1000, 0x01, 0)
+                return True
+        except Exception:
+            pass
+
+        return False
+
+    # =========================================================================
+    # Force/Torque Sensor (Optional)
+    # =========================================================================
+
+    def read_force_torque(self) -> list[float] | None:
+        """Read F/T sensor data.
+
+        Note: Piper doesn't typically have F/T sensor.
+        """
+        return None
+
+
+__all__ = ["PiperBackend"]
diff --git a/dimos/hardware/manipulators/piper/can_activate.sh b/dimos/hardware/manipulators/piper/can_activate.sh
deleted file mode 100644
index addb892557..0000000000
--- a/dimos/hardware/manipulators/piper/can_activate.sh
+++ /dev/null
@@ -1,138 +0,0 @@
-#!/bin/bash
-
-# The default CAN name can be set by the user via command-line parameters.
-DEFAULT_CAN_NAME="${1:-can0}"
-
-# The default bitrate for a single CAN module can be set by the user via command-line parameters.
-DEFAULT_BITRATE="${2:-1000000}"
-
-# USB hardware address (optional parameter)
-USB_ADDRESS="${3}"
-echo "-------------------START-----------------------"
-# Check if ethtool is installed.
-if ! dpkg -l | grep -q "ethtool"; then
-    echo "\e[31mError: ethtool not detected in the system.\e[0m"
-    echo "Please use the following command to install ethtool:"
-    echo "sudo apt update && sudo apt install ethtool"
-    exit 1
-fi
-
-# Check if can-utils is installed.
-if ! dpkg -l | grep -q "can-utils"; then
-    echo "\e[31mError: can-utils not detected in the system.\e[0m"
-    echo "Please use the following command to install ethtool:"
-    echo "sudo apt update && sudo apt install can-utils"
-    exit 1
-fi
-
-echo "Both ethtool and can-utils are installed."
-
-# Retrieve the number of CAN modules in the current system.
-CURRENT_CAN_COUNT=$(ip link show type can | grep -c "link/can")
-
-# Verify if the number of CAN modules in the current system matches the expected value.
-if [ "$CURRENT_CAN_COUNT" -ne "1" ]; then
-    if [ -z "$USB_ADDRESS" ]; then
-        # Iterate through all CAN interfaces.
-        for iface in $(ip -br link show type can | awk '{print $1}'); do
-            # Use ethtool to retrieve bus-info.
-            BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}')
-
-            if [ -z "$BUS_INFO" ];then
-                echo "Error: Unable to retrieve bus-info for interface $iface."
-                continue
-            fi
-
-            echo "Interface $iface is inserted into USB port $BUS_INFO"
-        done
-        echo -e " \e[31m Error: The number of CAN modules detected by the system ($CURRENT_CAN_COUNT) does not match the expected number (1). \e[0m"
-        echo -e " \e[31m Please add the USB hardware address parameter, such as: \e[0m"
-        echo -e " bash can_activate.sh can0 1000000 1-2:1.0"
-        echo "-------------------ERROR-----------------------"
-        exit 1
-    fi
-fi
-
-# Load the gs_usb module.
-# sudo modprobe gs_usb
-# if [ $? -ne 0 ]; then
-#     echo "Error: Unable to load the gs_usb module."
-#     exit 1
-# fi
-
-if [ -n "$USB_ADDRESS" ]; then
-    echo "Detected USB hardware address parameter: $USB_ADDRESS"
-
-    # Use ethtool to find the CAN interface corresponding to the USB hardware address.
-    INTERFACE_NAME=""
-    for iface in $(ip -br link show type can | awk '{print $1}'); do
-        BUS_INFO=$(sudo ethtool -i "$iface" | grep "bus-info" | awk '{print $2}')
-        if [ "$BUS_INFO" = "$USB_ADDRESS" ]; then
-            INTERFACE_NAME="$iface"
-            break
-        fi
-    done
-
-    if [ -z "$INTERFACE_NAME" ]; then
-        echo "Error: Unable to find CAN interface corresponding to USB hardware address $USB_ADDRESS."
-        exit 1
-    else
-        echo "Found the interface corresponding to USB hardware address $USB_ADDRESS: $INTERFACE_NAME."
-    fi
-else
-    # Retrieve the unique CAN interface.
-    INTERFACE_NAME=$(ip -br link show type can | awk '{print $1}')
-
-    # Check if the interface name has been retrieved.
-    if [ -z "$INTERFACE_NAME" ]; then
-        echo "Error: Unable to detect CAN interface."
-        exit 1
-    fi
-    BUS_INFO=$(sudo ethtool -i "$INTERFACE_NAME" | grep "bus-info" | awk '{print $2}')
-    echo "Expected to configure a single CAN module, detected interface $INTERFACE_NAME with corresponding USB address $BUS_INFO."
-fi
-
-# Check if the current interface is already activated.
-IS_LINK_UP=$(ip link show "$INTERFACE_NAME" | grep -q "UP" && echo "yes" || echo "no")
-
-# Retrieve the bitrate of the current interface.
-CURRENT_BITRATE=$(ip -details link show "$INTERFACE_NAME" | grep -oP 'bitrate \K\d+')
-
-if [ "$IS_LINK_UP" = "yes" ] && [ "$CURRENT_BITRATE" -eq "$DEFAULT_BITRATE" ]; then
-    echo "Interface $INTERFACE_NAME is already activated with a bitrate of $DEFAULT_BITRATE."
-
-    # Check if the interface name matches the default name.
-    if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then
-        echo "Rename interface $INTERFACE_NAME to $DEFAULT_CAN_NAME."
-        sudo ip link set "$INTERFACE_NAME" down
-        sudo ip link set "$INTERFACE_NAME" name "$DEFAULT_CAN_NAME"
-        sudo ip link set "$DEFAULT_CAN_NAME" up
-        echo "The interface has been renamed to $DEFAULT_CAN_NAME and reactivated."
-    else
-        echo "The interface name is already $DEFAULT_CAN_NAME."
-    fi
-else
-    # If the interface is not activated or the bitrate is different, configure it.
-    if [ "$IS_LINK_UP" = "yes" ]; then
-        echo "Interface $INTERFACE_NAME is already activated, but the bitrate is $CURRENT_BITRATE, which does not match the set value of $DEFAULT_BITRATE."
-    else
-        echo "Interface $INTERFACE_NAME is not activated or bitrate is not set."
-    fi
-
-    # Set the interface bitrate and activate it.
-    sudo ip link set "$INTERFACE_NAME" down
-    sudo ip link set "$INTERFACE_NAME" type can bitrate $DEFAULT_BITRATE
-    sudo ip link set "$INTERFACE_NAME" up
-    echo "Interface $INTERFACE_NAME has been reset to bitrate $DEFAULT_BITRATE and activated."
-
-    # Rename the interface to the default name.
-    if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then
-        echo "Rename interface $INTERFACE_NAME to $DEFAULT_CAN_NAME."
-        sudo ip link set "$INTERFACE_NAME" down
-        sudo ip link set "$INTERFACE_NAME" name "$DEFAULT_CAN_NAME"
-        sudo ip link set "$DEFAULT_CAN_NAME" up
-        echo "The interface has been renamed to $DEFAULT_CAN_NAME and reactivated."
-    fi
-fi
-
-echo "-------------------OVER------------------------"
diff --git a/dimos/hardware/manipulators/piper/components/__init__.py b/dimos/hardware/manipulators/piper/components/__init__.py
deleted file mode 100644
index 2c6d863ca1..0000000000
--- a/dimos/hardware/manipulators/piper/components/__init__.py
+++ /dev/null
@@ -1,17 +0,0 @@
-"""Component classes for PiperDriver."""
-
-from .configuration import ConfigurationComponent
-from .gripper_control import GripperControlComponent
-from .kinematics import KinematicsComponent
-from .motion_control import MotionControlComponent
-from .state_queries import StateQueryComponent
-from .system_control import SystemControlComponent
-
-__all__ = [
-    "ConfigurationComponent",
-    "GripperControlComponent",
-    "KinematicsComponent",
-    "MotionControlComponent",
-    "StateQueryComponent",
-    "SystemControlComponent",
-]
diff --git a/dimos/hardware/manipulators/piper/components/configuration.py b/dimos/hardware/manipulators/piper/components/configuration.py
deleted file mode 100644
index b7ac53c371..0000000000
--- a/dimos/hardware/manipulators/piper/components/configuration.py
+++ /dev/null
@@ -1,348 +0,0 @@
-# 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.
-
-"""
-Configuration Component for PiperDriver.
-
-Provides RPC methods for configuring robot parameters including:
-- Joint parameters (limits, speeds, acceleration)
-- End-effector parameters (speed, acceleration)
-- Collision protection
-- Motor configuration
-"""
-
-from typing import Any
-
-from dimos.core import rpc
-from dimos.utils.logging_config import setup_logger
-
-logger = setup_logger()
-
-
-class ConfigurationComponent:
-    """
-    Component providing configuration RPC methods for PiperDriver.
-
-    This component assumes the parent class has:
-    - self.piper: C_PiperInterface_V2 instance
-    - self.config: PiperDriverConfig instance
-    """
-
-    # Type hints for attributes provided by parent class
-    piper: Any
-    config: Any
-
-    @rpc
-    def set_joint_config(
-        self,
-        motor_num: int,
-        kp_factor: int,
-        ki_factor: int,
-        kd_factor: int,
-        ke_factor: int = 0,
-    ) -> tuple[bool, str]:
-        """
-        Configure joint control parameters.
-
-        Args:
-            motor_num: Motor number (1-6)
-            kp_factor: Proportional gain factor
-            ki_factor: Integral gain factor
-            kd_factor: Derivative gain factor
-            ke_factor: Error gain factor
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            if motor_num not in range(1, 7):
-                return (False, f"Invalid motor_num: {motor_num}. Must be 1-6")
-
-            result = self.piper.JointConfig(motor_num, kp_factor, ki_factor, kd_factor, ke_factor)
-
-            if result:
-                return (True, f"Joint {motor_num} configuration set successfully")
-            else:
-                return (False, f"Failed to configure joint {motor_num}")
-
-        except Exception as e:
-            logger.error(f"set_joint_config failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def set_joint_max_acc(self, motor_num: int, max_joint_acc: int) -> tuple[bool, str]:
-        """
-        Set joint maximum acceleration.
-
-        Args:
-            motor_num: Motor number (1-6)
-            max_joint_acc: Maximum joint acceleration
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            if motor_num not in range(1, 7):
-                return (False, f"Invalid motor_num: {motor_num}. Must be 1-6")
-
-            result = self.piper.JointMaxAccConfig(motor_num, max_joint_acc)
-
-            if result:
-                return (True, f"Joint {motor_num} max acceleration set to {max_joint_acc}")
-            else:
-                return (False, f"Failed to set max acceleration for joint {motor_num}")
-
-        except Exception as e:
-            logger.error(f"set_joint_max_acc failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def set_motor_angle_limit_max_speed(
-        self,
-        motor_num: int,
-        min_joint_angle: int,
-        max_joint_angle: int,
-        max_joint_speed: int,
-    ) -> tuple[bool, str]:
-        """
-        Set motor angle limits and maximum speed.
-
-        Args:
-            motor_num: Motor number (1-6)
-            min_joint_angle: Minimum joint angle (in Piper units: 0.001 degrees)
-            max_joint_angle: Maximum joint angle (in Piper units: 0.001 degrees)
-            max_joint_speed: Maximum joint speed
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            if motor_num not in range(1, 7):
-                return (False, f"Invalid motor_num: {motor_num}. Must be 1-6")
-
-            result = self.piper.MotorAngleLimitMaxSpdSet(
-                motor_num, min_joint_angle, max_joint_angle, max_joint_speed
-            )
-
-            if result:
-                return (
-                    True,
-                    f"Joint {motor_num} angle limits and max speed set successfully",
-                )
-            else:
-                return (False, f"Failed to set angle limits for joint {motor_num}")
-
-        except Exception as e:
-            logger.error(f"set_motor_angle_limit_max_speed failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def set_motor_max_speed(self, motor_num: int, max_joint_spd: int) -> tuple[bool, str]:
-        """
-        Set motor maximum speed.
-
-        Args:
-            motor_num: Motor number (1-6)
-            max_joint_spd: Maximum joint speed
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            if motor_num not in range(1, 7):
-                return (False, f"Invalid motor_num: {motor_num}. Must be 1-6")
-
-            result = self.piper.MotorMaxSpdSet(motor_num, max_joint_spd)
-
-            if result:
-                return (True, f"Joint {motor_num} max speed set to {max_joint_spd}")
-            else:
-                return (False, f"Failed to set max speed for joint {motor_num}")
-
-        except Exception as e:
-            logger.error(f"set_motor_max_speed failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def set_end_speed_and_acc(
-        self,
-        end_max_linear_vel: int,
-        end_max_angular_vel: int,
-        end_max_linear_acc: int,
-        end_max_angular_acc: int,
-    ) -> tuple[bool, str]:
-        """
-        Set end-effector speed and acceleration parameters.
-
-        Args:
-            end_max_linear_vel: Maximum linear velocity
-            end_max_angular_vel: Maximum angular velocity
-            end_max_linear_acc: Maximum linear acceleration
-            end_max_angular_acc: Maximum angular acceleration
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.EndSpdAndAccParamSet(
-                end_max_linear_vel,
-                end_max_angular_vel,
-                end_max_linear_acc,
-                end_max_angular_acc,
-            )
-
-            if result:
-                return (True, "End-effector speed and acceleration parameters set successfully")
-            else:
-                return (False, "Failed to set end-effector parameters")
-
-        except Exception as e:
-            logger.error(f"set_end_speed_and_acc failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def set_crash_protection_level(self, level: int) -> tuple[bool, str]:
-        """
-        Set collision/crash protection level.
-
-        Args:
-            level: Protection level (0=disabled, higher values = more sensitive)
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.CrashProtectionConfig(level)
-
-            if result:
-                return (True, f"Crash protection level set to {level}")
-            else:
-                return (False, "Failed to set crash protection level")
-
-        except Exception as e:
-            logger.error(f"set_crash_protection_level failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def search_motor_max_angle_speed_acc_limit(self, motor_num: int) -> tuple[bool, str]:
-        """
-        Search for motor maximum angle, speed, and acceleration limits.
-
-        Args:
-            motor_num: Motor number (1-6)
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            if motor_num not in range(1, 7):
-                return (False, f"Invalid motor_num: {motor_num}. Must be 1-6")
-
-            result = self.piper.SearchMotorMaxAngleSpdAccLimit(motor_num)
-
-            if result:
-                return (True, f"Search initiated for motor {motor_num} limits")
-            else:
-                return (False, f"Failed to search limits for motor {motor_num}")
-
-        except Exception as e:
-            logger.error(f"search_motor_max_angle_speed_acc_limit failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def search_all_motor_max_angle_speed(self) -> tuple[bool, str]:
-        """
-        Search for all motors' maximum angle and speed limits.
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.SearchAllMotorMaxAngleSpd()
-
-            if result:
-                return (True, "Search initiated for all motor angle/speed limits")
-            else:
-                return (False, "Failed to search all motor limits")
-
-        except Exception as e:
-            logger.error(f"search_all_motor_max_angle_speed failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def search_all_motor_max_acc_limit(self) -> tuple[bool, str]:
-        """
-        Search for all motors' maximum acceleration limits.
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.SearchAllMotorMaxAccLimit()
-
-            if result:
-                return (True, "Search initiated for all motor acceleration limits")
-            else:
-                return (False, "Failed to search all motor acceleration limits")
-
-        except Exception as e:
-            logger.error(f"search_all_motor_max_acc_limit failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def set_sdk_joint_limit_param(
-        self, joint_limits: list[tuple[float, float]]
-    ) -> tuple[bool, str]:
-        """
-        Set SDK joint limit parameters.
-
-        Args:
-            joint_limits: List of (min_angle, max_angle) tuples for each joint in radians
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            if len(joint_limits) != 6:
-                return (False, f"Expected 6 joint limit tuples, got {len(joint_limits)}")
-
-            # Convert to Piper units and call SDK method
-            # Note: Actual SDK method signature may vary
-            logger.info(f"Setting SDK joint limits: {joint_limits}")
-            return (True, "SDK joint limits set (method may vary by SDK version)")
-
-        except Exception as e:
-            logger.error(f"set_sdk_joint_limit_param failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def set_sdk_gripper_range_param(self, min_range: int, max_range: int) -> tuple[bool, str]:
-        """
-        Set SDK gripper range parameters.
-
-        Args:
-            min_range: Minimum gripper range
-            max_range: Maximum gripper range
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            # Note: Actual SDK method signature may vary
-            logger.info(f"Setting SDK gripper range: {min_range} - {max_range}")
-            return (True, "SDK gripper range set (method may vary by SDK version)")
-
-        except Exception as e:
-            logger.error(f"set_sdk_gripper_range_param failed: {e}")
-            return (False, str(e))
diff --git a/dimos/hardware/manipulators/piper/components/gripper_control.py b/dimos/hardware/manipulators/piper/components/gripper_control.py
deleted file mode 100644
index 5f500097cd..0000000000
--- a/dimos/hardware/manipulators/piper/components/gripper_control.py
+++ /dev/null
@@ -1,120 +0,0 @@
-# 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.
-
-"""
-Gripper Control Component for PiperDriver.
-
-Provides RPC methods for gripper control operations.
-"""
-
-from typing import Any
-
-from dimos.core import rpc
-from dimos.utils.logging_config import setup_logger
-
-logger = setup_logger()
-
-
-class GripperControlComponent:
-    """
-    Component providing gripper control RPC methods for PiperDriver.
-
-    This component assumes the parent class has:
-    - self.piper: C_PiperInterface_V2 instance
-    - self.config: PiperDriverConfig instance
-    """
-
-    # Type hints for attributes provided by parent class
-    piper: Any
-    config: Any
-
-    @rpc
-    def set_gripper(
-        self,
-        gripper_angle: int,
-        gripper_effort: int = 100,
-        gripper_enable: int = 0x01,
-        gripper_state: int = 0x00,
-    ) -> tuple[bool, str]:
-        """
-        Set gripper position and parameters.
-
-        Args:
-            gripper_angle: Gripper angle (0-1000, 0=closed, 1000=open)
-            gripper_effort: Gripper effort/force (0-1000)
-            gripper_enable: Gripper enable (0x00=disabled, 0x01=enabled)
-            gripper_state: Gripper state
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.GripperCtrl(
-                gripper_angle, gripper_effort, gripper_enable, gripper_state
-            )
-
-            if result:
-                return (True, f"Gripper set to angle={gripper_angle}, effort={gripper_effort}")
-            else:
-                return (False, "Failed to set gripper")
-
-        except Exception as e:
-            logger.error(f"set_gripper failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def open_gripper(self, effort: int = 100) -> tuple[bool, str]:
-        """
-        Open gripper.
-
-        Args:
-            effort: Gripper effort (0-1000)
-
-        Returns:
-            Tuple of (success, message)
-        """
-        result: tuple[bool, str] = self.set_gripper(gripper_angle=1000, gripper_effort=effort)
-        return result
-
-    @rpc
-    def close_gripper(self, effort: int = 100) -> tuple[bool, str]:
-        """
-        Close gripper.
-
-        Args:
-            effort: Gripper effort (0-1000)
-
-        Returns:
-            Tuple of (success, message)
-        """
-        result: tuple[bool, str] = self.set_gripper(gripper_angle=0, gripper_effort=effort)
-        return result
-
-    @rpc
-    def set_gripper_zero(self) -> tuple[bool, str]:
-        """
-        Set gripper zero position.
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            # This method may require specific SDK implementation
-            # For now, we'll just document it
-            logger.info("set_gripper_zero called - implementation may vary by SDK version")
-            return (True, "Gripper zero set (if supported by SDK)")
-
-        except Exception as e:
-            logger.error(f"set_gripper_zero failed: {e}")
-            return (False, str(e))
diff --git a/dimos/hardware/manipulators/piper/components/kinematics.py b/dimos/hardware/manipulators/piper/components/kinematics.py
deleted file mode 100644
index 51be97a764..0000000000
--- a/dimos/hardware/manipulators/piper/components/kinematics.py
+++ /dev/null
@@ -1,116 +0,0 @@
-# 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.
-
-"""
-Kinematics Component for PiperDriver.
-
-Provides RPC methods for kinematic calculations including:
-- Forward kinematics
-"""
-
-from typing import Any
-
-from dimos.core import rpc
-from dimos.utils.logging_config import setup_logger
-
-logger = setup_logger()
-
-
-class KinematicsComponent:
-    """
-    Component providing kinematics RPC methods for PiperDriver.
-
-    This component assumes the parent class has:
-    - self.piper: C_PiperInterface_V2 instance
-    - self.config: PiperDriverConfig instance
-    - PIPER_TO_RAD: conversion constant (0.001 degrees → radians)
-    """
-
-    # Type hints for attributes provided by parent class
-    piper: Any
-    config: Any
-
-    @rpc
-    def get_forward_kinematics(
-        self, mode: str = "feedback"
-    ) -> tuple[bool, dict[str, float] | None]:
-        """
-        Compute forward kinematics.
-
-        Args:
-            mode: "feedback" for current joint angles, "control" for commanded angles
-
-        Returns:
-            Tuple of (success, pose_dict) with keys: x, y, z, rx, ry, rz
-        """
-        try:
-            fk_result = self.piper.GetFK(mode=mode)
-
-            if fk_result is not None:
-                # Convert from Piper units
-                pose_dict = {
-                    "x": fk_result[0] * 0.001,  # 0.001 mm → mm
-                    "y": fk_result[1] * 0.001,
-                    "z": fk_result[2] * 0.001,
-                    "rx": fk_result[3] * 0.001 * (3.14159 / 180.0),  # → rad
-                    "ry": fk_result[4] * 0.001 * (3.14159 / 180.0),
-                    "rz": fk_result[5] * 0.001 * (3.14159 / 180.0),
-                }
-                return (True, pose_dict)
-            else:
-                return (False, None)
-
-        except Exception as e:
-            logger.error(f"get_forward_kinematics failed: {e}")
-            return (False, None)
-
-    @rpc
-    def enable_fk_calculation(self) -> tuple[bool, str]:
-        """
-        Enable forward kinematics calculation.
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.EnableFkCal()
-
-            if result:
-                return (True, "FK calculation enabled")
-            else:
-                return (False, "Failed to enable FK calculation")
-
-        except Exception as e:
-            logger.error(f"enable_fk_calculation failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def disable_fk_calculation(self) -> tuple[bool, str]:
-        """
-        Disable forward kinematics calculation.
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.DisableFkCal()
-
-            if result:
-                return (True, "FK calculation disabled")
-            else:
-                return (False, "Failed to disable FK calculation")
-
-        except Exception as e:
-            logger.error(f"disable_fk_calculation failed: {e}")
-            return (False, str(e))
diff --git a/dimos/hardware/manipulators/piper/components/motion_control.py b/dimos/hardware/manipulators/piper/components/motion_control.py
deleted file mode 100644
index 7a0dc36eed..0000000000
--- a/dimos/hardware/manipulators/piper/components/motion_control.py
+++ /dev/null
@@ -1,286 +0,0 @@
-# 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.
-
-"""
-Motion Control Component for PiperDriver.
-
-Provides RPC methods for motion control operations including:
-- Joint position control
-- Joint velocity control
-- End-effector pose control
-- Emergency stop
-- Circular motion
-"""
-
-import math
-import time
-from typing import Any
-
-from dimos.core import rpc
-from dimos.utils.logging_config import setup_logger
-
-logger = setup_logger()
-
-
-class MotionControlComponent:
-    """
-    Component providing motion control RPC methods for PiperDriver.
-
-    This component assumes the parent class has:
-    - self.piper: C_PiperInterface_V2 instance
-    - self.config: PiperDriverConfig instance
-    - RAD_TO_PIPER: conversion constant (radians → 0.001 degrees)
-    - PIPER_TO_RAD: conversion constant (0.001 degrees → radians)
-    """
-
-    # Type hints for attributes expected from parent class
-    piper: Any
-    config: Any
-    RAD_TO_PIPER: float
-    PIPER_TO_RAD: float
-    _joint_cmd_lock: Any
-    _joint_cmd_: Any
-    _vel_cmd_: Any
-    _last_cmd_time: float
-
-    @rpc
-    def set_joint_angles(self, angles: list[float], gripper_state: int = 0x00) -> tuple[bool, str]:
-        """
-        Set joint angles (RPC method).
-
-        Args:
-            angles: List of joint angles in radians
-            gripper_state: Gripper state (0x00 = no change, 0x01 = open, 0x02 = close)
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            if len(angles) != 6:
-                return (False, f"Expected 6 joint angles, got {len(angles)}")
-
-            # Convert radians to Piper units (0.001 degrees)
-            piper_joints = [round(rad * self.RAD_TO_PIPER) for rad in angles]
-
-            # Send joint control command
-            result = self.piper.JointCtrl(
-                piper_joints[0],
-                piper_joints[1],
-                piper_joints[2],
-                piper_joints[3],
-                piper_joints[4],
-                piper_joints[5],
-                gripper_state,
-            )
-
-            if result:
-                return (True, "Joint angles set successfully")
-            else:
-                return (False, "Failed to set joint angles")
-
-        except Exception as e:
-            logger.error(f"set_joint_angles failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def set_joint_command(self, positions: list[float]) -> tuple[bool, str]:
-        """
-        Manually set the joint command (for testing).
-        This updates the shared joint_cmd that the control loop reads.
-
-        Args:
-            positions: List of joint positions in radians
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            if len(positions) != 6:
-                return (False, f"Expected 6 joint positions, got {len(positions)}")
-
-            with self._joint_cmd_lock:
-                self._joint_cmd_ = list(positions)
-
-            logger.info(f"✓ Joint command set: {[f'{math.degrees(p):.2f}°' for p in positions]}")
-            return (True, "Joint command updated")
-        except Exception as e:
-            return (False, str(e))
-
-    @rpc
-    def set_end_pose(
-        self, x: float, y: float, z: float, rx: float, ry: float, rz: float
-    ) -> tuple[bool, str]:
-        """
-        Set end-effector pose.
-
-        Args:
-            x: X position in millimeters
-            y: Y position in millimeters
-            z: Z position in millimeters
-            rx: Roll in radians
-            ry: Pitch in radians
-            rz: Yaw in radians
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            # Convert to Piper units
-            # Position: mm → 0.001 mm
-            x_piper = round(x * 1000)
-            y_piper = round(y * 1000)
-            z_piper = round(z * 1000)
-
-            # Rotation: radians → 0.001 degrees
-            rx_piper = round(math.degrees(rx) * 1000)
-            ry_piper = round(math.degrees(ry) * 1000)
-            rz_piper = round(math.degrees(rz) * 1000)
-
-            # Send end pose control command
-            result = self.piper.EndPoseCtrl(x_piper, y_piper, z_piper, rx_piper, ry_piper, rz_piper)
-
-            if result:
-                return (True, "End pose set successfully")
-            else:
-                return (False, "Failed to set end pose")
-
-        except Exception as e:
-            logger.error(f"set_end_pose failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def emergency_stop(self) -> tuple[bool, str]:
-        """Emergency stop the arm."""
-        try:
-            result = self.piper.EmergencyStop()
-
-            if result:
-                logger.warning("Emergency stop activated")
-                return (True, "Emergency stop activated")
-            else:
-                return (False, "Failed to activate emergency stop")
-
-        except Exception as e:
-            logger.error(f"emergency_stop failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def move_c_axis_update(self, instruction_num: int = 0x00) -> tuple[bool, str]:
-        """
-        Update circular motion axis.
-
-        Args:
-            instruction_num: Instruction number (0x00, 0x01, 0x02, 0x03)
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            if instruction_num not in [0x00, 0x01, 0x02, 0x03]:
-                return (False, f"Invalid instruction_num: {instruction_num}")
-
-            result = self.piper.MoveCAxisUpdateCtrl(instruction_num)
-
-            if result:
-                return (True, f"Move C axis updated with instruction {instruction_num}")
-            else:
-                return (False, "Failed to update Move C axis")
-
-        except Exception as e:
-            logger.error(f"move_c_axis_update failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def set_joint_mit_ctrl(
-        self,
-        motor_num: int,
-        pos_target: float,
-        vel_target: float,
-        torq_target: float,
-        kp: int,
-        kd: int,
-    ) -> tuple[bool, str]:
-        """
-        Set joint MIT (Model-based Inverse Torque) control.
-
-        Args:
-            motor_num: Motor number (1-6)
-            pos_target: Target position in radians
-            vel_target: Target velocity in rad/s
-            torq_target: Target torque in Nm
-            kp: Proportional gain (0-100)
-            kd: Derivative gain (0-100)
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            if motor_num not in range(1, 7):
-                return (False, f"Invalid motor_num: {motor_num}. Must be 1-6")
-
-            # Convert to Piper units
-            pos_piper = round(pos_target * self.RAD_TO_PIPER)
-            vel_piper = round(vel_target * self.RAD_TO_PIPER)
-            torq_piper = round(torq_target * 1000)  # Torque in millinewton-meters
-
-            result = self.piper.JointMitCtrl(motor_num, pos_piper, vel_piper, torq_piper, kp, kd)
-
-            if result:
-                return (True, f"Joint {motor_num} MIT control set successfully")
-            else:
-                return (False, f"Failed to set MIT control for joint {motor_num}")
-
-        except Exception as e:
-            logger.error(f"set_joint_mit_ctrl failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def set_joint_velocities(self, velocities: list[float]) -> tuple[bool, str]:
-        """
-        Set joint velocities (RPC method).
-
-        Requires velocity control mode to be enabled.
-
-        The control loop integrates velocities to positions:
-        - position_target += velocity * dt
-        - Integrated positions are sent to JointCtrl
-
-        This provides smooth velocity control while using the proven position API.
-
-        Args:
-            velocities: List of 6 joint velocities in rad/s
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            if len(velocities) != 6:
-                return (False, f"Expected 6 velocities, got {len(velocities)}")
-
-            if not self.config.velocity_control:
-                return (
-                    False,
-                    "Velocity control mode not enabled. Call enable_velocity_control_mode() first.",
-                )
-
-            with self._joint_cmd_lock:
-                self._vel_cmd_ = list(velocities)
-                self._last_cmd_time = time.time()
-
-            logger.info(f"✓ Velocity command set: {[f'{v:.3f} rad/s' for v in velocities]}")
-            return (True, "Velocity command updated")
-
-        except Exception as e:
-            logger.error(f"set_joint_velocities failed: {e}")
-            return (False, str(e))
diff --git a/dimos/hardware/manipulators/piper/components/state_queries.py b/dimos/hardware/manipulators/piper/components/state_queries.py
deleted file mode 100644
index 3fe00fffc6..0000000000
--- a/dimos/hardware/manipulators/piper/components/state_queries.py
+++ /dev/null
@@ -1,340 +0,0 @@
-# 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.
-
-"""
-State Query Component for PiperDriver.
-
-Provides RPC methods for querying robot state including:
-- Joint state
-- Robot state
-- End-effector pose
-- Gripper state
-- Motor information
-- Firmware version
-"""
-
-import threading
-from typing import Any
-
-from dimos.core import rpc
-from dimos.msgs.sensor_msgs import JointState, RobotState
-from dimos.utils.logging_config import setup_logger
-
-logger = setup_logger()
-
-
-class StateQueryComponent:
-    """
-    Component providing state query RPC methods for PiperDriver.
-
-    This component assumes the parent class has:
-    - self.piper: C_PiperInterface_V2 instance
-    - self.config: PiperDriverConfig instance
-    - self._joint_state_lock: threading.Lock
-    - self._joint_states_: Optional[JointState]
-    - self._robot_state_: Optional[RobotState]
-    - PIPER_TO_RAD: conversion constant (0.001 degrees → radians)
-    """
-
-    # Type hints for attributes expected from parent class
-    piper: Any  # C_PiperInterface_V2 instance
-    config: Any  # Config dict accessed as object
-    _joint_state_lock: threading.Lock
-    _joint_states_: JointState | None
-    _robot_state_: RobotState | None
-    PIPER_TO_RAD: float
-
-    @rpc
-    def get_joint_state(self) -> JointState | None:
-        """
-        Get the current joint state (RPC method).
-
-        Returns:
-            Current JointState or None
-        """
-        with self._joint_state_lock:
-            return self._joint_states_
-
-    @rpc
-    def get_robot_state(self) -> RobotState | None:
-        """
-        Get the current robot state (RPC method).
-
-        Returns:
-            Current RobotState or None
-        """
-        with self._joint_state_lock:
-            return self._robot_state_
-
-    @rpc
-    def get_arm_status(self) -> tuple[bool, dict[str, Any] | None]:
-        """
-        Get arm status.
-
-        Returns:
-            Tuple of (success, status_dict)
-        """
-        try:
-            status = self.piper.GetArmStatus()
-
-            if status is not None:
-                status_dict = {
-                    "time_stamp": status.time_stamp,
-                    "Hz": status.Hz,
-                    "motion_mode": status.arm_status.motion_mode,
-                    "mode_feedback": status.arm_status.mode_feedback,
-                    "teach_status": status.arm_status.teach_status,
-                    "motion_status": status.arm_status.motion_status,
-                    "trajectory_num": status.arm_status.trajectory_num,
-                }
-                return (True, status_dict)
-            else:
-                return (False, None)
-
-        except Exception as e:
-            logger.error(f"get_arm_status failed: {e}")
-            return (False, None)
-
-    @rpc
-    def get_arm_joint_angles(self) -> tuple[bool, list[float] | None]:
-        """
-        Get arm joint angles in radians.
-
-        Returns:
-            Tuple of (success, joint_angles)
-        """
-        try:
-            arm_joint = self.piper.GetArmJointMsgs()
-
-            if arm_joint is not None:
-                # Convert from Piper units (0.001 degrees) to radians
-                angles = [
-                    arm_joint.joint_state.joint_1 * self.PIPER_TO_RAD,
-                    arm_joint.joint_state.joint_2 * self.PIPER_TO_RAD,
-                    arm_joint.joint_state.joint_3 * self.PIPER_TO_RAD,
-                    arm_joint.joint_state.joint_4 * self.PIPER_TO_RAD,
-                    arm_joint.joint_state.joint_5 * self.PIPER_TO_RAD,
-                    arm_joint.joint_state.joint_6 * self.PIPER_TO_RAD,
-                ]
-                return (True, angles)
-            else:
-                return (False, None)
-
-        except Exception as e:
-            logger.error(f"get_arm_joint_angles failed: {e}")
-            return (False, None)
-
-    @rpc
-    def get_end_pose(self) -> tuple[bool, dict[str, float] | None]:
-        """
-        Get end-effector pose.
-
-        Returns:
-            Tuple of (success, pose_dict) with keys: x, y, z, rx, ry, rz
-        """
-        try:
-            end_pose = self.piper.GetArmEndPoseMsgs()
-
-            if end_pose is not None:
-                # Convert from Piper units
-                pose_dict = {
-                    "x": end_pose.end_pose.end_pose_x * 0.001,  # 0.001 mm → mm
-                    "y": end_pose.end_pose.end_pose_y * 0.001,
-                    "z": end_pose.end_pose.end_pose_z * 0.001,
-                    "rx": end_pose.end_pose.end_pose_rx * 0.001 * (3.14159 / 180.0),  # → rad
-                    "ry": end_pose.end_pose.end_pose_ry * 0.001 * (3.14159 / 180.0),
-                    "rz": end_pose.end_pose.end_pose_rz * 0.001 * (3.14159 / 180.0),
-                    "time_stamp": end_pose.time_stamp,
-                    "Hz": end_pose.Hz,
-                }
-                return (True, pose_dict)
-            else:
-                return (False, None)
-
-        except Exception as e:
-            logger.error(f"get_end_pose failed: {e}")
-            return (False, None)
-
-    @rpc
-    def get_gripper_state(self) -> tuple[bool, dict[str, Any] | None]:
-        """
-        Get gripper state.
-
-        Returns:
-            Tuple of (success, gripper_dict)
-        """
-        try:
-            gripper = self.piper.GetArmGripperMsgs()
-
-            if gripper is not None:
-                gripper_dict = {
-                    "gripper_angle": gripper.gripper_state.grippers_angle,
-                    "gripper_effort": gripper.gripper_state.grippers_effort,
-                    "gripper_enable": gripper.gripper_state.grippers_enabled,
-                    "time_stamp": gripper.time_stamp,
-                    "Hz": gripper.Hz,
-                }
-                return (True, gripper_dict)
-            else:
-                return (False, None)
-
-        except Exception as e:
-            logger.error(f"get_gripper_state failed: {e}")
-            return (False, None)
-
-    @rpc
-    def get_arm_enable_status(self) -> tuple[bool, list[int] | None]:
-        """
-        Get arm enable status for all joints.
-
-        Returns:
-            Tuple of (success, enable_status_list)
-        """
-        try:
-            enable_status = self.piper.GetArmEnableStatus()
-
-            if enable_status is not None:
-                return (True, enable_status)
-            else:
-                return (False, None)
-
-        except Exception as e:
-            logger.error(f"get_arm_enable_status failed: {e}")
-            return (False, None)
-
-    @rpc
-    def get_firmware_version(self) -> tuple[bool, str | None]:
-        """
-        Get Piper firmware version.
-
-        Returns:
-            Tuple of (success, version_string)
-        """
-        try:
-            version = self.piper.GetPiperFirmwareVersion()
-
-            if version is not None:
-                return (True, version)
-            else:
-                return (False, None)
-
-        except Exception as e:
-            logger.error(f"get_firmware_version failed: {e}")
-            return (False, None)
-
-    @rpc
-    def get_sdk_version(self) -> tuple[bool, str | None]:
-        """
-        Get Piper SDK version.
-
-        Returns:
-            Tuple of (success, version_string)
-        """
-        try:
-            version = self.piper.GetCurrentSDKVersion()
-
-            if version is not None:
-                return (True, version)
-            else:
-                return (False, None)
-
-        except Exception:
-            return (False, None)
-
-    @rpc
-    def get_interface_version(self) -> tuple[bool, str | None]:
-        """
-        Get Piper interface version.
-
-        Returns:
-            Tuple of (success, version_string)
-        """
-        try:
-            version = self.piper.GetCurrentInterfaceVersion()
-
-            if version is not None:
-                return (True, version)
-            else:
-                return (False, None)
-
-        except Exception:
-            return (False, None)
-
-    @rpc
-    def get_protocol_version(self) -> tuple[bool, str | None]:
-        """
-        Get Piper protocol version.
-
-        Returns:
-            Tuple of (success, version_string)
-        """
-        try:
-            version = self.piper.GetCurrentProtocolVersion()
-
-            if version is not None:
-                return (True, version)
-            else:
-                return (False, None)
-
-        except Exception:
-            return (False, None)
-
-    @rpc
-    def get_can_fps(self) -> tuple[bool, float | None]:
-        """
-        Get CAN bus FPS (frames per second).
-
-        Returns:
-            Tuple of (success, fps_value)
-        """
-        try:
-            fps = self.piper.GetCanFps()
-
-            if fps is not None:
-                return (True, fps)
-            else:
-                return (False, None)
-
-        except Exception as e:
-            logger.error(f"get_can_fps failed: {e}")
-            return (False, None)
-
-    @rpc
-    def get_motor_max_acc_limit(self) -> tuple[bool, dict[str, Any] | None]:
-        """
-        Get maximum acceleration limit for all motors.
-
-        Returns:
-            Tuple of (success, acc_limit_dict)
-        """
-        try:
-            acc_limit = self.piper.GetCurrentMotorMaxAccLimit()
-
-            if acc_limit is not None:
-                acc_dict = {
-                    "motor_1": acc_limit.current_motor_max_acc_limit.motor_1_max_acc_limit,
-                    "motor_2": acc_limit.current_motor_max_acc_limit.motor_2_max_acc_limit,
-                    "motor_3": acc_limit.current_motor_max_acc_limit.motor_3_max_acc_limit,
-                    "motor_4": acc_limit.current_motor_max_acc_limit.motor_4_max_acc_limit,
-                    "motor_5": acc_limit.current_motor_max_acc_limit.motor_5_max_acc_limit,
-                    "motor_6": acc_limit.current_motor_max_acc_limit.motor_6_max_acc_limit,
-                    "time_stamp": acc_limit.time_stamp,
-                }
-                return (True, acc_dict)
-            else:
-                return (False, None)
-
-        except Exception as e:
-            logger.error(f"get_motor_max_acc_limit failed: {e}")
-            return (False, None)
diff --git a/dimos/hardware/manipulators/piper/components/system_control.py b/dimos/hardware/manipulators/piper/components/system_control.py
deleted file mode 100644
index a15eb29133..0000000000
--- a/dimos/hardware/manipulators/piper/components/system_control.py
+++ /dev/null
@@ -1,395 +0,0 @@
-# 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.
-
-"""
-System Control Component for PiperDriver.
-
-Provides RPC methods for system-level control operations including:
-- Enable/disable arm
-- Mode control (drag teach, MIT control, etc.)
-- Motion control
-- Master/slave configuration
-"""
-
-from typing import Any
-
-from dimos.core import rpc
-from dimos.utils.logging_config import setup_logger
-
-logger = setup_logger()
-
-
-class SystemControlComponent:
-    """
-    Component providing system control RPC methods for PiperDriver.
-
-    This component assumes the parent class has:
-    - self.piper: C_PiperInterface_V2 instance
-    - self.config: PiperDriverConfig instance
-    """
-
-    # Type hints for attributes expected from parent class
-    piper: Any  # C_PiperInterface_V2 instance
-    config: Any  # Config dict accessed as object
-
-    @rpc
-    def enable_servo_mode(self) -> tuple[bool, str]:
-        """
-        Enable servo mode.
-        This enables the arm to receive motion commands.
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.EnableArm()
-
-            if result:
-                logger.info("Servo mode enabled")
-                return (True, "Servo mode enabled")
-            else:
-                logger.warning("Failed to enable servo mode")
-                return (False, "Failed to enable servo mode")
-
-        except Exception as e:
-            logger.error(f"enable_servo_mode failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def disable_servo_mode(self) -> tuple[bool, str]:
-        """
-        Disable servo mode.
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.DisableArm()
-
-            if result:
-                logger.info("Servo mode disabled")
-                return (True, "Servo mode disabled")
-            else:
-                logger.warning("Failed to disable servo mode")
-                return (False, "Failed to disable servo mode")
-
-        except Exception as e:
-            logger.error(f"disable_servo_mode failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def motion_enable(self, enable: bool = True) -> tuple[bool, str]:
-        """Enable or disable arm motion."""
-        try:
-            if enable:
-                result = self.piper.EnableArm()
-                msg = "Motion enabled"
-            else:
-                result = self.piper.DisableArm()
-                msg = "Motion disabled"
-
-            if result:
-                return (True, msg)
-            else:
-                return (False, f"Failed to {msg.lower()}")
-
-        except Exception as e:
-            return (False, str(e))
-
-    @rpc
-    def set_motion_ctrl_1(
-        self,
-        ctrl_mode: int = 0x00,
-        move_mode: int = 0x00,
-        move_spd_rate: int = 50,
-        coor_mode: int = 0x00,
-        reference_joint: int = 0x00,
-    ) -> tuple[bool, str]:
-        """
-        Set motion control parameters (MotionCtrl_1).
-
-        Args:
-            ctrl_mode: Control mode
-            move_mode: Movement mode
-            move_spd_rate: Movement speed rate (0-100)
-            coor_mode: Coordinate mode
-            reference_joint: Reference joint
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.MotionCtrl_1(
-                ctrl_mode, move_mode, move_spd_rate, coor_mode, reference_joint
-            )
-
-            if result:
-                return (True, "Motion control 1 parameters set successfully")
-            else:
-                return (False, "Failed to set motion control 1 parameters")
-
-        except Exception as e:
-            logger.error(f"set_motion_ctrl_1 failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def set_motion_ctrl_2(
-        self,
-        limit_fun_en: int = 0x00,
-        collis_detect_en: int = 0x00,
-        friction_feed_en: int = 0x00,
-        gravity_feed_en: int = 0x00,
-        is_mit_mode: int = 0x00,
-    ) -> tuple[bool, str]:
-        """
-        Set motion control parameters (MotionCtrl_2).
-
-        Args:
-            limit_fun_en: Limit function enable (0x00 = disabled, 0x01 = enabled)
-            collis_detect_en: Collision detection enable
-            friction_feed_en: Friction compensation enable
-            gravity_feed_en: Gravity compensation enable
-            is_mit_mode: MIT mode enable (0x00 = disabled, 0x01 = enabled)
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.MotionCtrl_2(
-                limit_fun_en,
-                collis_detect_en,
-                friction_feed_en,
-                gravity_feed_en,
-                is_mit_mode,
-            )
-
-            if result:
-                return (True, "Motion control 2 parameters set successfully")
-            else:
-                return (False, "Failed to set motion control 2 parameters")
-
-        except Exception as e:
-            logger.error(f"set_motion_ctrl_2 failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def set_mode_ctrl(
-        self,
-        drag_teach_en: int = 0x00,
-        teach_record_en: int = 0x00,
-    ) -> tuple[bool, str]:
-        """
-        Set mode control (drag teaching, recording, etc.).
-
-        Args:
-            drag_teach_en: Drag teaching enable (0x00 = disabled, 0x01 = enabled)
-            teach_record_en: Teaching record enable
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.ModeCtrl(drag_teach_en, teach_record_en)
-
-            if result:
-                mode_str = []
-                if drag_teach_en == 0x01:
-                    mode_str.append("drag teaching")
-                if teach_record_en == 0x01:
-                    mode_str.append("recording")
-
-                if mode_str:
-                    return (True, f"Mode control set: {', '.join(mode_str)} enabled")
-                else:
-                    return (True, "Mode control set: all modes disabled")
-            else:
-                return (False, "Failed to set mode control")
-
-        except Exception as e:
-            logger.error(f"set_mode_ctrl failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def configure_master_slave(
-        self,
-        linkage_config: int,
-        feedback_offset: int,
-        ctrl_offset: int,
-        linkage_offset: int,
-    ) -> tuple[bool, str]:
-        """
-        Configure master/slave linkage.
-
-        Args:
-            linkage_config: Linkage configuration
-            feedback_offset: Feedback offset
-            ctrl_offset: Control offset
-            linkage_offset: Linkage offset
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.MasterSlaveConfig(
-                linkage_config, feedback_offset, ctrl_offset, linkage_offset
-            )
-
-            if result:
-                return (True, "Master/slave configuration set successfully")
-            else:
-                return (False, "Failed to set master/slave configuration")
-
-        except Exception as e:
-            logger.error(f"configure_master_slave failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def search_firmware_version(self) -> tuple[bool, str]:
-        """
-        Search for firmware version.
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.SearchPiperFirmwareVersion()
-
-            if result:
-                return (True, "Firmware version search initiated")
-            else:
-                return (False, "Failed to search firmware version")
-
-        except Exception as e:
-            logger.error(f"search_firmware_version failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def piper_init(self) -> tuple[bool, str]:
-        """
-        Initialize Piper arm.
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.PiperInit()
-
-            if result:
-                logger.info("Piper initialized")
-                return (True, "Piper initialized successfully")
-            else:
-                logger.warning("Failed to initialize Piper")
-                return (False, "Failed to initialize Piper")
-
-        except Exception as e:
-            logger.error(f"piper_init failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def enable_piper(self) -> tuple[bool, str]:
-        """
-        Enable Piper (convenience method).
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.EnablePiper()
-
-            if result:
-                logger.info("Piper enabled")
-                return (True, "Piper enabled")
-            else:
-                logger.warning("Failed to enable Piper")
-                return (False, "Failed to enable Piper")
-
-        except Exception as e:
-            logger.error(f"enable_piper failed: {e}")
-            return (False, str(e))
-
-    @rpc
-    def disable_piper(self) -> tuple[bool, str]:
-        """
-        Disable Piper (convenience method).
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            result = self.piper.DisablePiper()
-
-            if result:
-                logger.info("Piper disabled")
-                return (True, "Piper disabled")
-            else:
-                logger.warning("Failed to disable Piper")
-                return (False, "Failed to disable Piper")
-
-        except Exception as e:
-            logger.error(f"disable_piper failed: {e}")
-            return (False, str(e))
-
-    # =========================================================================
-    # Velocity Control Mode
-    # =========================================================================
-
-    @rpc
-    def enable_velocity_control_mode(self) -> tuple[bool, str]:
-        """
-        Enable velocity control mode (integration-based).
-
-        This switches the control loop to use velocity integration:
-        - Velocity commands are integrated: position_target += velocity * dt
-        - Integrated positions are sent to JointCtrl (standard position control)
-        - Provides smooth velocity control interface while using proven position API
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            # Set config flag to enable velocity control
-            # The control loop will integrate velocities to positions
-            self.config.velocity_control = True
-
-            logger.info("Velocity control mode enabled (integration-based)")
-            return (True, "Velocity control mode enabled")
-
-        except Exception as e:
-            logger.error(f"enable_velocity_control_mode failed: {e}")
-            self.config.velocity_control = False  # Revert on exception
-            return (False, str(e))
-
-    @rpc
-    def disable_velocity_control_mode(self) -> tuple[bool, str]:
-        """
-        Disable velocity control mode and return to position control.
-
-        Returns:
-            Tuple of (success, message)
-        """
-        try:
-            # Set config flag to disable velocity control
-            # The control loop will switch back to standard position control mode
-            self.config.velocity_control = False
-
-            # Reset position target to allow re-initialization when re-enabled
-            self._position_target_ = None
-
-            logger.info("Position control mode enabled (velocity mode disabled)")
-            return (True, "Position control mode enabled")
-
-        except Exception as e:
-            logger.error(f"disable_velocity_control_mode failed: {e}")
-            self.config.velocity_control = True  # Revert on exception
-            return (False, str(e))
diff --git a/dimos/hardware/manipulators/piper/piper_blueprints.py b/dimos/hardware/manipulators/piper/piper_blueprints.py
deleted file mode 100644
index 1145616841..0000000000
--- a/dimos/hardware/manipulators/piper/piper_blueprints.py
+++ /dev/null
@@ -1,172 +0,0 @@
-# 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.
-
-"""
-Blueprints for Piper manipulator control systems.
-
-This module provides declarative blueprints for configuring Piper servo control,
-following the same pattern used for xArm and other manipulators.
-
-Usage:
-    # Run via CLI:
-    dimos run piper-servo           # Driver only
-    dimos run piper-cartesian       # Driver + Cartesian motion controller
-    dimos run piper-trajectory      # Driver + Joint trajectory controller
-
-    # Or programmatically:
-    from dimos.hardware.manipulators.piper.piper_blueprints import piper_servo
-    coordinator = piper_servo.build()
-    coordinator.loop()
-"""
-
-from typing import Any
-
-from dimos.core.blueprints import autoconnect
-from dimos.core.transport import LCMTransport
-from dimos.hardware.manipulators.piper.piper_driver import piper_driver as piper_driver_blueprint
-from dimos.manipulation.control import cartesian_motion_controller, joint_trajectory_controller
-from dimos.msgs.geometry_msgs import PoseStamped
-from dimos.msgs.sensor_msgs import (
-    JointCommand,
-    JointState,
-    RobotState,
-)
-from dimos.msgs.trajectory_msgs import JointTrajectory
-
-
-# Create a blueprint wrapper for the component-based driver
-def piper_driver(**config: Any) -> Any:
-    """Create a blueprint for PiperDriver.
-
-    Args:
-        **config: Configuration parameters passed to PiperDriver
-            - can_port: CAN interface name (default: "can0")
-            - has_gripper: Whether gripper is attached (default: True)
-            - enable_on_start: Whether to enable servos on start (default: True)
-            - control_rate: Control loop + joint feedback rate in Hz (default: 100)
-            - monitor_rate: Robot state monitoring rate in Hz (default: 10)
-
-    Returns:
-        Blueprint configuration for PiperDriver
-    """
-    # Set defaults
-    config.setdefault("can_port", "can0")
-    config.setdefault("has_gripper", True)
-    config.setdefault("enable_on_start", True)
-    config.setdefault("control_rate", 100)
-    config.setdefault("monitor_rate", 10)
-
-    # Return the piper_driver blueprint with the config
-    return piper_driver_blueprint(**config)
-
-
-# =============================================================================
-# Piper Servo Control Blueprint
-# =============================================================================
-# PiperDriver configured for servo control mode using component-based architecture.
-# Publishes joint states and robot state, listens for joint commands.
-# =============================================================================
-
-piper_servo = piper_driver(
-    can_port="can0",
-    has_gripper=True,
-    enable_on_start=True,
-    control_rate=100,
-    monitor_rate=10,
-).transports(
-    {
-        # Joint state feedback (position, velocity, effort)
-        ("joint_state", JointState): LCMTransport("/piper/joint_states", JointState),
-        # Robot state feedback (mode, state, errors)
-        ("robot_state", RobotState): LCMTransport("/piper/robot_state", RobotState),
-        # Position commands input
-        ("joint_position_command", JointCommand): LCMTransport(
-            "/piper/joint_position_command", JointCommand
-        ),
-        # Velocity commands input
-        ("joint_velocity_command", JointCommand): LCMTransport(
-            "/piper/joint_velocity_command", JointCommand
-        ),
-    }
-)
-
-# =============================================================================
-# Piper Cartesian Control Blueprint (Driver + Controller)
-# =============================================================================
-# Combines PiperDriver with CartesianMotionController for Cartesian space control.
-# The controller receives target_pose and converts to joint commands via IK.
-# =============================================================================
-
-piper_cartesian = autoconnect(
-    piper_driver(
-        can_port="can0",
-        has_gripper=True,
-        enable_on_start=True,
-        control_rate=100,
-        monitor_rate=10,
-    ),
-    cartesian_motion_controller(
-        control_frequency=20.0,
-        position_kp=5.0,
-        position_ki=0.0,
-        position_kd=0.1,
-        max_linear_velocity=0.2,
-        max_angular_velocity=1.0,
-    ),
-).transports(
-    {
-        # Shared topics between driver and controller
-        ("joint_state", JointState): LCMTransport("/piper/joint_states", JointState),
-        ("robot_state", RobotState): LCMTransport("/piper/robot_state", RobotState),
-        ("joint_position_command", JointCommand): LCMTransport(
-            "/piper/joint_position_command", JointCommand
-        ),
-        # Controller-specific topics
-        ("target_pose", PoseStamped): LCMTransport("/target_pose", PoseStamped),
-        ("current_pose", PoseStamped): LCMTransport("/piper/current_pose", PoseStamped),
-    }
-)
-
-# =============================================================================
-# Piper Trajectory Control Blueprint (Driver + Trajectory Controller)
-# =============================================================================
-# Combines PiperDriver with JointTrajectoryController for trajectory execution.
-# The controller receives JointTrajectory messages and executes them at 100Hz.
-# =============================================================================
-
-piper_trajectory = autoconnect(
-    piper_driver(
-        can_port="can0",
-        has_gripper=True,
-        enable_on_start=True,
-        control_rate=100,
-        monitor_rate=10,
-    ),
-    joint_trajectory_controller(
-        control_frequency=100.0,
-    ),
-).transports(
-    {
-        # Shared topics between driver and controller
-        ("joint_state", JointState): LCMTransport("/piper/joint_states", JointState),
-        ("robot_state", RobotState): LCMTransport("/piper/robot_state", RobotState),
-        ("joint_position_command", JointCommand): LCMTransport(
-            "/piper/joint_position_command", JointCommand
-        ),
-        # Trajectory input topic
-        ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory),
-    }
-)
-
-__all__ = ["piper_cartesian", "piper_servo", "piper_trajectory"]
diff --git a/dimos/hardware/manipulators/piper/piper_description.urdf b/dimos/hardware/manipulators/piper/piper_description.urdf
deleted file mode 100755
index c8a5a11ded..0000000000
--- a/dimos/hardware/manipulators/piper/piper_description.urdf
+++ /dev/null
@@ -1,497 +0,0 @@
-<robot
-  name="piper">
-  <link
-    name="base_link">
-    <inertial>
-      <origin xyz="-0.00473641164191482 2.56829134630247E-05 0.041451518036016" rpy="0 0 0" />
-      <mass value="1.02" />
-      <inertia ixx="0.00267433" ixy="-0.00000073" ixz="-0.00017389" iyy="0.00282612" iyz="0.0000004" izz="0.00089624" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/base_link.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/base_link.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <link
-    name="link1">
-    <inertial>
-      <origin xyz="0.000121504734057468 0.000104632162460536 -0.00438597309559853" rpy="0 0 0" />
-      <mass value="0.71" />
-      <inertia ixx="0.00048916" ixy="-0.00000036" ixz="-0.00000224" iyy="0.00040472" iyz="-0.00000242" izz="0.00043982" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link1.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link1.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint1"
-    type="revolute">
-    <origin
-      xyz="0 0 0.123"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="link1" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-2.618"
-      upper="2.168"
-      effort="100"
-      velocity="5" />
-  </joint>
-  <link
-    name="link2">
-    <inertial>
-      <origin xyz="0.198666145229743 -0.010926924140076 0.00142121714502687" rpy="0 0 0" />
-      <mass value="1.17" />
-      <inertia ixx="0.00116918" ixy="-0.00180037" ixz="0.00025146" iyy="0.06785384" iyz="-0.00000455" izz="0.06774489" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link2.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link2.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint2"
-    type="revolute">
-    <origin
-      xyz="0 0 0"
-      rpy="1.5708 -0.1359 -3.1416" />
-      <!-- rpy="1.5708 -0.10095-0.03490659 -3.1416" /> -->
-    <parent
-      link="link1" />
-    <child
-      link="link2" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="0"
-      upper="3.14"
-      effort="100"
-      velocity="5" />
-  </joint>
-  <link
-    name="link3">
-    <inertial>
-      <origin xyz="-0.0202737662122021 -0.133914995944595 -0.000458682652737356" rpy="0 0 0" />
-      <mass value="0.5" />
-      <inertia ixx="0.01361711" ixy="0.00165794" ixz="-0.00000048" iyy="0.00045024" iyz="-0.00000045" izz="0.01380322" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link3.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link3.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint3"
-    type="revolute">
-    <origin
-      xyz="0.28503 0 0"
-      rpy="0 0 -1.7939" />
-      <!-- rpy="0 0 -1.759-0.03490659" /> -->
-    <parent
-      link="link2" />
-    <child
-      link="link3" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-2.967"
-      upper="0"
-      effort="100"
-      velocity="5" />
-  </joint>
-  <link
-    name="link4">
-    <inertial>
-      <origin xyz="-9.66635791618542E-05 0.000876064475651083 -0.00496880904640868" rpy="0 0 0" />
-      <mass value="0.38" />
-      <inertia ixx="0.00018501" ixy="0.00000054" ixz="0.00000120" iyy="0.00018965" iyz="-0.00000841" izz="0.00015484" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link4.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link4.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint4"
-    type="revolute">
-    <origin
-      xyz="-0.021984 -0.25075 0"
-      rpy="1.5708 0 0" />
-    <parent
-      link="link3" />
-    <child
-      link="link4" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-1.745"
-      upper="1.745"
-      effort="100"
-      velocity="5" />
-  </joint>
-  <link
-    name="link5">
-    <inertial>
-      <origin xyz="-4.10554118924211E-05 -0.0566486692356075 -0.0037205791677906" rpy="0 0 0" />
-      <mass value="0.383" />
-      <inertia ixx="0.00166169" ixy="0.00000006" ixz="-0.00000007" iyy="0.00018510" iyz="0.00001026" izz="0.00164321" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link5.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link5.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint5"
-    type="revolute">
-    <origin
-      xyz="0 0 0"
-      rpy="-1.5708 0 0" />
-    <parent
-      link="link4" />
-    <child
-      link="link5" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-1.22"
-      upper="1.22"
-      effort="100"
-      velocity="5" />
-  </joint>
-  <link
-    name="link6">
-    <inertial>
-      <origin xyz="-8.82590762930069E-05 9.0598378529832E-06 -0.002" rpy="0 0 0" />
-      <mass value="0.00699089613564366" />
-      <inertia ixx="5.73015540542155E-07" ixy="-1.98305403089247E-22" ixz="-7.2791893904596E-23" iyy="5.73015540542155E-07" iyz="-3.4146026640245E-24" izz="1.06738869138926E-06" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link6.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link6.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint6"
-    type="revolute">
-    <origin
-      xyz="8.8259E-05 -0.091 0"
-      rpy="1.5708 0 0" />
-    <parent
-      link="link5" />
-    <child
-      link="link6" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-2.0944"
-      upper="2.0944"
-      effort="100"
-      velocity="3" />
-  </joint>
-  <link
-    name="gripper_base">
-    <inertial>
-      <origin
-        xyz="-0.000183807162235591 8.05033155577911E-05 0.0321436689908876"
-        rpy="0 0 0" />
-      <mass
-        value="0.45" />
-      <inertia
-        ixx="0.00092934"
-        ixy="0.00000034"
-        ixz="-0.00000738"
-        iyy="0.00071447"
-        iyz="0.00000005"
-        izz="0.00039442" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/gripper_base.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/gripper_base.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint6_to_gripper_base"
-    type="fixed">
-    <origin
-      xyz="0 0 0"
-      rpy="0 0 0" />
-    <parent
-      link="link6" />
-    <child
-      link="gripper_base" />
-  </joint>
-  <link
-    name="link7">
-    <inertial>
-      <origin
-        xyz="0.00065123185041968 -0.0491929869131989 0.00972258769184025"
-        rpy="0 0 0" />
-      <mass
-        value="0.025" />
-      <inertia
-        ixx="0.00007371"
-        ixy="-0.00000113"
-        ixz="0.00000021"
-        iyy="-0.00000113"
-        iyz="-0.00001372"
-        izz="0.0000747" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link7.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link7.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint7"
-    type="prismatic">
-    <origin
-      xyz="0 0 0.1358"
-      rpy="1.5708 0 0" />
-    <parent
-      link="gripper_base" />
-    <child
-      link="link7" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="0"
-      upper="0.035"
-      effort="10"
-      velocity="1" />
-  </joint>
-  <link
-    name="link8">
-    <inertial>
-      <origin
-        xyz="0.000651231850419722 -0.0491929869131991 0.00972258769184024"
-        rpy="0 0 0" />
-      <mass
-        value="0.025" />
-      <inertia
-        ixx="0.00007371"
-        ixy="-0.00000113"
-        ixz="0.00000021"
-        iyy="-0.00000113"
-        iyz="-0.00001372"
-        izz="0.0000747" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link8.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://piper_description/meshes/link8.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint8"
-    type="prismatic">
-    <origin
-      xyz="0 0 0.1358"
-      rpy="1.5708 0 -3.1416" />
-    <parent
-      link="gripper_base" />
-    <child
-      link="link8" />
-    <axis
-      xyz="0 0 -1" />
-    <limit
-      lower="-0.035"
-      upper="0"
-      effort="10"
-      velocity="1" />
-  </joint>
-</robot>
diff --git a/dimos/hardware/manipulators/piper/piper_driver.py b/dimos/hardware/manipulators/piper/piper_driver.py
deleted file mode 100644
index 5730a4394a..0000000000
--- a/dimos/hardware/manipulators/piper/piper_driver.py
+++ /dev/null
@@ -1,241 +0,0 @@
-# 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.
-
-"""Piper driver using the generalized component-based architecture."""
-
-import logging
-import time
-from typing import Any
-
-from dimos.hardware.manipulators.base import (
-    BaseManipulatorDriver,
-    StandardMotionComponent,
-    StandardServoComponent,
-    StandardStatusComponent,
-)
-
-from .piper_wrapper import PiperSDKWrapper
-
-logger = logging.getLogger(__name__)
-
-
-class PiperDriver(BaseManipulatorDriver):
-    """Piper driver using component-based architecture.
-
-    This driver supports the Piper 6-DOF manipulator via CAN bus.
-    All the complex logic is handled by the base class and standard components.
-    This file just assembles the pieces.
-    """
-
-    def __init__(self, **kwargs: Any) -> None:
-        """Initialize the Piper driver.
-
-        Args:
-            **kwargs: Arguments for Module initialization.
-                Driver configuration can be passed via 'config' keyword arg:
-                - can_port: CAN interface name (e.g., 'can0')
-                - has_gripper: Whether gripper is attached
-                - enable_on_start: Whether to enable servos on start
-        """
-        # Extract driver-specific config from kwargs
-        config: dict[str, Any] = kwargs.pop("config", {})
-
-        # Extract driver-specific params that might be passed directly
-        driver_params = [
-            "can_port",
-            "has_gripper",
-            "enable_on_start",
-            "control_rate",
-            "monitor_rate",
-        ]
-        for param in driver_params:
-            if param in kwargs:
-                config[param] = kwargs.pop(param)
-
-        logger.info(f"Initializing PiperDriver with config: {config}")
-
-        # Create SDK wrapper
-        sdk = PiperSDKWrapper()
-
-        # Create standard components
-        components = [
-            StandardMotionComponent(sdk),
-            StandardServoComponent(sdk),
-            StandardStatusComponent(sdk),
-        ]
-
-        # Optional: Add gripper component if configured
-        # if config.get('has_gripper', False):
-        #     from dimos.hardware.manipulators.base.components import StandardGripperComponent
-        #     components.append(StandardGripperComponent(sdk))
-
-        # Remove any kwargs that would conflict with explicit arguments
-        kwargs.pop("sdk", None)
-        kwargs.pop("components", None)
-        kwargs.pop("name", None)
-
-        # Initialize base driver with SDK and components
-        super().__init__(
-            sdk=sdk, components=components, config=config, name="PiperDriver", **kwargs
-        )
-
-        # Initialize position target for velocity integration
-        self._position_target: list[float] | None = None
-        self._last_velocity_time: float = 0.0
-
-        # Enable on start if configured
-        if config.get("enable_on_start", False):
-            logger.info("Enabling Piper servos on start...")
-            servo_component = self.get_component(StandardServoComponent)
-            if servo_component:
-                result = servo_component.enable_servo()
-                if result["success"]:
-                    logger.info("Piper servos enabled successfully")
-                else:
-                    logger.warning(f"Failed to enable servos: {result.get('error')}")
-
-        logger.info("PiperDriver initialized successfully")
-
-    def _process_command(self, command: Any) -> None:
-        """Override to implement velocity control via position integration.
-
-        Args:
-            command: Command to process
-        """
-        # Handle velocity commands specially for Piper
-        if command.type == "velocity":
-            # Piper doesn't have native velocity control - integrate to position
-            current_time = time.time()
-
-            # Initialize position target from current state on first velocity command
-            if self._position_target is None:
-                positions = self.shared_state.joint_positions
-                if positions:
-                    self._position_target = list(positions)
-                    logger.info(
-                        f"Velocity control: Initialized position target from current state: {self._position_target}"
-                    )
-                else:
-                    logger.warning("Cannot start velocity control - no current position available")
-                    return
-
-            # Calculate dt since last velocity command
-            if self._last_velocity_time > 0:
-                dt = current_time - self._last_velocity_time
-            else:
-                dt = 1.0 / self.control_rate  # Use nominal period for first command
-
-            self._last_velocity_time = current_time
-
-            # Integrate velocity to position: pos += vel * dt
-            velocities = command.data["velocities"]
-            for i in range(min(len(velocities), len(self._position_target))):
-                self._position_target[i] += velocities[i] * dt
-
-            # Send integrated position command
-            success = self.sdk.set_joint_positions(
-                self._position_target,
-                velocity=1.0,  # Use max velocity for responsiveness
-                acceleration=1.0,
-                wait=False,
-            )
-
-            if success:
-                self.shared_state.target_positions = self._position_target
-                self.shared_state.target_velocities = velocities
-
-        else:
-            # Reset velocity integration when switching to position mode
-            if command.type == "position":
-                self._position_target = None
-                self._last_velocity_time = 0.0
-
-            # Use base implementation for other command types
-            super()._process_command(command)
-
-
-# Blueprint configuration for the driver
-def get_blueprint() -> dict[str, Any]:
-    """Get the blueprint configuration for the Piper driver.
-
-    Returns:
-        Dictionary with blueprint configuration
-    """
-    return {
-        "name": "PiperDriver",
-        "class": PiperDriver,
-        "config": {
-            "can_port": "can0",  # Default CAN interface
-            "has_gripper": True,  # Piper usually has gripper
-            "enable_on_start": True,  # Enable servos on startup
-            "control_rate": 100,  # Hz - control loop + joint feedback
-            "monitor_rate": 10,  # Hz - robot state monitoring
-        },
-        "inputs": {
-            "joint_position_command": "JointCommand",
-            "joint_velocity_command": "JointCommand",
-        },
-        "outputs": {
-            "joint_state": "JointState",
-            "robot_state": "RobotState",
-        },
-        "rpc_methods": [
-            # Motion control
-            "move_joint",
-            "move_joint_velocity",
-            "move_joint_effort",
-            "stop_motion",
-            "get_joint_state",
-            "get_joint_limits",
-            "get_velocity_limits",
-            "set_velocity_scale",
-            "set_acceleration_scale",
-            "move_cartesian",
-            "get_cartesian_state",
-            "execute_trajectory",
-            "stop_trajectory",
-            # Servo control
-            "enable_servo",
-            "disable_servo",
-            "toggle_servo",
-            "get_servo_state",
-            "emergency_stop",
-            "reset_emergency_stop",
-            "set_control_mode",
-            "get_control_mode",
-            "clear_errors",
-            "reset_fault",
-            "home_robot",
-            "brake_release",
-            "brake_engage",
-            # Status monitoring
-            "get_robot_state",
-            "get_system_info",
-            "get_capabilities",
-            "get_error_state",
-            "get_health_metrics",
-            "get_statistics",
-            "check_connection",
-            "get_force_torque",
-            "zero_force_torque",
-            "get_digital_inputs",
-            "set_digital_outputs",
-            "get_analog_inputs",
-            "get_gripper_state",
-        ],
-    }
-
-
-# Expose blueprint for declarative composition (compatible with dimos framework)
-piper_driver = PiperDriver.blueprint
diff --git a/dimos/hardware/manipulators/piper/piper_wrapper.py b/dimos/hardware/manipulators/piper/piper_wrapper.py
deleted file mode 100644
index 7384f6c06e..0000000000
--- a/dimos/hardware/manipulators/piper/piper_wrapper.py
+++ /dev/null
@@ -1,671 +0,0 @@
-# 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.
-
-"""Piper SDK wrapper implementation."""
-
-import logging
-import time
-from typing import Any
-
-from ..base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo
-
-# Unit conversion constants
-RAD_TO_PIPER = 57295.7795  # radians to Piper units (0.001 degrees)
-PIPER_TO_RAD = 1.0 / RAD_TO_PIPER  # Piper units to radians
-
-
-class PiperSDKWrapper(BaseManipulatorSDK):
-    """SDK wrapper for Piper manipulators.
-
-    This wrapper translates Piper's native SDK (which uses radians but 1-indexed joints)
-    to our standard interface (0-indexed).
-    """
-
-    def __init__(self) -> None:
-        """Initialize the Piper SDK wrapper."""
-        self.logger = logging.getLogger(self.__class__.__name__)
-        self.native_sdk: Any = None
-        self.dof = 6  # Piper is always 6-DOF
-        self._connected = False
-        self._enabled = False
-
-    # ============= Connection Management =============
-
-    def connect(self, config: dict[str, Any]) -> bool:
-        """Connect to Piper via CAN bus.
-
-        Args:
-            config: Configuration with 'can_port' (e.g., 'can0')
-
-        Returns:
-            True if connection successful
-        """
-        try:
-            from piper_sdk import C_PiperInterface_V2
-
-            can_port = config.get("can_port", "can0")
-            self.logger.info(f"Connecting to Piper via CAN port {can_port}...")
-
-            # Create Piper SDK instance
-            self.native_sdk = C_PiperInterface_V2(
-                can_name=can_port,
-                judge_flag=True,  # Enable safety checks
-                can_auto_init=True,  # Let SDK handle CAN initialization
-                dh_is_offset=False,
-            )
-
-            # Connect to CAN port
-            self.native_sdk.ConnectPort(piper_init=True, start_thread=True)
-
-            # Wait for initialization
-            time.sleep(0.025)
-
-            # Check connection by trying to get status
-            status = self.native_sdk.GetArmStatus()
-            if status is not None:
-                self._connected = True
-
-                # Get firmware version
-                try:
-                    version = self.native_sdk.GetPiperFirmwareVersion()
-                    self.logger.info(f"Connected to Piper (firmware: {version})")
-                except:
-                    self.logger.info("Connected to Piper")
-
-                return True
-            else:
-                self.logger.error("Failed to connect to Piper - no status received")
-                return False
-
-        except ImportError:
-            self.logger.error("Piper SDK not installed. Please install piper_sdk")
-            return False
-        except Exception as e:
-            self.logger.error(f"Connection failed: {e}")
-            return False
-
-    def disconnect(self) -> None:
-        """Disconnect from Piper."""
-        if self.native_sdk:
-            try:
-                # Disable arm first
-                if self._enabled:
-                    self.native_sdk.DisablePiper()
-                    self._enabled = False
-
-                # Disconnect
-                self.native_sdk.DisconnectPort()
-                self._connected = False
-                self.logger.info("Disconnected from Piper")
-            except:
-                pass
-            finally:
-                self.native_sdk = None
-
-    def is_connected(self) -> bool:
-        """Check if connected to Piper.
-
-        Returns:
-            True if connected
-        """
-        if not self._connected or not self.native_sdk:
-            return False
-
-        # Try to get status to verify connection
-        try:
-            status = self.native_sdk.GetArmStatus()
-            return status is not None
-        except:
-            return False
-
-    # ============= Joint State Query =============
-
-    def get_joint_positions(self) -> list[float]:
-        """Get current joint positions.
-
-        Returns:
-            Joint positions in RADIANS (0-indexed)
-        """
-        joint_msgs = self.native_sdk.GetArmJointMsgs()
-        if not joint_msgs or not joint_msgs.joint_state:
-            raise RuntimeError("Failed to get Piper joint positions")
-
-        # Get joint positions from joint_state (values are in Piper units: 0.001 degrees)
-        # Convert to radians using PIPER_TO_RAD conversion factor
-        joint_state = joint_msgs.joint_state
-        positions = [
-            joint_state.joint_1 * PIPER_TO_RAD,  # Convert Piper units to radians
-            joint_state.joint_2 * PIPER_TO_RAD,
-            joint_state.joint_3 * PIPER_TO_RAD,
-            joint_state.joint_4 * PIPER_TO_RAD,
-            joint_state.joint_5 * PIPER_TO_RAD,
-            joint_state.joint_6 * PIPER_TO_RAD,
-        ]
-        return positions
-
-    def get_joint_velocities(self) -> list[float]:
-        """Get current joint velocities.
-
-        Returns:
-            Joint velocities in RAD/S (0-indexed)
-        """
-        # TODO: Get actual velocities from Piper SDK
-        # For now return zeros as velocity feedback may not be available
-        return [0.0] * self.dof
-
-    def get_joint_efforts(self) -> list[float]:
-        """Get current joint efforts/torques.
-
-        Returns:
-            Joint efforts in Nm (0-indexed)
-        """
-        # TODO: Get actual efforts/torques from Piper SDK if available
-        # For now return zeros as effort feedback may not be available
-        return [0.0] * self.dof
-
-    # ============= Joint Motion Control =============
-
-    def set_joint_positions(
-        self,
-        positions: list[float],
-        velocity: float = 1.0,
-        acceleration: float = 1.0,
-        wait: bool = False,
-    ) -> bool:
-        """Move joints to target positions.
-
-        Args:
-            positions: Target positions in RADIANS (0-indexed)
-            velocity: Max velocity fraction (0-1)
-            acceleration: Max acceleration fraction (0-1)
-            wait: If True, block until motion completes
-
-        Returns:
-            True if command accepted
-        """
-        # Convert radians to Piper units (0.001 degrees)
-        piper_joints = [round(rad * RAD_TO_PIPER) for rad in positions]
-
-        # Optionally set motion control parameters based on velocity/acceleration
-        if velocity < 1.0 or acceleration < 1.0:
-            # Scale speed rate based on velocity parameter (0-100)
-            speed_rate = int(velocity * 100)
-            self.native_sdk.MotionCtrl_2(
-                ctrl_mode=0x01,  # CAN control mode
-                move_mode=0x01,  # Move mode
-                move_spd_rate_ctrl=speed_rate,  # Speed rate
-                is_mit_mode=0x00,  # Not MIT mode
-            )
-
-        # Send joint control command using JointCtrl with 6 individual parameters
-        try:
-            self.native_sdk.JointCtrl(
-                piper_joints[0],  # Joint 1
-                piper_joints[1],  # Joint 2
-                piper_joints[2],  # Joint 3
-                piper_joints[3],  # Joint 4
-                piper_joints[4],  # Joint 5
-                piper_joints[5],  # Joint 6
-            )
-            result = True
-        except Exception as e:
-            self.logger.error(f"Error setting joint positions: {e}")
-            result = False
-
-        # If wait requested, poll until motion completes
-        if wait and result:
-            start_time = time.time()
-            timeout = 30.0  # 30 second timeout
-
-            while time.time() - start_time < timeout:
-                try:
-                    # Check if reached target (within tolerance)
-                    current = self.get_joint_positions()
-                    tolerance = 0.01  # radians
-                    if all(abs(current[i] - positions[i]) < tolerance for i in range(6)):
-                        break
-                except:
-                    pass  # Continue waiting
-                time.sleep(0.01)
-
-        return result
-
-    def set_joint_velocities(self, velocities: list[float]) -> bool:
-        """Set joint velocity targets.
-
-        Note: Piper doesn't have native velocity control. The driver should
-        implement velocity control via position integration if needed.
-
-        Args:
-            velocities: Target velocities in RAD/S (0-indexed)
-
-        Returns:
-            False - velocity control not supported at SDK level
-        """
-        # Piper doesn't have native velocity control
-        # The driver layer should implement this via position integration
-        self.logger.debug("Velocity control not supported at SDK level - use position integration")
-        return False
-
-    def set_joint_efforts(self, efforts: list[float]) -> bool:
-        """Set joint effort/torque targets.
-
-        Args:
-            efforts: Target efforts in Nm (0-indexed)
-
-        Returns:
-            True if command accepted
-        """
-        # Check if torque control is supported
-        if not hasattr(self.native_sdk, "SetJointTorque"):
-            self.logger.warning("Torque control not available in this Piper version")
-            return False
-
-        # Convert 0-indexed to 1-indexed dict
-        torque_dict = {i + 1: torque for i, torque in enumerate(efforts)}
-
-        # Send torque command
-        self.native_sdk.SetJointTorque(torque_dict)
-        return True
-
-    def stop_motion(self) -> bool:
-        """Stop all ongoing motion.
-
-        Returns:
-            True if stop successful
-        """
-        # Piper emergency stop
-        if hasattr(self.native_sdk, "EmergencyStop"):
-            self.native_sdk.EmergencyStop()
-        else:
-            # Alternative: set zero velocities
-            zero_vel = {i: 0.0 for i in range(1, 7)}
-            if hasattr(self.native_sdk, "SetJointSpeed"):
-                self.native_sdk.SetJointSpeed(zero_vel)
-
-        return True
-
-    # ============= Servo Control =============
-
-    def enable_servos(self) -> bool:
-        """Enable motor control.
-
-        Returns:
-            True if servos enabled
-        """
-        # Enable Piper
-        attempts = 0
-        max_attempts = 100
-
-        while not self.native_sdk.EnablePiper() and attempts < max_attempts:
-            time.sleep(0.01)
-            attempts += 1
-
-        if attempts < max_attempts:
-            self._enabled = True
-
-            # Set control mode
-            self.native_sdk.MotionCtrl_2(
-                ctrl_mode=0x01,  # CAN control mode
-                move_mode=0x01,  # Move mode
-                move_spd_rate_ctrl=30,  # Speed rate
-                is_mit_mode=0x00,  # Not MIT mode
-            )
-
-            return True
-
-        return False
-
-    def disable_servos(self) -> bool:
-        """Disable motor control.
-
-        Returns:
-            True if servos disabled
-        """
-        self.native_sdk.DisablePiper()
-        self._enabled = False
-        return True
-
-    def are_servos_enabled(self) -> bool:
-        """Check if servos are enabled.
-
-        Returns:
-            True if enabled
-        """
-        return self._enabled
-
-    # ============= System State =============
-
-    def get_robot_state(self) -> dict[str, Any]:
-        """Get current robot state.
-
-        Returns:
-            State dictionary
-        """
-        status = self.native_sdk.GetArmStatus()
-
-        if status and status.arm_status:
-            # Map Piper states to standard states
-            # Use the nested arm_status object
-            arm_status = status.arm_status
-
-            # Default state mapping
-            state = 0  # idle
-            mode = 0  # position mode
-            error_code = 0
-
-            # Check for error status
-            if hasattr(arm_status, "err_code"):
-                error_code = arm_status.err_code
-                if error_code != 0:
-                    state = 2  # error state
-
-            # Check motion status if available
-            if hasattr(arm_status, "motion_status"):
-                # Could check if moving
-                pass
-
-            return {
-                "state": state,
-                "mode": mode,
-                "error_code": error_code,
-                "warn_code": 0,  # Piper doesn't have warn codes
-                "is_moving": False,  # Would need to track this
-                "cmd_num": 0,  # Piper doesn't expose command queue
-            }
-
-        return {
-            "state": 2,  # Error if can't get status
-            "mode": 0,
-            "error_code": 999,
-            "warn_code": 0,
-            "is_moving": False,
-            "cmd_num": 0,
-        }
-
-    def get_error_code(self) -> int:
-        """Get current error code.
-
-        Returns:
-            Error code (0 = no error)
-        """
-        status = self.native_sdk.GetArmStatus()
-        if status and hasattr(status, "error_code"):
-            return int(status.error_code)
-        return 0
-
-    def get_error_message(self) -> str:
-        """Get human-readable error message.
-
-        Returns:
-            Error message string
-        """
-        error_code = self.get_error_code()
-        if error_code == 0:
-            return ""
-
-        # Piper error codes (approximate)
-        error_map = {
-            1: "Communication error",
-            2: "Motor error",
-            3: "Encoder error",
-            4: "Overtemperature",
-            5: "Overcurrent",
-            6: "Joint limit error",
-            7: "Emergency stop",
-            8: "Power error",
-        }
-
-        return error_map.get(error_code, f"Unknown error {error_code}")
-
-    def clear_errors(self) -> bool:
-        """Clear error states.
-
-        Returns:
-            True if errors cleared
-        """
-        if hasattr(self.native_sdk, "ClearError"):
-            self.native_sdk.ClearError()
-            return True
-
-        # Alternative: disable and re-enable
-        self.disable_servos()
-        time.sleep(0.1)
-        return self.enable_servos()
-
-    def emergency_stop(self) -> bool:
-        """Execute emergency stop.
-
-        Returns:
-            True if e-stop executed
-        """
-        if hasattr(self.native_sdk, "EmergencyStop"):
-            self.native_sdk.EmergencyStop()
-            return True
-
-        # Alternative: disable servos
-        return self.disable_servos()
-
-    # ============= Information =============
-
-    def get_info(self) -> ManipulatorInfo:
-        """Get manipulator information.
-
-        Returns:
-            ManipulatorInfo object
-        """
-        firmware_version = None
-        try:
-            firmware_version = self.native_sdk.GetPiperFirmwareVersion()
-        except:
-            pass
-
-        return ManipulatorInfo(
-            vendor="Agilex",
-            model="Piper",
-            dof=self.dof,
-            firmware_version=firmware_version,
-            serial_number=None,  # Piper doesn't expose serial number
-        )
-
-    def get_joint_limits(self) -> tuple[list[float], list[float]]:
-        """Get joint position limits.
-
-        Returns:
-            Tuple of (lower_limits, upper_limits) in RADIANS
-        """
-        # Piper joint limits (approximate, in radians)
-        lower_limits = [-3.14, -2.35, -2.35, -3.14, -2.35, -3.14]
-        upper_limits = [3.14, 2.35, 2.35, 3.14, 2.35, 3.14]
-
-        return (lower_limits, upper_limits)
-
-    def get_velocity_limits(self) -> list[float]:
-        """Get joint velocity limits.
-
-        Returns:
-            Maximum velocities in RAD/S
-        """
-        # Piper max velocities (approximate)
-        max_vel = 3.14  # rad/s
-        return [max_vel] * self.dof
-
-    def get_acceleration_limits(self) -> list[float]:
-        """Get joint acceleration limits.
-
-        Returns:
-            Maximum accelerations in RAD/S²
-        """
-        # Piper max accelerations (approximate)
-        max_acc = 10.0  # rad/s²
-        return [max_acc] * self.dof
-
-    # ============= Optional Methods =============
-
-    def get_cartesian_position(self) -> dict[str, float] | None:
-        """Get current end-effector pose.
-
-        Returns:
-            Pose dict or None if not supported
-        """
-        if hasattr(self.native_sdk, "GetEndPose"):
-            pose = self.native_sdk.GetEndPose()
-            if pose:
-                return {
-                    "x": pose.x,
-                    "y": pose.y,
-                    "z": pose.z,
-                    "roll": pose.roll,
-                    "pitch": pose.pitch,
-                    "yaw": pose.yaw,
-                }
-        return None
-
-    def set_cartesian_position(
-        self,
-        pose: dict[str, float],
-        velocity: float = 1.0,
-        acceleration: float = 1.0,
-        wait: bool = False,
-    ) -> bool:
-        """Move end-effector to target pose.
-
-        Args:
-            pose: Target pose dict
-            velocity: Max velocity fraction (0-1)
-            acceleration: Max acceleration fraction (0-1)
-            wait: Block until complete
-
-        Returns:
-            True if command accepted
-        """
-        if not hasattr(self.native_sdk, "MoveL"):
-            self.logger.warning("Cartesian control not available")
-            return False
-
-        # Create pose object for Piper
-        target = {
-            "x": pose["x"],
-            "y": pose["y"],
-            "z": pose["z"],
-            "roll": pose["roll"],
-            "pitch": pose["pitch"],
-            "yaw": pose["yaw"],
-        }
-
-        # Send Cartesian command
-        self.native_sdk.MoveL(target)
-
-        # Wait if requested
-        if wait:
-            start_time = time.time()
-            timeout = 30.0
-
-            while time.time() - start_time < timeout:
-                current = self.get_cartesian_position()
-                if current:
-                    # Check if reached target (within tolerance)
-                    tol_pos = 0.005  # 5mm
-                    tol_rot = 0.05  # ~3 degrees
-
-                    if (
-                        abs(current["x"] - pose["x"]) < tol_pos
-                        and abs(current["y"] - pose["y"]) < tol_pos
-                        and abs(current["z"] - pose["z"]) < tol_pos
-                        and abs(current["roll"] - pose["roll"]) < tol_rot
-                        and abs(current["pitch"] - pose["pitch"]) < tol_rot
-                        and abs(current["yaw"] - pose["yaw"]) < tol_rot
-                    ):
-                        break
-
-                time.sleep(0.01)
-
-        return True
-
-    def get_gripper_position(self) -> float | None:
-        """Get gripper position.
-
-        Returns:
-            Position in meters or None
-        """
-        if hasattr(self.native_sdk, "GetGripperState"):
-            state = self.native_sdk.GetGripperState()
-            if state:
-                # Piper gripper position is 0-100 (percentage)
-                # Convert to meters (assume max opening 0.08m)
-                return float(state / 100.0) * 0.08
-        return None
-
-    def set_gripper_position(self, position: float, force: float = 1.0) -> bool:
-        """Set gripper position.
-
-        Args:
-            position: Target position in meters
-            force: Force fraction (0-1)
-
-        Returns:
-            True if successful
-        """
-        if not hasattr(self.native_sdk, "GripperCtrl"):
-            self.logger.warning("Gripper control not available")
-            return False
-
-        # Convert meters to percentage (0-100)
-        # Assume max opening 0.08m
-        percentage = int((position / 0.08) * 100)
-        percentage = max(0, min(100, percentage))
-
-        # Control gripper
-        self.native_sdk.GripperCtrl(percentage)
-        return True
-
-    def set_control_mode(self, mode: str) -> bool:
-        """Set control mode.
-
-        Args:
-            mode: 'position', 'velocity', 'torque', or 'impedance'
-
-        Returns:
-            True if successful
-        """
-        # Piper modes via MotionCtrl_2
-        # ctrl_mode: 0x01=CAN control
-        # move_mode: 0x01=position, 0x02=velocity?
-
-        if not hasattr(self.native_sdk, "MotionCtrl_2"):
-            return False
-
-        move_mode = 0x01  # Default position
-        if mode == "velocity":
-            move_mode = 0x02
-
-        self.native_sdk.MotionCtrl_2(
-            ctrl_mode=0x01, move_mode=move_mode, move_spd_rate_ctrl=30, is_mit_mode=0x00
-        )
-
-        return True
-
-    def get_control_mode(self) -> str | None:
-        """Get current control mode.
-
-        Returns:
-            Mode string or None
-        """
-        status = self.native_sdk.GetArmStatus()
-        if status and hasattr(status, "arm_mode"):
-            # Map Piper modes
-            mode_map = {0x01: "position", 0x02: "velocity"}
-            return mode_map.get(status.arm_mode, "unknown")
-
-        return "position"  # Default assumption
diff --git a/dimos/hardware/manipulators/spec.py b/dimos/hardware/manipulators/spec.py
new file mode 100644
index 0000000000..585043421e
--- /dev/null
+++ b/dimos/hardware/manipulators/spec.py
@@ -0,0 +1,261 @@
+# 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.
+
+"""Manipulator specifications: Protocol and shared types.
+
+This file defines:
+1. Shared enums and dataclasses used by all arms
+2. ManipulatorBackend Protocol that backends must implement
+
+Note: No ABC for drivers. Each arm implements its own driver
+with full control over threading and logic.
+"""
+
+from dataclasses import dataclass
+from enum import Enum
+from typing import Protocol, runtime_checkable
+
+from dimos.msgs.geometry_msgs import Quaternion, Transform, Vector3
+
+# ============================================================================
+# SHARED TYPES
+# ============================================================================
+
+
+class DriverStatus(Enum):
+    """Status returned by driver operations."""
+
+    DISCONNECTED = "disconnected"
+    CONNECTED = "connected"
+    ENABLED = "enabled"
+    MOVING = "moving"
+    ERROR = "error"
+
+
+class ControlMode(Enum):
+    """Control modes for manipulator."""
+
+    POSITION = "position"  # Planned position control (slower, smoother)
+    SERVO_POSITION = "servo_position"  # High-freq joint position streaming (100Hz+)
+    VELOCITY = "velocity"
+    TORQUE = "torque"
+    CARTESIAN = "cartesian"
+    CARTESIAN_VELOCITY = "cartesian_velocity"
+    IMPEDANCE = "impedance"
+
+
+@dataclass
+class ManipulatorInfo:
+    """Information about the manipulator."""
+
+    vendor: str
+    model: str
+    dof: int
+    firmware_version: str | None = None
+    serial_number: str | None = None
+
+
+@dataclass
+class JointLimits:
+    """Joint position and velocity limits."""
+
+    position_lower: list[float]  # radians
+    position_upper: list[float]  # radians
+    velocity_max: list[float]  # rad/s
+
+
+def default_base_transform() -> Transform:
+    """Default identity transform for arm mounting."""
+    return Transform(
+        translation=Vector3(0.0, 0.0, 0.0),
+        rotation=Quaternion(0.0, 0.0, 0.0, 1.0),
+    )
+
+
+# ============================================================================
+# BACKEND PROTOCOL
+# ============================================================================
+
+
+@runtime_checkable
+class ManipulatorBackend(Protocol):
+    """Protocol for hardware-specific IO.
+
+    Implement this per vendor SDK. All methods use SI units:
+    - Angles: radians
+    - Angular velocity: rad/s
+    - Torque: Nm
+    - Position: meters
+    - Force: Newtons
+    """
+
+    # --- Connection ---
+
+    def connect(self) -> bool:
+        """Connect to hardware. Returns True on success."""
+        ...
+
+    def disconnect(self) -> None:
+        """Disconnect from hardware."""
+        ...
+
+    def is_connected(self) -> bool:
+        """Check if connected."""
+        ...
+
+    # --- Info ---
+
+    def get_info(self) -> ManipulatorInfo:
+        """Get manipulator info (vendor, model, DOF)."""
+        ...
+
+    def get_dof(self) -> int:
+        """Get degrees of freedom."""
+        ...
+
+    def get_limits(self) -> JointLimits:
+        """Get joint limits."""
+        ...
+
+    # --- Control Mode ---
+
+    def set_control_mode(self, mode: ControlMode) -> bool:
+        """Set control mode (position, velocity, torque, cartesian, etc).
+
+        Args:
+            mode: Target control mode
+
+        Returns:
+            True if mode switch successful, False otherwise
+
+        Note: Some arms (like XArm) may accept commands in any mode,
+        while others (like Piper) require explicit mode switching.
+        """
+        ...
+
+    def get_control_mode(self) -> ControlMode:
+        """Get current control mode.
+
+        Returns:
+            Current control mode
+        """
+        ...
+
+    # --- State Reading ---
+
+    def read_joint_positions(self) -> list[float]:
+        """Read current joint positions (radians)."""
+        ...
+
+    def read_joint_velocities(self) -> list[float]:
+        """Read current joint velocities (rad/s)."""
+        ...
+
+    def read_joint_efforts(self) -> list[float]:
+        """Read current joint efforts (Nm)."""
+        ...
+
+    def read_state(self) -> dict[str, int]:
+        """Read robot state (mode, state code, etc)."""
+        ...
+
+    def read_error(self) -> tuple[int, str]:
+        """Read error code and message. (0, '') means no error."""
+        ...
+
+    # --- Motion Control (Joint Space) ---
+
+    def write_joint_positions(
+        self,
+        positions: list[float],
+        velocity: float = 1.0,
+    ) -> bool:
+        """Command joint positions (radians). Returns success."""
+        ...
+
+    def write_joint_velocities(self, velocities: list[float]) -> bool:
+        """Command joint velocities (rad/s). Returns success."""
+        ...
+
+    def write_stop(self) -> bool:
+        """Stop all motion immediately."""
+        ...
+
+    # --- Servo Control ---
+
+    def write_enable(self, enable: bool) -> bool:
+        """Enable or disable servos. Returns success."""
+        ...
+
+    def read_enabled(self) -> bool:
+        """Check if servos are enabled."""
+        ...
+
+    def write_clear_errors(self) -> bool:
+        """Clear error state. Returns success."""
+        ...
+
+    # --- Optional: Cartesian Control ---
+    # Return None/False if not supported
+
+    def read_cartesian_position(self) -> dict[str, float] | None:
+        """Read end-effector pose.
+
+        Returns:
+            Dict with keys: x, y, z (meters), roll, pitch, yaw (radians)
+            None if not supported
+        """
+        ...
+
+    def write_cartesian_position(
+        self,
+        pose: dict[str, float],
+        velocity: float = 1.0,
+    ) -> bool:
+        """Command end-effector pose.
+
+        Args:
+            pose: Dict with keys: x, y, z (meters), roll, pitch, yaw (radians)
+            velocity: Speed as fraction of max (0-1)
+
+        Returns:
+            True if command accepted, False if not supported
+        """
+        ...
+
+    # --- Optional: Gripper ---
+
+    def read_gripper_position(self) -> float | None:
+        """Read gripper position (meters). None if no gripper."""
+        ...
+
+    def write_gripper_position(self, position: float) -> bool:
+        """Command gripper position. False if no gripper."""
+        ...
+
+    # --- Optional: Force/Torque Sensor ---
+
+    def read_force_torque(self) -> list[float] | None:
+        """Read F/T sensor [fx, fy, fz, tx, ty, tz]. None if no sensor."""
+        ...
+
+
+__all__ = [
+    "ControlMode",
+    "DriverStatus",
+    "JointLimits",
+    "ManipulatorBackend",
+    "ManipulatorInfo",
+    "default_base_transform",
+]
diff --git a/dimos/hardware/manipulators/test_integration_runner.py b/dimos/hardware/manipulators/test_integration_runner.py
deleted file mode 100644
index eab6a022da..0000000000
--- a/dimos/hardware/manipulators/test_integration_runner.py
+++ /dev/null
@@ -1,626 +0,0 @@
-#!/usr/bin/env python3
-# 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.
-
-"""
-Integration test runner for manipulator drivers.
-
-This is a standalone script (NOT a pytest test file) that tests the common
-BaseManipulatorDriver interface that all arms implement.
-Supports both mock mode (for CI/CD) and hardware mode (for real testing).
-
-NOTE: This file is intentionally NOT named test_*.py to avoid pytest auto-discovery.
-For pytest-based unit tests, see: dimos/hardware/manipulators/base/tests/test_driver_unit.py
-
-Usage:
-    # Run with mock (CI/CD safe, default)
-    python -m dimos.hardware.manipulators.integration_test_runner
-
-    # Run specific arm with mock
-    python -m dimos.hardware.manipulators.integration_test_runner --arm piper
-
-    # Run with real hardware (xArm)
-    python -m dimos.hardware.manipulators.integration_test_runner --hardware --ip 192.168.1.210
-
-    # Run with real hardware (Piper)
-    python -m dimos.hardware.manipulators.integration_test_runner --hardware --arm piper --can can0
-
-    # Run specific test
-    python -m dimos.hardware.manipulators.integration_test_runner --test connection
-
-    # Skip motion tests (safer for hardware)
-    python -m dimos.hardware.manipulators.integration_test_runner --hardware --skip-motion
-"""
-
-import argparse
-import math
-import sys
-import time
-
-from dimos.core.transport import LCMTransport
-from dimos.hardware.manipulators.base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo
-from dimos.msgs.sensor_msgs import JointState, RobotState
-
-
-class MockSDK(BaseManipulatorSDK):
-    """Mock SDK for testing without hardware. Works for any arm type."""
-
-    def __init__(self, dof: int = 6, vendor: str = "Mock", model: str = "TestArm"):
-        self._connected = True
-        self._dof = dof
-        self._vendor = vendor
-        self._model = model
-        self._positions = [0.0] * dof
-        self._velocities = [0.0] * dof
-        self._efforts = [0.0] * dof
-        self._servos_enabled = False
-        self._mode = 0
-        self._state = 0
-        self._error_code = 0
-
-    def connect(self, config: dict) -> bool:
-        self._connected = True
-        return True
-
-    def disconnect(self) -> None:
-        self._connected = False
-
-    def is_connected(self) -> bool:
-        return self._connected
-
-    def get_joint_positions(self) -> list[float]:
-        return self._positions.copy()
-
-    def get_joint_velocities(self) -> list[float]:
-        return self._velocities.copy()
-
-    def get_joint_efforts(self) -> list[float]:
-        return self._efforts.copy()
-
-    def set_joint_positions(
-        self,
-        positions: list[float],
-        velocity: float = 1.0,
-        acceleration: float = 1.0,
-        wait: bool = False,
-    ) -> bool:
-        if not self._servos_enabled:
-            return False
-        self._positions = list(positions)
-        return True
-
-    def set_joint_velocities(self, velocities: list[float]) -> bool:
-        if not self._servos_enabled:
-            return False
-        self._velocities = list(velocities)
-        return True
-
-    def set_joint_efforts(self, efforts: list[float]) -> bool:
-        return False  # Not supported in mock
-
-    def stop_motion(self) -> bool:
-        self._velocities = [0.0] * self._dof
-        return True
-
-    def enable_servos(self) -> bool:
-        self._servos_enabled = True
-        return True
-
-    def disable_servos(self) -> bool:
-        self._servos_enabled = False
-        return True
-
-    def are_servos_enabled(self) -> bool:
-        return self._servos_enabled
-
-    def get_robot_state(self) -> dict:
-        return {
-            "state": self._state,
-            "mode": self._mode,
-            "error_code": self._error_code,
-            "is_moving": any(v != 0 for v in self._velocities),
-        }
-
-    def get_error_code(self) -> int:
-        return self._error_code
-
-    def get_error_message(self) -> str:
-        return "" if self._error_code == 0 else f"Error {self._error_code}"
-
-    def clear_errors(self) -> bool:
-        self._error_code = 0
-        return True
-
-    def emergency_stop(self) -> bool:
-        self._velocities = [0.0] * self._dof
-        self._servos_enabled = False
-        return True
-
-    def get_info(self) -> ManipulatorInfo:
-        return ManipulatorInfo(
-            vendor=self._vendor,
-            model=f"{self._model} (Mock)",
-            dof=self._dof,
-            firmware_version="mock-1.0.0",
-            serial_number="MOCK-001",
-        )
-
-    def get_joint_limits(self) -> tuple[list[float], list[float]]:
-        lower = [-2 * math.pi] * self._dof
-        upper = [2 * math.pi] * self._dof
-        return lower, upper
-
-    def get_velocity_limits(self) -> list[float]:
-        return [math.pi] * self._dof
-
-    def get_acceleration_limits(self) -> list[float]:
-        return [math.pi * 2] * self._dof
-
-
-# =============================================================================
-# Test Functions (work with any driver implementing BaseManipulatorDriver)
-# =============================================================================
-
-
-def check_connection(driver, hardware: bool) -> bool:
-    """Test that driver connects to hardware/mock."""
-    print("Testing connection...")
-
-    if not driver.sdk.is_connected():
-        print("  FAIL: SDK not connected")
-        return False
-
-    info = driver.sdk.get_info()
-    print(f"  Connected to: {info.vendor} {info.model}")
-    print(f"  DOF: {info.dof}")
-    print(f"  Firmware: {info.firmware_version}")
-    print(f"  Mode: {'HARDWARE' if hardware else 'MOCK'}")
-    print("  PASS")
-    return True
-
-
-def check_read_joint_state(driver, hardware: bool) -> bool:
-    """Test reading joint state."""
-    print("Testing read joint state...")
-
-    result = driver.get_joint_state()
-    if not result.get("success"):
-        print(f"  FAIL: {result.get('error')}")
-        return False
-
-    positions = result["positions"]
-    velocities = result["velocities"]
-    efforts = result["efforts"]
-
-    print(f"  Positions (deg): {[f'{math.degrees(p):.1f}' for p in positions]}")
-    print(f"  Velocities: {[f'{v:.3f}' for v in velocities]}")
-    print(f"  Efforts: {[f'{e:.2f}' for e in efforts]}")
-
-    if len(positions) != driver.capabilities.dof:
-        print(f"  FAIL: Expected {driver.capabilities.dof} joints, got {len(positions)}")
-        return False
-
-    print("  PASS")
-    return True
-
-
-def check_get_robot_state(driver, hardware: bool) -> bool:
-    """Test getting robot state."""
-    print("Testing robot state...")
-
-    result = driver.get_robot_state()
-    if not result.get("success"):
-        print(f"  FAIL: {result.get('error')}")
-        return False
-
-    print(f"  State: {result.get('state')}")
-    print(f"  Mode: {result.get('mode')}")
-    print(f"  Error code: {result.get('error_code')}")
-    print(f"  Is moving: {result.get('is_moving')}")
-    print("  PASS")
-    return True
-
-
-def check_servo_enable_disable(driver, hardware: bool) -> bool:
-    """Test enabling and disabling servos."""
-    print("Testing servo enable/disable...")
-
-    # Enable
-    result = driver.enable_servo()
-    if not result.get("success"):
-        print(f"  FAIL enable: {result.get('error')}")
-        return False
-    print("  Enabled servos")
-
-    # Hardware needs more time for state to propagate
-    time.sleep(1.0 if hardware else 0.01)
-
-    # Check state with retry for hardware
-    enabled = driver.sdk.are_servos_enabled()
-    if not enabled and hardware:
-        # Retry after additional delay
-        time.sleep(0.5)
-        enabled = driver.sdk.are_servos_enabled()
-
-    if not enabled:
-        print("  FAIL: Servos not enabled after enable_servo()")
-        return False
-    print("  Verified servos enabled")
-
-    # # Disable
-    # result = driver.disable_servo()
-    # if not result.get("success"):
-    #     print(f"  FAIL disable: {result.get('error')}")
-    #     return False
-    # print("  Disabled servos")
-
-    print("  PASS")
-    return True
-
-
-def check_joint_limits(driver, hardware: bool) -> bool:
-    """Test getting joint limits."""
-    print("Testing joint limits...")
-
-    result = driver.get_joint_limits()
-    if not result.get("success"):
-        print(f"  FAIL: {result.get('error')}")
-        return False
-
-    lower = result["lower"]
-    upper = result["upper"]
-
-    print(f"  Lower (deg): {[f'{math.degrees(l):.1f}' for l in lower]}")
-    print(f"  Upper (deg): {[f'{math.degrees(u):.1f}' for u in upper]}")
-
-    if len(lower) != driver.capabilities.dof:
-        print("  FAIL: Wrong number of limits")
-        return False
-
-    print("  PASS")
-    return True
-
-
-def check_stop_motion(driver, hardware: bool) -> bool:
-    """Test stop motion command."""
-    print("Testing stop motion...")
-
-    result = driver.stop_motion()
-    # Note: stop_motion may return success=False if arm isn't moving,
-    # which is expected behavior. We just verify no exception occurred.
-    if result is None:
-        print("  FAIL: stop_motion returned None")
-        return False
-
-    if result.get("error"):
-        print(f"  FAIL: {result.get('error')}")
-        return False
-
-    # success=False when not moving is OK, success=True is also OK
-    print(f"  stop_motion returned success={result.get('success')}")
-    print("  PASS")
-    return True
-
-
-def check_small_motion(driver, hardware: bool) -> bool:
-    """Test a small joint motion (5 degrees on joint 1).
-
-    WARNING: With --hardware, this MOVES the real robot!
-    """
-    print("Testing small motion (5 deg on J1)...")
-    if hardware:
-        print("  WARNING: Robot will move!")
-
-    # Get current position
-    result = driver.get_joint_state()
-    if not result.get("success"):
-        print(f"  FAIL: Cannot read state: {result.get('error')}")
-        return False
-
-    current_pos = list(result["positions"])
-    print(f"  Current J1: {math.degrees(current_pos[0]):.2f} deg")
-
-    driver.clear_errors()
-    # print(driver.get_state())
-
-    # Enable servos
-    result = driver.enable_servo()
-    print(result)
-    if not result.get("success"):
-        print(f"  FAIL: Cannot enable servos: {result.get('error')}")
-        return False
-
-    time.sleep(0.5 if hardware else 0.01)
-
-    # Move +5 degrees on joint 1
-    target_pos = current_pos.copy()
-    target_pos[0] += math.radians(5.0)
-    print(f"  Target J1: {math.degrees(target_pos[0]):.2f} deg")
-
-    result = driver.move_joint(target_pos, velocity=0.3, wait=True)
-    if not result.get("success"):
-        print(f"  FAIL: Motion failed: {result.get('error')}")
-        return False
-
-    time.sleep(1.0 if hardware else 0.01)
-
-    # Verify position
-    result = driver.get_joint_state()
-    new_pos = result["positions"]
-    error = abs(new_pos[0] - target_pos[0])
-    print(
-        f"  Reached J1: {math.degrees(new_pos[0]):.2f} deg (error: {math.degrees(error):.3f} deg)"
-    )
-
-    if hardware and error > math.radians(1.0):  # Allow 1 degree error for real hardware
-        print("  FAIL: Position error too large")
-        return False
-
-    # Move back
-    print("  Moving back to original position...")
-    driver.move_joint(current_pos, velocity=0.3, wait=True)
-    time.sleep(1.0 if hardware else 0.01)
-
-    print("  PASS")
-    return True
-
-
-# =============================================================================
-# Driver Factory
-# =============================================================================
-
-
-def create_driver(arm: str, hardware: bool, config: dict):
-    """Create driver for the specified arm type.
-
-    Args:
-        arm: Arm type ('xarm', 'piper', etc.)
-        hardware: If True, use real hardware; if False, use mock SDK
-        config: Configuration dict (ip, dof, etc.)
-
-    Returns:
-        Driver instance
-    """
-    if arm == "xarm":
-        from dimos.hardware.manipulators.xarm.xarm_driver import XArmDriver
-
-        if hardware:
-            return XArmDriver(config=config)
-        else:
-            # Create driver with mock SDK
-            driver = XArmDriver.__new__(XArmDriver)
-            # Manually initialize with mock
-            from dimos.hardware.manipulators.base import (
-                BaseManipulatorDriver,
-                StandardMotionComponent,
-                StandardServoComponent,
-                StandardStatusComponent,
-            )
-
-            mock_sdk = MockSDK(dof=config.get("dof", 6), vendor="UFactory", model="xArm")
-            components = [
-                StandardMotionComponent(),
-                StandardServoComponent(),
-                StandardStatusComponent(),
-            ]
-            BaseManipulatorDriver.__init__(
-                driver, sdk=mock_sdk, components=components, config=config, name="XArmDriver"
-            )
-            return driver
-
-    elif arm == "piper":
-        from dimos.hardware.manipulators.piper.piper_driver import PiperDriver
-
-        if hardware:
-            return PiperDriver(config=config)
-        else:
-            # Create driver with mock SDK
-            driver = PiperDriver.__new__(PiperDriver)
-            from dimos.hardware.manipulators.base import (
-                BaseManipulatorDriver,
-                StandardMotionComponent,
-                StandardServoComponent,
-                StandardStatusComponent,
-            )
-
-            mock_sdk = MockSDK(dof=6, vendor="Agilex", model="Piper")
-            components = [
-                StandardMotionComponent(),
-                StandardServoComponent(),
-                StandardStatusComponent(),
-            ]
-            BaseManipulatorDriver.__init__(
-                driver, sdk=mock_sdk, components=components, config=config, name="PiperDriver"
-            )
-            return driver
-
-    else:
-        raise ValueError(f"Unknown arm type: {arm}. Supported: xarm, piper")
-
-
-# =============================================================================
-# Test Runner
-# =============================================================================
-
-
-def configure_transports(driver, arm: str):
-    """Configure LCM transports for the driver (like production does).
-
-    Args:
-        driver: The driver instance
-        arm: Arm type for topic naming
-    """
-    # Create LCM transports for state publishing
-    joint_state_transport = LCMTransport(f"/test/{arm}/joint_state", JointState)
-    robot_state_transport = LCMTransport(f"/test/{arm}/robot_state", RobotState)
-
-    # Set transports on driver's Out streams
-    if driver.joint_state:
-        driver.joint_state._transport = joint_state_transport
-    if driver.robot_state:
-        driver.robot_state._transport = robot_state_transport
-
-
-def run_tests(
-    arm: str,
-    hardware: bool,
-    config: dict,
-    test_name: str | None = None,
-    skip_motion: bool = False,
-):
-    """Run integration tests."""
-    mode = "HARDWARE" if hardware else "MOCK"
-    print("=" * 60)
-    print(f"Manipulator Driver Integration Tests ({mode})")
-    print("=" * 60)
-    print(f"Arm: {arm}")
-    print(f"Config: {config}")
-    print()
-
-    # Create driver
-    print("Creating driver...")
-    try:
-        driver = create_driver(arm, hardware, config)
-    except Exception as e:
-        print(f"FATAL: Failed to create driver: {e}")
-        return False
-
-    # Configure transports (like production does)
-    print("Configuring transports...")
-    configure_transports(driver, arm)
-
-    # Start driver
-    print("Starting driver...")
-    try:
-        driver.start()
-        # Piper needs more initialization time before commands work
-        wait_time = 3.0 if (hardware and arm == "piper") else (1.0 if hardware else 0.1)
-        time.sleep(wait_time)
-    except Exception as e:
-        print(f"FATAL: Failed to start driver: {e}")
-        return False
-
-    # Define tests (stop_motion last since it leaves arm in stopped state)
-    tests = [
-        ("connection", check_connection),
-        ("read_state", check_read_joint_state),
-        ("robot_state", check_get_robot_state),
-        ("joint_limits", check_joint_limits),
-        # ("servo", check_servo_enable_disable),
-    ]
-
-    if not skip_motion:
-        tests.append(("motion", check_small_motion))
-
-    # Stop test always last (leaves arm in stopped state)
-    tests.append(("stop", check_stop_motion))
-
-    # Run tests
-    results = {}
-    print()
-    print("-" * 60)
-
-    for name, test_func in tests:
-        if test_name and name != test_name:
-            continue
-
-        try:
-            results[name] = test_func(driver, hardware)
-        except Exception as e:
-            print(f"  EXCEPTION: {e}")
-            import traceback
-
-            traceback.print_exc()
-            results[name] = False
-
-        print()
-
-    # Stop driver
-    print("Stopping driver...")
-    try:
-        driver.stop()
-    except Exception as e:
-        print(f"Warning: Error stopping driver: {e}")
-
-    # Summary
-    print("-" * 60)
-    print("SUMMARY")
-    print("-" * 60)
-    passed = sum(1 for r in results.values() if r)
-    total = len(results)
-
-    for name, result in results.items():
-        status = "PASS" if result else "FAIL"
-        print(f"  {name}: {status}")
-
-    print()
-    print(f"Result: {passed}/{total} tests passed")
-
-    return passed == total
-
-
-def main():
-    parser = argparse.ArgumentParser(
-        description="Generic manipulator driver integration tests",
-        formatter_class=argparse.RawDescriptionHelpFormatter,
-        epilog="""
-Examples:
-  # Mock mode (CI/CD safe, default)
-  python -m dimos.hardware.manipulators.integration_test_runner
-
-  # xArm hardware mode
-  python -m dimos.hardware.manipulators.integration_test_runner --hardware --ip 192.168.1.210
-
-  # Piper hardware mode
-  python -m dimos.hardware.manipulators.integration_test_runner --hardware --arm piper --can can0
-
-  # Skip motion tests
-  python -m dimos.hardware.manipulators.integration_test_runner --hardware --skip-motion
-""",
-    )
-    parser.add_argument(
-        "--arm", default="xarm", choices=["xarm", "piper"], help="Arm type to test (default: xarm)"
-    )
-    parser.add_argument(
-        "--hardware", action="store_true", help="Use real hardware (default: mock mode)"
-    )
-    parser.add_argument(
-        "--ip", default="192.168.1.210", help="IP address for xarm (default: 192.168.1.210)"
-    )
-    parser.add_argument("--can", default="can0", help="CAN interface for piper (default: can0)")
-    parser.add_argument(
-        "--dof", type=int, help="Degrees of freedom (auto-detected in hardware mode)"
-    )
-    parser.add_argument("--test", help="Run specific test only")
-    parser.add_argument("--skip-motion", action="store_true", help="Skip motion tests")
-    args = parser.parse_args()
-
-    # Build config - DOF auto-detected from hardware if not specified
-    config = {}
-    if args.arm == "xarm" and args.ip:
-        config["ip"] = args.ip
-    if args.arm == "piper" and args.can:
-        config["can_port"] = args.can
-    if args.dof:
-        config["dof"] = args.dof
-    elif not args.hardware:
-        # Mock mode needs explicit DOF
-        config["dof"] = 6
-
-    success = run_tests(args.arm, args.hardware, config, args.test, args.skip_motion)
-    sys.exit(0 if success else 1)
-
-
-if __name__ == "__main__":
-    main()
diff --git a/dimos/hardware/manipulators/xarm/README.md b/dimos/hardware/manipulators/xarm/README.md
deleted file mode 100644
index ff7a797cad..0000000000
--- a/dimos/hardware/manipulators/xarm/README.md
+++ /dev/null
@@ -1,149 +0,0 @@
-# xArm Driver for dimos
-
-Real-time driver for UFACTORY xArm5/6/7 manipulators integrated with the dimos framework.
-
-## Quick Start
-
-### 1. Specify Robot IP
-
-**On boot** (Important)
-```bash
-sudo ifconfig lo multicast
-sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev lo
-```
-
-**Option A: Command-line argument** (recommended)
-```bash
-python test_xarm_driver.py --ip 192.168.1.235
-python interactive_control.py --ip 192.168.1.235
-```
-
-**Option B: Environment variable**
-```bash
-export XARM_IP=192.168.1.235
-python test_xarm_driver.py
-```
-
-**Option C: Use default** (192.168.1.235)
-```bash
-python test_xarm_driver.py  # Uses default
-```
-
-**Note:** Command-line `--ip` takes precedence over `XARM_IP` environment variable.
-
-### 2. Basic Usage
-
-```python
-from dimos import core
-from dimos.hardware.manipulators.xarm.xarm_driver import XArmDriver
-from dimos.msgs.sensor_msgs import JointState, JointCommand
-
-# Start dimos and deploy driver
-dimos = core.start(1)
-xarm = dimos.deploy(XArmDriver, ip_address="192.168.1.235", xarm_type="xarm6")
-
-# Configure LCM transports
-xarm.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState)
-xarm.joint_position_command.transport = core.LCMTransport("/xarm/joint_commands", JointCommand)
-
-# Start and enable servo mode
-xarm.start()
-xarm.enable_servo_mode()
-
-# Control via RPC
-xarm.set_joint_angles([0, 0, 0, 0, 0, 0], speed=50, mvacc=100, mvtime=0)
-
-# Cleanup
-xarm.stop()
-dimos.stop()
-```
-
-## Key Features
-
-- **100Hz control loop** for real-time position/velocity control
-- **LCM pub/sub** for distributed system integration
-- **RPC methods** for direct hardware control
-- **Position mode** (radians) and **velocity mode** (deg/s)
-- **Component-based API**: motion, kinematics, system, gripper control
-
-## Topics
-
-**Subscribed:**
-- `/xarm/joint_position_command` - JointCommand (positions in radians)
-- `/xarm/joint_velocity_command` - JointCommand (velocities in deg/s)
-
-**Published:**
-- `/xarm/joint_states` - JointState (100Hz)
-- `/xarm/robot_state` - RobotState (10Hz)
-- `/xarm/ft_ext`, `/xarm/ft_raw` - WrenchStamped (force/torque)
-
-## Common RPC Methods
-
-```python
-# System control
-xarm.enable_servo_mode()           # Enable position control (mode 1)
-xarm.enable_velocity_control_mode() # Enable velocity control (mode 4)
-xarm.motion_enable(True)           # Enable motors
-xarm.clean_error()                 # Clear errors
-
-# Motion control
-xarm.set_joint_angles([...], speed=50, mvacc=100, mvtime=0)
-xarm.set_servo_angle(joint_id=5, angle=0.5, speed=50)
-
-# State queries
-state = xarm.get_joint_state()
-position = xarm.get_position()
-```
-
-## Configuration
-
-Key parameters for `XArmDriver`:
-- `ip_address`: Robot IP (default: "192.168.1.235")
-- `xarm_type`: Robot model - "xarm5", "xarm6", or "xarm7" (default: "xarm6")
-- `control_frequency`: Control loop rate in Hz (default: 100.0)
-- `is_radian`: Use radians vs degrees (default: True)
-- `enable_on_start`: Auto-enable servo mode (default: True)
-- `velocity_control`: Use velocity vs position mode (default: False)
-
-## Testing
-
-### With Mock Hardware (No Physical Robot)
-
-```bash
-# Unit tests with mocked xArm hardware
-python tests/test_xarm_rt_driver.py
-```
-
-### With Real Hardware
-
-**⚠️ Note:** Interactive control and hardware tests require a physical xArm connected to the network. Interactive control, and sample_trajectory_generator are part of test suite, and will be deprecated.
-
-**Using Alfred Embodiment:**
-
-To test with real hardware using the current Alfred embodiment:
-
-1. **Turn on the Flowbase** (xArm controller)
-2. **SSH into dimensional-cpu-2:**
-   ```
-3. **Verify PC is connected to the controller:**
-   ```bash
-   ping 192.168.1.235  # Should respond
-   ```
-4. **Run the interactive control:**
-   ```bash
-   # Interactive control (recommended)
-   venv/bin/python dimos/hardware/manipulators/xarm/interactive_control.py --ip 192.168.1.235
-
-   # Run driver standalone
-   venv/bin/python dimos/hardware/manipulators/xarm/test_xarm_driver.py --ip 192.168.1.235
-
-   # Run automated test suite
-   venv/bin/python dimos/hardware/manipulators/xarm/test_xarm_driver.py --ip 192.168.1.235 --run-tests
-
-   # Specify xArm model type (if using xArm7)
-   venv/bin/python dimos/hardware/manipulators/xarm/interactive_control.py --ip 192.168.1.235 --type xarm7
-   ```
-
-## License
-
-Copyright 2025 Dimensional Inc. - Apache License 2.0
diff --git a/dimos/hardware/manipulators/xarm/__init__.py b/dimos/hardware/manipulators/xarm/__init__.py
index ef0c6763c1..343ebc4e0e 100644
--- a/dimos/hardware/manipulators/xarm/__init__.py
+++ b/dimos/hardware/manipulators/xarm/__init__.py
@@ -12,18 +12,15 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-"""
-xArm Manipulator Driver Module
+"""XArm manipulator hardware backend.
 
-Real-time driver and components for xArm5/6/7 manipulators.
+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.spec import ArmDriverSpec
-from dimos.hardware.manipulators.xarm.xarm_driver import XArmDriver
-from dimos.hardware.manipulators.xarm.xarm_wrapper import XArmSDKWrapper
+from dimos.hardware.manipulators.xarm.backend import XArmBackend
 
-__all__ = [
-    "ArmDriverSpec",
-    "XArmDriver",
-    "XArmSDKWrapper",
-]
+__all__ = ["XArmBackend"]
diff --git a/dimos/hardware/manipulators/xarm/backend.py b/dimos/hardware/manipulators/xarm/backend.py
new file mode 100644
index 0000000000..9adcdca24f
--- /dev/null
+++ b/dimos/hardware/manipulators/xarm/backend.py
@@ -0,0 +1,392 @@
+# 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.
+
+"""XArm backend - implements ManipulatorBackend protocol.
+
+Handles all XArm SDK communication and unit conversion.
+"""
+
+import math
+
+from xarm.wrapper import XArmAPI
+
+from dimos.hardware.manipulators.spec import (
+    ControlMode,
+    JointLimits,
+    ManipulatorBackend,
+    ManipulatorInfo,
+)
+
+# XArm mode codes
+_XARM_MODE_POSITION = 0
+_XARM_MODE_SERVO_CARTESIAN = 1
+_XARM_MODE_JOINT_VELOCITY = 4
+_XARM_MODE_CARTESIAN_VELOCITY = 5
+_XARM_MODE_JOINT_TORQUE = 6
+
+
+class XArmBackend(ManipulatorBackend):
+    """XArm-specific backend.
+
+    Implements ManipulatorBackend 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
+        self._dof = dof
+        self._arm: XArmAPI | None = None
+        self._control_mode: ControlMode = ControlMode.POSITION
+
+    # =========================================================================
+    # Connection
+    # =========================================================================
+
+    def connect(self) -> bool:
+        """Connect to XArm via TCP/IP."""
+        try:
+            self._arm = XArmAPI(self._ip)
+            self._arm.connect()
+
+            if not self._arm.connected:
+                print(f"ERROR: XArm at {self._ip} not reachable (connected=False)")
+                return False
+
+            # Initialize to servo mode for high-frequency control
+            self._arm.set_mode(_XARM_MODE_SERVO_CARTESIAN)  # Mode 1 = servo mode
+            self._arm.set_state(0)
+            self._control_mode = ControlMode.SERVO_POSITION
+
+            return True
+        except Exception as e:
+            print(f"ERROR: Failed to connect to XArm at {self._ip}: {e}")
+            return False
+
+    def disconnect(self) -> None:
+        """Disconnect from XArm."""
+        if self._arm:
+            self._arm.disconnect()
+            self._arm = None
+
+    def is_connected(self) -> bool:
+        """Check if connected to XArm."""
+        return self._arm is not None and self._arm.connected
+
+    # =========================================================================
+    # Info
+    # =========================================================================
+
+    def get_info(self) -> ManipulatorInfo:
+        """Get XArm information."""
+        return ManipulatorInfo(
+            vendor="UFACTORY",
+            model=f"xArm{self._dof}",
+            dof=self._dof,
+        )
+
+    def get_dof(self) -> int:
+        """Get degrees of freedom."""
+        return self._dof
+
+    def get_limits(self) -> JointLimits:
+        """Get joint limits (default XArm limits)."""
+        # XArm typical joint limits (varies by joint, using conservative values)
+        limit = 2 * math.pi
+        return JointLimits(
+            position_lower=[-limit] * self._dof,
+            position_upper=[limit] * self._dof,
+            velocity_max=[math.pi] * self._dof,  # ~180 deg/s
+        )
+
+    # =========================================================================
+    # Control Mode
+    # =========================================================================
+
+    def set_control_mode(self, mode: ControlMode) -> bool:
+        """Set XArm control mode.
+
+        Note: XArm is flexible and often accepts commands without explicit
+        mode switching, but some operations require the correct mode.
+        """
+        if not self._arm:
+            return False
+
+        mode_map = {
+            ControlMode.POSITION: _XARM_MODE_POSITION,
+            ControlMode.SERVO_POSITION: _XARM_MODE_SERVO_CARTESIAN,  # Mode 1 for high-freq
+            ControlMode.VELOCITY: _XARM_MODE_JOINT_VELOCITY,
+            ControlMode.TORQUE: _XARM_MODE_JOINT_TORQUE,
+            ControlMode.CARTESIAN: _XARM_MODE_SERVO_CARTESIAN,
+            ControlMode.CARTESIAN_VELOCITY: _XARM_MODE_CARTESIAN_VELOCITY,
+        }
+
+        xarm_mode = mode_map.get(mode)
+        if xarm_mode is None:
+            return False
+
+        code = self._arm.set_mode(xarm_mode)
+        if code == 0:
+            self._arm.set_state(0)
+            self._control_mode = mode
+            return True
+        return False
+
+    def get_control_mode(self) -> ControlMode:
+        """Get current control mode."""
+        return self._control_mode
+
+    # =========================================================================
+    # State Reading
+    # =========================================================================
+
+    def read_joint_positions(self) -> list[float]:
+        """Read joint positions (degrees -> radians)."""
+        if not self._arm:
+            raise RuntimeError("Not connected")
+
+        _, angles = self._arm.get_servo_angle()
+        if not angles:
+            raise RuntimeError("Failed to read joint positions")
+        return [math.radians(a) for a in angles[: self._dof]]
+
+    def read_joint_velocities(self) -> list[float]:
+        """Read joint velocities.
+
+        Note: XArm doesn't provide real-time velocity feedback directly.
+        Returns zeros. For velocity estimation, use finite differences
+        on positions in the driver.
+        """
+        return [0.0] * self._dof
+
+    def read_joint_efforts(self) -> list[float]:
+        """Read joint torques in Nm."""
+        if not self._arm:
+            return [0.0] * self._dof
+
+        code, torques = self._arm.get_joints_torque()
+        if code == 0 and torques:
+            return list(torques[: self._dof])
+        return [0.0] * self._dof
+
+    def read_state(self) -> dict[str, int]:
+        """Read robot state."""
+        if not self._arm:
+            return {"state": 0, "mode": 0}
+
+        return {
+            "state": self._arm.state,
+            "mode": self._arm.mode,
+        }
+
+    def read_error(self) -> tuple[int, str]:
+        """Read error code and message."""
+        if not self._arm:
+            return 0, ""
+
+        code = self._arm.error_code
+        if code == 0:
+            return 0, ""
+        return code, f"XArm error {code}"
+
+    # =========================================================================
+    # Motion Control (Joint Space)
+    # =========================================================================
+
+    def write_joint_positions(
+        self,
+        positions: list[float],
+        velocity: float = 1.0,
+    ) -> bool:
+        """Write joint positions for servo mode (radians -> degrees).
+
+        Uses set_servo_angle_j() for high-frequency servo control.
+        Requires mode 1 (servo mode) to be active.
+
+        Args:
+            positions: Target positions in radians
+            velocity: Speed as fraction of max (0-1) - not used in servo mode
+        """
+        if not self._arm:
+            return False
+
+        # Convert radians to degrees
+        angles = [math.degrees(p) for p in positions]
+
+        # Use set_servo_angle_j for high-frequency servo control (100Hz+)
+        # This only executes the last instruction, suitable for real-time control
+        code: int = self._arm.set_servo_angle_j(angles, speed=100, mvacc=500)
+        return code == 0
+
+    def write_joint_velocities(self, velocities: list[float]) -> bool:
+        """Write joint velocities (rad/s -> deg/s).
+
+        Note: Requires velocity mode to be active.
+        """
+        if not self._arm:
+            return False
+
+        # Convert rad/s to deg/s
+        speeds = [math.degrees(v) for v in velocities]
+        code: int = self._arm.vc_set_joint_velocity(speeds)
+        return code == 0
+
+    def write_stop(self) -> bool:
+        """Emergency stop."""
+        if not self._arm:
+            return False
+        code: int = self._arm.emergency_stop()
+        return code == 0
+
+    # =========================================================================
+    # Servo Control
+    # =========================================================================
+
+    def write_enable(self, enable: bool) -> bool:
+        """Enable or disable servos."""
+        if not self._arm:
+            return False
+        code: int = self._arm.motion_enable(enable=enable)
+        return code == 0
+
+    def read_enabled(self) -> bool:
+        """Check if servos are enabled."""
+        if not self._arm:
+            return False
+        # XArm state 0 = ready/enabled
+        state: int = self._arm.state
+        return state == 0
+
+    def write_clear_errors(self) -> bool:
+        """Clear error state."""
+        if not self._arm:
+            return False
+        code: int = self._arm.clean_error()
+        return code == 0
+
+    # =========================================================================
+    # Cartesian Control (Optional)
+    # =========================================================================
+
+    def read_cartesian_position(self) -> dict[str, float] | None:
+        """Read end-effector pose (mm -> meters, degrees -> radians)."""
+        if not self._arm:
+            return 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]),
+            }
+        return None
+
+    def write_cartesian_position(
+        self,
+        pose: dict[str, float],
+        velocity: float = 1.0,
+    ) -> bool:
+        """Write end-effector pose (meters -> mm, radians -> degrees)."""
+        if not self._arm:
+            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),
+            wait=False,
+        )
+        return code == 0
+
+    # =========================================================================
+    # Gripper (Optional)
+    # =========================================================================
+
+    def read_gripper_position(self) -> float | None:
+        """Read gripper position (mm -> meters)."""
+        if not self._arm:
+            return None
+
+        result = self._arm.get_gripper_position()
+        code: int = result[0]
+        pos: float | None = result[1]
+        if code == 0 and pos is not None:
+            return pos / 1000.0  # mm -> m
+        return None
+
+    def write_gripper_position(self, position: float) -> bool:
+        """Write gripper position (meters -> mm)."""
+        if not self._arm:
+            return False
+
+        pos_mm = position * 1000.0  # m -> mm
+        code: int = self._arm.set_gripper_position(pos_mm)
+        return code == 0
+
+    # =========================================================================
+    # Force/Torque Sensor (Optional)
+    # =========================================================================
+
+    def read_force_torque(self) -> list[float] | None:
+        """Read F/T sensor data if available."""
+        if not self._arm:
+            return None
+
+        code, ft = self._arm.get_ft_sensor_data()
+        if code == 0 and ft:
+            return list(ft)
+        return None
+
+
+__all__ = ["XArmBackend"]
diff --git a/dimos/hardware/manipulators/xarm/components/__init__.py b/dimos/hardware/manipulators/xarm/components/__init__.py
deleted file mode 100644
index 4592560cda..0000000000
--- a/dimos/hardware/manipulators/xarm/components/__init__.py
+++ /dev/null
@@ -1,15 +0,0 @@
-"""Component classes for XArmDriver."""
-
-from .gripper_control import GripperControlComponent
-from .kinematics import KinematicsComponent
-from .motion_control import MotionControlComponent
-from .state_queries import StateQueryComponent
-from .system_control import SystemControlComponent
-
-__all__ = [
-    "GripperControlComponent",
-    "KinematicsComponent",
-    "MotionControlComponent",
-    "StateQueryComponent",
-    "SystemControlComponent",
-]
diff --git a/dimos/hardware/manipulators/xarm/components/gripper_control.py b/dimos/hardware/manipulators/xarm/components/gripper_control.py
deleted file mode 100644
index 13b8347978..0000000000
--- a/dimos/hardware/manipulators/xarm/components/gripper_control.py
+++ /dev/null
@@ -1,372 +0,0 @@
-# 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.
-
-"""
-Gripper Control Component for XArmDriver.
-
-Provides RPC methods for controlling various grippers:
-- Standard xArm gripper
-- Bio gripper
-- Vacuum gripper
-- Robotiq gripper
-"""
-
-from typing import TYPE_CHECKING, Any
-
-from dimos.core import rpc
-from dimos.utils.logging_config import setup_logger
-
-if TYPE_CHECKING:
-    from xarm.wrapper import XArmAPI
-
-logger = setup_logger()
-
-
-class GripperControlComponent:
-    """
-    Component providing gripper control RPC methods for XArmDriver.
-
-    This component assumes the parent class has:
-    - self.arm: XArmAPI instance
-    - self.config: XArmDriverConfig instance
-    """
-
-    # Type hints for attributes expected from parent class
-    arm: "XArmAPI"
-    config: Any  # Config dict accessed as object (dict with attribute access)
-
-    # =========================================================================
-    # Standard xArm Gripper
-    # =========================================================================
-
-    @rpc
-    def set_gripper_enable(self, enable: int) -> tuple[int, str]:
-        """Enable/disable gripper."""
-        try:
-            code = self.arm.set_gripper_enable(enable)
-            return (
-                code,
-                f"Gripper {'enabled' if enable else 'disabled'}"
-                if code == 0
-                else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_gripper_mode(self, mode: int) -> tuple[int, str]:
-        """Set gripper mode (0=location mode, 1=speed mode, 2=current mode)."""
-        try:
-            code = self.arm.set_gripper_mode(mode)
-            return (code, f"Gripper mode set to {mode}" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_gripper_speed(self, speed: float) -> tuple[int, str]:
-        """Set gripper speed (r/min)."""
-        try:
-            code = self.arm.set_gripper_speed(speed)
-            return (code, f"Gripper speed set to {speed}" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_gripper_position(
-        self,
-        position: float,
-        wait: bool = False,
-        speed: float | None = None,
-        timeout: float | None = None,
-    ) -> tuple[int, str]:
-        """
-        Set gripper position.
-
-        Args:
-            position: Target position (0-850)
-            wait: Wait for completion
-            speed: Optional speed override
-            timeout: Optional timeout for wait
-        """
-        try:
-            code = self.arm.set_gripper_position(position, wait=wait, speed=speed, timeout=timeout)
-            return (
-                code,
-                f"Gripper position set to {position}" if code == 0 else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def get_gripper_position(self) -> tuple[int, float | None]:
-        """Get current gripper position."""
-        try:
-            code, position = self.arm.get_gripper_position()
-            return (code, position if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def get_gripper_err_code(self) -> tuple[int, int | None]:
-        """Get gripper error code."""
-        try:
-            code, err = self.arm.get_gripper_err_code()
-            return (code, err if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def clean_gripper_error(self) -> tuple[int, str]:
-        """Clear gripper error."""
-        try:
-            code = self.arm.clean_gripper_error()
-            return (code, "Gripper error cleared" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    # =========================================================================
-    # Bio Gripper
-    # =========================================================================
-
-    @rpc
-    def set_bio_gripper_enable(self, enable: int, wait: bool = True) -> tuple[int, str]:
-        """Enable/disable bio gripper."""
-        try:
-            code = self.arm.set_bio_gripper_enable(enable, wait=wait)
-            return (
-                code,
-                f"Bio gripper {'enabled' if enable else 'disabled'}"
-                if code == 0
-                else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_bio_gripper_speed(self, speed: int) -> tuple[int, str]:
-        """Set bio gripper speed (1-100)."""
-        try:
-            code = self.arm.set_bio_gripper_speed(speed)
-            return (
-                code,
-                f"Bio gripper speed set to {speed}" if code == 0 else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def open_bio_gripper(
-        self, speed: int = 0, wait: bool = True, timeout: float = 5
-    ) -> tuple[int, str]:
-        """Open bio gripper."""
-        try:
-            code = self.arm.open_bio_gripper(speed=speed, wait=wait, timeout=timeout)
-            return (code, "Bio gripper opened" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def close_bio_gripper(
-        self, speed: int = 0, wait: bool = True, timeout: float = 5
-    ) -> tuple[int, str]:
-        """Close bio gripper."""
-        try:
-            code = self.arm.close_bio_gripper(speed=speed, wait=wait, timeout=timeout)
-            return (code, "Bio gripper closed" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def get_bio_gripper_status(self) -> tuple[int, int | None]:
-        """Get bio gripper status."""
-        try:
-            code, status = self.arm.get_bio_gripper_status()
-            return (code, status if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def get_bio_gripper_error(self) -> tuple[int, int | None]:
-        """Get bio gripper error code."""
-        try:
-            code, error = self.arm.get_bio_gripper_error()
-            return (code, error if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def clean_bio_gripper_error(self) -> tuple[int, str]:
-        """Clear bio gripper error."""
-        try:
-            code = self.arm.clean_bio_gripper_error()
-            return (code, "Bio gripper error cleared" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    # =========================================================================
-    # Vacuum Gripper
-    # =========================================================================
-
-    @rpc
-    def set_vacuum_gripper(self, on: int) -> tuple[int, str]:
-        """Turn vacuum gripper on/off (0=off, 1=on)."""
-        try:
-            code = self.arm.set_vacuum_gripper(on)
-            return (
-                code,
-                f"Vacuum gripper {'on' if on else 'off'}" if code == 0 else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def get_vacuum_gripper(self) -> tuple[int, int | None]:
-        """Get vacuum gripper state."""
-        try:
-            code, state = self.arm.get_vacuum_gripper()
-            return (code, state if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    # =========================================================================
-    # Robotiq Gripper
-    # =========================================================================
-
-    @rpc
-    def robotiq_reset(self) -> tuple[int, str]:
-        """Reset Robotiq gripper."""
-        try:
-            code = self.arm.robotiq_reset()
-            return (code, "Robotiq gripper reset" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def robotiq_set_activate(self, wait: bool = True, timeout: float = 3) -> tuple[int, str]:
-        """Activate Robotiq gripper."""
-        try:
-            code = self.arm.robotiq_set_activate(wait=wait, timeout=timeout)
-            return (code, "Robotiq gripper activated" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def robotiq_set_position(
-        self,
-        position: int,
-        speed: int = 0xFF,
-        force: int = 0xFF,
-        wait: bool = True,
-        timeout: float = 5,
-    ) -> tuple[int, str]:
-        """
-        Set Robotiq gripper position.
-
-        Args:
-            position: Target position (0-255, 0=open, 255=closed)
-            speed: Gripper speed (0-255)
-            force: Gripper force (0-255)
-            wait: Wait for completion
-            timeout: Timeout for wait
-        """
-        try:
-            code = self.arm.robotiq_set_position(
-                position, speed=speed, force=force, wait=wait, timeout=timeout
-            )
-            return (
-                code,
-                f"Robotiq position set to {position}" if code == 0 else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def robotiq_open(
-        self, speed: int = 0xFF, force: int = 0xFF, wait: bool = True, timeout: float = 5
-    ) -> tuple[int, str]:
-        """Open Robotiq gripper."""
-        try:
-            code = self.arm.robotiq_open(speed=speed, force=force, wait=wait, timeout=timeout)
-            return (code, "Robotiq gripper opened" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def robotiq_close(
-        self, speed: int = 0xFF, force: int = 0xFF, wait: bool = True, timeout: float = 5
-    ) -> tuple[int, str]:
-        """Close Robotiq gripper."""
-        try:
-            code = self.arm.robotiq_close(speed=speed, force=force, wait=wait, timeout=timeout)
-            return (code, "Robotiq gripper closed" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def robotiq_get_status(self) -> tuple[int, dict[str, Any] | None]:
-        """Get Robotiq gripper status."""
-        try:
-            ret = self.arm.robotiq_get_status()
-            if isinstance(ret, tuple) and len(ret) >= 2:
-                code = ret[0]
-                if code == 0:
-                    # Return status as dict if successful
-                    status = {
-                        "gOBJ": ret[1] if len(ret) > 1 else None,  # Object detection status
-                        "gSTA": ret[2] if len(ret) > 2 else None,  # Gripper status
-                        "gGTO": ret[3] if len(ret) > 3 else None,  # Go to requested position
-                        "gACT": ret[4] if len(ret) > 4 else None,  # Activation status
-                        "kFLT": ret[5] if len(ret) > 5 else None,  # Fault status
-                        "gFLT": ret[6] if len(ret) > 6 else None,  # Fault status
-                        "gPR": ret[7] if len(ret) > 7 else None,  # Requested position echo
-                        "gPO": ret[8] if len(ret) > 8 else None,  # Actual position
-                        "gCU": ret[9] if len(ret) > 9 else None,  # Current
-                    }
-                    return (code, status)
-                return (code, None)
-            return (-1, None)
-        except Exception as e:
-            logger.error(f"robotiq_get_status failed: {e}")
-            return (-1, None)
-
-    # =========================================================================
-    # Lite6 Gripper
-    # =========================================================================
-
-    @rpc
-    def open_lite6_gripper(self) -> tuple[int, str]:
-        """Open Lite6 gripper."""
-        try:
-            code = self.arm.open_lite6_gripper()
-            return (code, "Lite6 gripper opened" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def close_lite6_gripper(self) -> tuple[int, str]:
-        """Close Lite6 gripper."""
-        try:
-            code = self.arm.close_lite6_gripper()
-            return (code, "Lite6 gripper closed" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def stop_lite6_gripper(self) -> tuple[int, str]:
-        """Stop Lite6 gripper."""
-        try:
-            code = self.arm.stop_lite6_gripper()
-            return (code, "Lite6 gripper stopped" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
diff --git a/dimos/hardware/manipulators/xarm/components/kinematics.py b/dimos/hardware/manipulators/xarm/components/kinematics.py
deleted file mode 100644
index c29007a426..0000000000
--- a/dimos/hardware/manipulators/xarm/components/kinematics.py
+++ /dev/null
@@ -1,85 +0,0 @@
-# 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.
-
-"""
-Kinematics Component for XArmDriver.
-
-Provides RPC methods for kinematic calculations including:
-- Forward kinematics
-- Inverse kinematics
-"""
-
-from typing import TYPE_CHECKING, Any
-
-from dimos.core import rpc
-from dimos.utils.logging_config import setup_logger
-
-if TYPE_CHECKING:
-    from xarm.wrapper import XArmAPI
-
-logger = setup_logger()
-
-
-class KinematicsComponent:
-    """
-    Component providing kinematics RPC methods for XArmDriver.
-
-    This component assumes the parent class has:
-    - self.arm: XArmAPI instance
-    - self.config: XArmDriverConfig instance
-    """
-
-    # Type hints for attributes expected from parent class
-    arm: "XArmAPI"
-    config: Any  # Config dict accessed as object (dict with attribute access)
-
-    @rpc
-    def get_inverse_kinematics(self, pose: list[float]) -> tuple[int, list[float] | None]:
-        """
-        Compute inverse kinematics.
-
-        Args:
-            pose: [x, y, z, roll, pitch, yaw]
-
-        Returns:
-            Tuple of (code, joint_angles)
-        """
-        try:
-            code, angles = self.arm.get_inverse_kinematics(
-                pose, input_is_radian=self.config.is_radian, return_is_radian=self.config.is_radian
-            )
-            return (code, list(angles) if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def get_forward_kinematics(self, angles: list[float]) -> tuple[int, list[float] | None]:
-        """
-        Compute forward kinematics.
-
-        Args:
-            angles: Joint angles
-
-        Returns:
-            Tuple of (code, pose)
-        """
-        try:
-            code, pose = self.arm.get_forward_kinematics(
-                angles,
-                input_is_radian=self.config.is_radian,
-                return_is_radian=self.config.is_radian,
-            )
-            return (code, list(pose) if code == 0 else None)
-        except Exception:
-            return (-1, None)
diff --git a/dimos/hardware/manipulators/xarm/components/motion_control.py b/dimos/hardware/manipulators/xarm/components/motion_control.py
deleted file mode 100644
index 64aaa861e0..0000000000
--- a/dimos/hardware/manipulators/xarm/components/motion_control.py
+++ /dev/null
@@ -1,147 +0,0 @@
-# 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.
-
-"""
-Motion Control Component for XArmDriver.
-
-Provides RPC methods for motion control operations including:
-- Joint position control
-- Joint velocity control
-- Cartesian position control
-- Home positioning
-"""
-
-import math
-import threading
-from typing import TYPE_CHECKING, Any
-
-from dimos.core import rpc
-from dimos.utils.logging_config import setup_logger
-
-if TYPE_CHECKING:
-    from xarm.wrapper import XArmAPI
-
-logger = setup_logger()
-
-
-class MotionControlComponent:
-    """
-    Component providing motion control RPC methods for XArmDriver.
-
-    This component assumes the parent class has:
-    - self.arm: XArmAPI instance
-    - self.config: XArmDriverConfig instance
-    - self._joint_cmd_lock: threading.Lock
-    - self._joint_cmd_: Optional[list[float]]
-    """
-
-    # Type hints for attributes expected from parent class
-    arm: "XArmAPI"
-    config: Any  # Config dict accessed as object (dict with attribute access)
-    _joint_cmd_lock: threading.Lock
-    _joint_cmd_: list[float] | None
-
-    @rpc
-    def set_joint_angles(self, angles: list[float]) -> tuple[int, str]:
-        """
-        Set joint angles (RPC method).
-
-        Args:
-            angles: List of joint angles (in radians if is_radian=True)
-
-        Returns:
-            Tuple of (code, message)
-        """
-        try:
-            code = self.arm.set_servo_angle_j(angles=angles, is_radian=self.config.is_radian)
-            msg = "Success" if code == 0 else f"Error code: {code}"
-            return (code, msg)
-        except Exception as e:
-            logger.error(f"set_joint_angles failed: {e}")
-            return (-1, str(e))
-
-    @rpc
-    def set_joint_velocities(self, velocities: list[float]) -> tuple[int, str]:
-        """
-        Set joint velocities (RPC method).
-        Note: Requires velocity control mode.
-
-        Args:
-            velocities: List of joint velocities (rad/s)
-
-        Returns:
-            Tuple of (code, message)
-        """
-        try:
-            # For velocity control, you would use vc_set_joint_velocity
-            # This requires mode 4 (joint velocity control)
-            code = self.arm.vc_set_joint_velocity(
-                speeds=velocities, is_radian=self.config.is_radian
-            )
-            msg = "Success" if code == 0 else f"Error code: {code}"
-            return (code, msg)
-        except Exception as e:
-            logger.error(f"set_joint_velocities failed: {e}")
-            return (-1, str(e))
-
-    @rpc
-    def set_position(self, position: list[float], wait: bool = False) -> tuple[int, str]:
-        """
-        Set TCP position [x, y, z, roll, pitch, yaw].
-
-        Args:
-            position: Target position
-            wait: Wait for motion to complete
-
-        Returns:
-            Tuple of (code, message)
-        """
-        try:
-            code = self.arm.set_position(*position, is_radian=self.config.is_radian, wait=wait)
-            return (code, "Success" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def move_gohome(self, wait: bool = False) -> tuple[int, str]:
-        """Move to home position."""
-        try:
-            code = self.arm.move_gohome(wait=wait, is_radian=self.config.is_radian)
-            return (code, "Moving home" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_joint_command(self, positions: list[float]) -> tuple[int, str]:
-        """
-        Manually set the joint command (for testing).
-        This updates the shared joint_cmd that the control loop reads.
-
-        Args:
-            positions: List of joint positions in radians
-
-        Returns:
-            Tuple of (code, message)
-        """
-        try:
-            if len(positions) != self.config.num_joints:
-                return (-1, f"Expected {self.config.num_joints} positions, got {len(positions)}")
-
-            with self._joint_cmd_lock:
-                self._joint_cmd_ = list(positions)
-
-            logger.info(f"✓ Joint command set: {[f'{math.degrees(p):.2f}°' for p in positions]}")
-            return (0, "Joint command updated")
-        except Exception as e:
-            return (-1, str(e))
diff --git a/dimos/hardware/manipulators/xarm/components/state_queries.py b/dimos/hardware/manipulators/xarm/components/state_queries.py
deleted file mode 100644
index 5615763cc4..0000000000
--- a/dimos/hardware/manipulators/xarm/components/state_queries.py
+++ /dev/null
@@ -1,185 +0,0 @@
-# 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.
-
-"""
-State Query Component for XArmDriver.
-
-Provides RPC methods for querying robot state including:
-- Joint state
-- Robot state
-- Cartesian position
-- Firmware version
-"""
-
-import threading
-from typing import TYPE_CHECKING, Any
-
-from dimos.core import rpc
-from dimos.msgs.sensor_msgs import JointState, RobotState
-from dimos.utils.logging_config import setup_logger
-
-if TYPE_CHECKING:
-    from xarm.wrapper import XArmAPI
-
-logger = setup_logger()
-
-
-class StateQueryComponent:
-    """
-    Component providing state query RPC methods for XArmDriver.
-
-    This component assumes the parent class has:
-    - self.arm: XArmAPI instance
-    - self.config: XArmDriverConfig instance
-    - self._joint_state_lock: threading.Lock
-    - self._joint_states_: Optional[JointState]
-    - self._robot_state_: Optional[RobotState]
-    """
-
-    # Type hints for attributes expected from parent class
-    arm: "XArmAPI"
-    config: Any  # Config dict accessed as object (dict with attribute access)
-    _joint_state_lock: threading.Lock
-    _joint_states_: JointState | None
-    _robot_state_: RobotState | None
-
-    @rpc
-    def get_joint_state(self) -> JointState | None:
-        """
-        Get the current joint state (RPC method).
-
-        Returns:
-            Current JointState or None
-        """
-        with self._joint_state_lock:
-            return self._joint_states_
-
-    @rpc
-    def get_robot_state(self) -> RobotState | None:
-        """
-        Get the current robot state (RPC method).
-
-        Returns:
-            Current RobotState or None
-        """
-        with self._joint_state_lock:
-            return self._robot_state_
-
-    @rpc
-    def get_position(self) -> tuple[int, list[float] | None]:
-        """
-        Get TCP position [x, y, z, roll, pitch, yaw].
-
-        Returns:
-            Tuple of (code, position)
-        """
-        try:
-            code, position = self.arm.get_position(is_radian=self.config.is_radian)
-            return (code, list(position) if code == 0 else None)
-        except Exception as e:
-            logger.error(f"get_position failed: {e}")
-            return (-1, None)
-
-    @rpc
-    def get_version(self) -> tuple[int, str | None]:
-        """Get firmware version."""
-        try:
-            code, version = self.arm.get_version()
-            return (code, version if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def get_servo_angle(self) -> tuple[int, list[float] | None]:
-        """Get joint angles."""
-        try:
-            code, angles = self.arm.get_servo_angle(is_radian=self.config.is_radian)
-            return (code, list(angles) if code == 0 else None)
-        except Exception as e:
-            logger.error(f"get_servo_angle failed: {e}")
-            return (-1, None)
-
-    @rpc
-    def get_position_aa(self) -> tuple[int, list[float] | None]:
-        """Get TCP position in axis-angle format."""
-        try:
-            code, position = self.arm.get_position_aa(is_radian=self.config.is_radian)
-            return (code, list(position) if code == 0 else None)
-        except Exception as e:
-            logger.error(f"get_position_aa failed: {e}")
-            return (-1, None)
-
-    # =========================================================================
-    # Robot State Queries
-    # =========================================================================
-
-    @rpc
-    def get_state(self) -> tuple[int, int | None]:
-        """Get robot state (0=ready, 3=pause, 4=stop)."""
-        try:
-            code, state = self.arm.get_state()
-            return (code, state if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def get_cmdnum(self) -> tuple[int, int | None]:
-        """Get command queue length."""
-        try:
-            code, cmdnum = self.arm.get_cmdnum()
-            return (code, cmdnum if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def get_err_warn_code(self) -> tuple[int, list[int] | None]:
-        """Get error and warning codes."""
-        try:
-            err_warn = [0, 0]
-            code = self.arm.get_err_warn_code(err_warn)
-            return (code, err_warn if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    # =========================================================================
-    # Force/Torque Sensor Queries
-    # =========================================================================
-
-    @rpc
-    def get_ft_sensor_data(self) -> tuple[int, list[float] | None]:
-        """Get force/torque sensor data [fx, fy, fz, tx, ty, tz]."""
-        try:
-            code, ft_data = self.arm.get_ft_sensor_data()
-            return (code, list(ft_data) if code == 0 else None)
-        except Exception as e:
-            logger.error(f"get_ft_sensor_data failed: {e}")
-            return (-1, None)
-
-    @rpc
-    def get_ft_sensor_error(self) -> tuple[int, int | None]:
-        """Get FT sensor error code."""
-        try:
-            code, error = self.arm.get_ft_sensor_error()
-            return (code, error if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def get_ft_sensor_mode(self) -> tuple[int, int | None]:
-        """Get FT sensor application mode."""
-        try:
-            code, mode = self.arm.get_ft_sensor_app_get()
-            return (code, mode if code == 0 else None)
-        except Exception:
-            return (-1, None)
diff --git a/dimos/hardware/manipulators/xarm/components/system_control.py b/dimos/hardware/manipulators/xarm/components/system_control.py
deleted file mode 100644
index a04e9a94a0..0000000000
--- a/dimos/hardware/manipulators/xarm/components/system_control.py
+++ /dev/null
@@ -1,555 +0,0 @@
-# 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.
-
-"""
-System Control Component for XArmDriver.
-
-Provides RPC methods for system-level control operations including:
-- Mode control (servo, velocity)
-- State management
-- Error handling
-- Emergency stop
-"""
-
-from typing import TYPE_CHECKING, Any, Protocol
-
-from dimos.core import rpc
-from dimos.utils.logging_config import setup_logger
-
-if TYPE_CHECKING:
-    from xarm.wrapper import XArmAPI
-
-    class XArmConfig(Protocol):
-        """Protocol for XArm configuration."""
-
-        is_radian: bool
-        velocity_control: bool
-
-
-logger = setup_logger()
-
-
-class SystemControlComponent:
-    """
-    Component providing system control RPC methods for XArmDriver.
-
-    This component assumes the parent class has:
-    - self.arm: XArmAPI instance
-    - self.config: XArmDriverConfig instance
-    """
-
-    # Type hints for attributes expected from parent class
-    arm: "XArmAPI"
-    config: Any  # Should be XArmConfig but accessed as dict
-
-    @rpc
-    def enable_servo_mode(self) -> tuple[int, str]:
-        """
-        Enable servo mode (mode 1).
-        Required for set_servo_angle_j to work.
-
-        Returns:
-            Tuple of (code, message)
-        """
-        try:
-            code = self.arm.set_mode(1)
-            if code == 0:
-                logger.info("Servo mode enabled")
-                return (code, "Servo mode enabled")
-            else:
-                logger.warning(f"Failed to enable servo mode: code={code}")
-                return (code, f"Error code: {code}")
-        except Exception as e:
-            logger.error(f"enable_servo_mode failed: {e}")
-            return (-1, str(e))
-
-    @rpc
-    def disable_servo_mode(self) -> tuple[int, str]:
-        """
-        Disable servo mode (set to position mode).
-
-        Returns:
-            Tuple of (code, message)
-        """
-        try:
-            code = self.arm.set_mode(0)
-            if code == 0:
-                logger.info("Servo mode disabled (position mode)")
-                return (code, "Position mode enabled")
-            else:
-                logger.warning(f"Failed to disable servo mode: code={code}")
-                return (code, f"Error code: {code}")
-        except Exception as e:
-            logger.error(f"disable_servo_mode failed: {e}")
-            return (-1, str(e))
-
-    @rpc
-    def enable_velocity_control_mode(self) -> tuple[int, str]:
-        """
-        Enable velocity control mode (mode 4).
-        Required for vc_set_joint_velocity to work.
-
-        Returns:
-            Tuple of (code, message)
-        """
-        try:
-            # IMPORTANT: Set config flag BEFORE changing robot mode
-            # This prevents control loop from sending wrong command type during transition
-            self.config.velocity_control = True
-
-            # Step 1: Set mode to 4 (velocity control)
-            code = self.arm.set_mode(4)
-            if code != 0:
-                logger.warning(f"Failed to set mode to 4: code={code}")
-                self.config.velocity_control = False  # Revert on failure
-                return (code, f"Failed to set mode: code={code}")
-
-            # Step 2: Set state to 0 (ready/sport mode) - this activates the mode!
-            code = self.arm.set_state(0)
-            if code == 0:
-                logger.info("Velocity control mode enabled (mode=4, state=0)")
-                return (code, "Velocity control mode enabled")
-            else:
-                logger.warning(f"Failed to set state to 0: code={code}")
-                self.config.velocity_control = False  # Revert on failure
-                return (code, f"Failed to set state: code={code}")
-        except Exception as e:
-            logger.error(f"enable_velocity_control_mode failed: {e}")
-            self.config.velocity_control = False  # Revert on exception
-            return (-1, str(e))
-
-    @rpc
-    def disable_velocity_control_mode(self) -> tuple[int, str]:
-        """
-        Disable velocity control mode and return to position control (mode 1).
-
-        Returns:
-            Tuple of (code, message)
-        """
-        try:
-            # IMPORTANT: Set config flag BEFORE changing robot mode
-            # This prevents control loop from sending velocity commands after mode change
-            self.config.velocity_control = False
-
-            # Step 1: Clear any errors that may have occurred
-            self.arm.clean_error()
-            self.arm.clean_warn()
-
-            # Step 2: Set mode to 1 (servo/position control)
-            code = self.arm.set_mode(1)
-            if code != 0:
-                logger.warning(f"Failed to set mode to 1: code={code}")
-                self.config.velocity_control = True  # Revert on failure
-                return (code, f"Failed to set mode: code={code}")
-
-            # Step 3: Set state to 0 (ready) - CRITICAL for accepting new commands
-            code = self.arm.set_state(0)
-            if code == 0:
-                logger.info("Position control mode enabled (state=0, mode=1)")
-                return (code, "Position control mode enabled")
-            else:
-                logger.warning(f"Failed to set state to 0: code={code}")
-                self.config.velocity_control = True  # Revert on failure
-                return (code, f"Failed to set state: code={code}")
-        except Exception as e:
-            logger.error(f"disable_velocity_control_mode failed: {e}")
-            self.config.velocity_control = True  # Revert on exception
-            return (-1, str(e))
-
-    @rpc
-    def motion_enable(self, enable: bool = True) -> tuple[int, str]:
-        """Enable or disable arm motion."""
-        try:
-            code = self.arm.motion_enable(enable=enable)
-            msg = f"Motion {'enabled' if enable else 'disabled'}"
-            return (code, msg if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_state(self, state: int) -> tuple[int, str]:
-        """
-        Set robot state.
-
-        Args:
-            state: 0=ready, 3=pause, 4=stop
-        """
-        try:
-            code = self.arm.set_state(state=state)
-            return (code, "Success" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def clean_error(self) -> tuple[int, str]:
-        """Clear error codes."""
-        try:
-            code = self.arm.clean_error()
-            return (code, "Errors cleared" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def clean_warn(self) -> tuple[int, str]:
-        """Clear warning codes."""
-        try:
-            code = self.arm.clean_warn()
-            return (code, "Warnings cleared" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def emergency_stop(self) -> tuple[int, str]:
-        """Emergency stop the arm."""
-        try:
-            code = self.arm.emergency_stop()
-            return (code, "Emergency stop" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    # =========================================================================
-    # Configuration & Persistence
-    # =========================================================================
-
-    @rpc
-    def clean_conf(self) -> tuple[int, str]:
-        """Clean configuration."""
-        try:
-            code = self.arm.clean_conf()
-            return (code, "Configuration cleaned" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def save_conf(self) -> tuple[int, str]:
-        """Save current configuration to robot."""
-        try:
-            code = self.arm.save_conf()
-            return (code, "Configuration saved" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def reload_dynamics(self) -> tuple[int, str]:
-        """Reload dynamics parameters."""
-        try:
-            code = self.arm.reload_dynamics()
-            return (code, "Dynamics reloaded" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    # =========================================================================
-    # Mode & State Control
-    # =========================================================================
-
-    @rpc
-    def set_mode(self, mode: int) -> tuple[int, str]:
-        """
-        Set control mode.
-
-        Args:
-            mode: 0=position, 1=servo, 4=velocity, etc.
-        """
-        try:
-            code = self.arm.set_mode(mode)
-            return (code, f"Mode set to {mode}" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    # =========================================================================
-    # Collision & Safety
-    # =========================================================================
-
-    @rpc
-    def set_collision_sensitivity(self, sensitivity: int) -> tuple[int, str]:
-        """Set collision sensitivity (0-5, 0=least sensitive)."""
-        try:
-            code = self.arm.set_collision_sensitivity(sensitivity)
-            return (
-                code,
-                f"Collision sensitivity set to {sensitivity}"
-                if code == 0
-                else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_teach_sensitivity(self, sensitivity: int) -> tuple[int, str]:
-        """Set teach sensitivity (1-5)."""
-        try:
-            code = self.arm.set_teach_sensitivity(sensitivity)
-            return (
-                code,
-                f"Teach sensitivity set to {sensitivity}" if code == 0 else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_collision_rebound(self, enable: int) -> tuple[int, str]:
-        """Enable/disable collision rebound (0=disable, 1=enable)."""
-        try:
-            code = self.arm.set_collision_rebound(enable)
-            return (
-                code,
-                f"Collision rebound {'enabled' if enable else 'disabled'}"
-                if code == 0
-                else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_self_collision_detection(self, enable: int) -> tuple[int, str]:
-        """Enable/disable self collision detection."""
-        try:
-            code = self.arm.set_self_collision_detection(enable)
-            return (
-                code,
-                f"Self collision detection {'enabled' if enable else 'disabled'}"
-                if code == 0
-                else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    # =========================================================================
-    # Reduced Mode & Boundaries
-    # =========================================================================
-
-    @rpc
-    def set_reduced_mode(self, enable: int) -> tuple[int, str]:
-        """Enable/disable reduced mode."""
-        try:
-            code = self.arm.set_reduced_mode(enable)
-            return (
-                code,
-                f"Reduced mode {'enabled' if enable else 'disabled'}"
-                if code == 0
-                else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_reduced_max_tcp_speed(self, speed: float) -> tuple[int, str]:
-        """Set maximum TCP speed in reduced mode."""
-        try:
-            code = self.arm.set_reduced_max_tcp_speed(speed)
-            return (
-                code,
-                f"Reduced max TCP speed set to {speed}" if code == 0 else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_reduced_max_joint_speed(self, speed: float) -> tuple[int, str]:
-        """Set maximum joint speed in reduced mode."""
-        try:
-            code = self.arm.set_reduced_max_joint_speed(speed)
-            return (
-                code,
-                f"Reduced max joint speed set to {speed}" if code == 0 else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_fence_mode(self, enable: int) -> tuple[int, str]:
-        """Enable/disable fence mode."""
-        try:
-            code = self.arm.set_fence_mode(enable)
-            return (
-                code,
-                f"Fence mode {'enabled' if enable else 'disabled'}"
-                if code == 0
-                else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    # =========================================================================
-    # TCP & Dynamics Configuration
-    # =========================================================================
-
-    @rpc
-    def set_tcp_offset(self, offset: list[float]) -> tuple[int, str]:
-        """Set TCP offset [x, y, z, roll, pitch, yaw]."""
-        try:
-            code = self.arm.set_tcp_offset(offset)
-            return (code, "TCP offset set" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_tcp_load(self, weight: float, center_of_gravity: list[float]) -> tuple[int, str]:
-        """Set TCP load (payload)."""
-        try:
-            code = self.arm.set_tcp_load(weight, center_of_gravity)
-            return (code, f"TCP load set: {weight}kg" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_gravity_direction(self, direction: list[float]) -> tuple[int, str]:
-        """Set gravity direction vector."""
-        try:
-            code = self.arm.set_gravity_direction(direction)
-            return (code, "Gravity direction set" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_world_offset(self, offset: list[float]) -> tuple[int, str]:
-        """Set world coordinate offset."""
-        try:
-            code = self.arm.set_world_offset(offset)
-            return (code, "World offset set" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    # =========================================================================
-    # Motion Parameters
-    # =========================================================================
-
-    @rpc
-    def set_tcp_jerk(self, jerk: float) -> tuple[int, str]:
-        """Set TCP jerk (mm/s³)."""
-        try:
-            code = self.arm.set_tcp_jerk(jerk)
-            return (code, f"TCP jerk set to {jerk}" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_tcp_maxacc(self, acc: float) -> tuple[int, str]:
-        """Set TCP maximum acceleration (mm/s²)."""
-        try:
-            code = self.arm.set_tcp_maxacc(acc)
-            return (
-                code,
-                f"TCP max acceleration set to {acc}" if code == 0 else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_joint_jerk(self, jerk: float) -> tuple[int, str]:
-        """Set joint jerk (rad/s³ or °/s³)."""
-        try:
-            code = self.arm.set_joint_jerk(jerk, is_radian=self.config.is_radian)
-            return (code, f"Joint jerk set to {jerk}" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_joint_maxacc(self, acc: float) -> tuple[int, str]:
-        """Set joint maximum acceleration (rad/s² or °/s²)."""
-        try:
-            code = self.arm.set_joint_maxacc(acc, is_radian=self.config.is_radian)
-            return (
-                code,
-                f"Joint max acceleration set to {acc}" if code == 0 else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
-
-    @rpc
-    def set_pause_time(self, seconds: float) -> tuple[int, str]:
-        """Set pause time for motion commands."""
-        try:
-            code = self.arm.set_pause_time(seconds)
-            return (code, f"Pause time set to {seconds}s" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    # =========================================================================
-    # Digital I/O (Tool GPIO)
-    # =========================================================================
-
-    @rpc
-    def get_tgpio_digital(self, io_num: int) -> tuple[int, int | None]:
-        """Get tool GPIO digital input value."""
-        try:
-            code, value = self.arm.get_tgpio_digital(io_num)
-            return (code, value if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def set_tgpio_digital(self, io_num: int, value: int) -> tuple[int, str]:
-        """Set tool GPIO digital output value (0 or 1)."""
-        try:
-            code = self.arm.set_tgpio_digital(io_num, value)
-            return (code, f"TGPIO {io_num} set to {value}" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    # =========================================================================
-    # Digital I/O (Controller GPIO)
-    # =========================================================================
-
-    @rpc
-    def get_cgpio_digital(self, io_num: int) -> tuple[int, int | None]:
-        """Get controller GPIO digital input value."""
-        try:
-            code, value = self.arm.get_cgpio_digital(io_num)
-            return (code, value if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def set_cgpio_digital(self, io_num: int, value: int) -> tuple[int, str]:
-        """Set controller GPIO digital output value (0 or 1)."""
-        try:
-            code = self.arm.set_cgpio_digital(io_num, value)
-            return (code, f"CGPIO {io_num} set to {value}" if code == 0 else f"Error code: {code}")
-        except Exception as e:
-            return (-1, str(e))
-
-    # =========================================================================
-    # Analog I/O
-    # =========================================================================
-
-    @rpc
-    def get_tgpio_analog(self, io_num: int) -> tuple[int, float | None]:
-        """Get tool GPIO analog input value."""
-        try:
-            code, value = self.arm.get_tgpio_analog(io_num)
-            return (code, value if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def get_cgpio_analog(self, io_num: int) -> tuple[int, float | None]:
-        """Get controller GPIO analog input value."""
-        try:
-            code, value = self.arm.get_cgpio_analog(io_num)
-            return (code, value if code == 0 else None)
-        except Exception:
-            return (-1, None)
-
-    @rpc
-    def set_cgpio_analog(self, io_num: int, value: float) -> tuple[int, str]:
-        """Set controller GPIO analog output value."""
-        try:
-            code = self.arm.set_cgpio_analog(io_num, value)
-            return (
-                code,
-                f"CGPIO analog {io_num} set to {value}" if code == 0 else f"Error code: {code}",
-            )
-        except Exception as e:
-            return (-1, str(e))
diff --git a/dimos/hardware/manipulators/xarm/spec.py b/dimos/hardware/manipulators/xarm/spec.py
deleted file mode 100644
index 625f036a0b..0000000000
--- a/dimos/hardware/manipulators/xarm/spec.py
+++ /dev/null
@@ -1,63 +0,0 @@
-# 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.
-
-from dataclasses import dataclass
-from typing import Protocol
-
-from dimos.core import In, Out
-from dimos.msgs.geometry_msgs import WrenchStamped
-from dimos.msgs.sensor_msgs import JointCommand, JointState
-
-
-@dataclass
-class RobotState:
-    """Custom message containing full robot state (deprecated - use RobotStateMsg)."""
-
-    state: int = 0  # Robot state (0: ready, 3: paused, 4: stopped, etc.)
-    mode: int = 0  # Control mode (0: position, 1: servo, 4: joint velocity, 5: cartesian velocity)
-    error_code: int = 0  # Error code
-    warn_code: int = 0  # Warning code
-    cmdnum: int = 0  # Command queue length
-    mt_brake: int = 0  # Motor brake state
-    mt_able: int = 0  # Motor enable state
-
-
-class ArmDriverSpec(Protocol):
-    """Protocol specification for xArm manipulator driver.
-
-    Compatible with xArm5, xArm6, and xArm7 models.
-    """
-
-    # Input topics (commands)
-    joint_position_command: In[JointCommand]  # Desired joint positions (radians)
-    joint_velocity_command: In[JointCommand]  # Desired joint velocities (rad/s)
-
-    # Output topics
-    joint_state: Out[JointState]  # Current joint positions, velocities, and efforts
-    robot_state: Out[RobotState]  # Full robot state (errors, modes, etc.)
-    ft_ext: Out[WrenchStamped]  # External force/torque (compensated)
-    ft_raw: Out[WrenchStamped]  # Raw force/torque sensor data
-
-    # RPC Methods
-    def set_joint_angles(self, angles: list[float]) -> tuple[int, str]: ...
-
-    def set_joint_velocities(self, velocities: list[float]) -> tuple[int, str]: ...
-
-    def get_joint_state(self) -> JointState: ...
-
-    def get_robot_state(self) -> RobotState: ...
-
-    def enable_servo_mode(self) -> tuple[int, str]: ...
-
-    def disable_servo_mode(self) -> tuple[int, str]: ...
diff --git a/dimos/hardware/manipulators/xarm/xarm_blueprints.py b/dimos/hardware/manipulators/xarm/xarm_blueprints.py
deleted file mode 100644
index 4e84c9c991..0000000000
--- a/dimos/hardware/manipulators/xarm/xarm_blueprints.py
+++ /dev/null
@@ -1,260 +0,0 @@
-# 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.
-
-"""
-Blueprints for xArm manipulator control using component-based architecture.
-
-This module provides declarative blueprints for configuring xArm with the new
-generalized component-based driver architecture.
-
-Usage:
-    # Run via CLI:
-    dimos run xarm-servo           # Driver only
-    dimos run xarm-trajectory      # Driver + Joint trajectory controller
-    dimos run xarm-cartesian       # Driver + Cartesian motion controller
-
-    # Or programmatically:
-    from dimos.hardware.manipulators.xarm.xarm_blueprints import xarm_trajectory
-    coordinator = xarm_trajectory.build()
-    coordinator.loop()
-"""
-
-from typing import Any
-
-from dimos.core.blueprints import autoconnect
-from dimos.core.transport import LCMTransport
-from dimos.hardware.manipulators.xarm.xarm_driver import xarm_driver as xarm_driver_blueprint
-from dimos.manipulation.control import cartesian_motion_controller, joint_trajectory_controller
-from dimos.msgs.geometry_msgs import PoseStamped
-from dimos.msgs.sensor_msgs import (
-    JointCommand,
-    JointState,
-    RobotState,
-)
-from dimos.msgs.trajectory_msgs import JointTrajectory
-
-
-# Create a blueprint wrapper for the component-based driver
-def xarm_driver(**config: Any) -> Any:
-    """Create a blueprint for XArmDriver.
-
-    Args:
-        **config: Configuration parameters passed to XArmDriver
-            - ip: IP address of XArm controller (default: "192.168.1.210")
-            - dof: Degrees of freedom - 5, 6, or 7 (default: 6)
-            - has_gripper: Whether gripper is attached (default: False)
-            - has_force_torque: Whether F/T sensor is attached (default: False)
-            - control_rate: Control loop + joint feedback rate in Hz (default: 100)
-            - monitor_rate: Robot state monitoring rate in Hz (default: 10)
-
-    Returns:
-        Blueprint configuration for XArmDriver
-    """
-    # Set defaults
-    config.setdefault("ip", "192.168.1.210")
-    config.setdefault("dof", 6)
-    config.setdefault("has_gripper", False)
-    config.setdefault("has_force_torque", False)
-    config.setdefault("control_rate", 100)
-    config.setdefault("monitor_rate", 10)
-
-    # Return the xarm_driver blueprint with the config
-    return xarm_driver_blueprint(**config)
-
-
-# =============================================================================
-# xArm6 Servo Control Blueprint
-# =============================================================================
-# XArmDriver configured for servo control mode using component-based architecture.
-# Publishes joint states and robot state, listens for joint commands.
-# =============================================================================
-
-xarm_servo = xarm_driver(
-    ip="192.168.1.210",
-    dof=6,  # XArm6
-    has_gripper=False,
-    has_force_torque=False,
-    control_rate=100,
-    monitor_rate=10,
-).transports(
-    {
-        # Joint state feedback (position, velocity, effort)
-        ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState),
-        # Robot state feedback (mode, state, errors)
-        ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState),
-        # Position commands input
-        ("joint_position_command", JointCommand): LCMTransport(
-            "/xarm/joint_position_command", JointCommand
-        ),
-        # Velocity commands input
-        ("joint_velocity_command", JointCommand): LCMTransport(
-            "/xarm/joint_velocity_command", JointCommand
-        ),
-    }
-)
-
-# =============================================================================
-# xArm7 Servo Control Blueprint
-# =============================================================================
-
-xarm7_servo = xarm_driver(
-    ip="192.168.1.210",
-    dof=7,  # XArm7
-    has_gripper=False,
-    has_force_torque=False,
-    control_rate=100,
-    monitor_rate=10,
-).transports(
-    {
-        ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState),
-        ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState),
-        ("joint_position_command", JointCommand): LCMTransport(
-            "/xarm/joint_position_command", JointCommand
-        ),
-        ("joint_velocity_command", JointCommand): LCMTransport(
-            "/xarm/joint_velocity_command", JointCommand
-        ),
-    }
-)
-
-# =============================================================================
-# xArm5 Servo Control Blueprint
-# =============================================================================
-
-xarm5_servo = xarm_driver(
-    ip="192.168.1.210",
-    dof=5,  # XArm5
-    has_gripper=False,
-    has_force_torque=False,
-    control_rate=100,
-    monitor_rate=10,
-).transports(
-    {
-        ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState),
-        ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState),
-        ("joint_position_command", JointCommand): LCMTransport(
-            "/xarm/joint_position_command", JointCommand
-        ),
-        ("joint_velocity_command", JointCommand): LCMTransport(
-            "/xarm/joint_velocity_command", JointCommand
-        ),
-    }
-)
-
-# =============================================================================
-# xArm Trajectory Control Blueprint (Driver + Trajectory Controller)
-# =============================================================================
-# Combines XArmDriver with JointTrajectoryController for trajectory execution.
-# The controller receives JointTrajectory messages and executes them at 100Hz.
-# =============================================================================
-
-xarm_trajectory = autoconnect(
-    xarm_driver(
-        ip="192.168.1.210",
-        dof=6,  # XArm6
-        has_gripper=False,
-        has_force_torque=False,
-        control_rate=500,
-        monitor_rate=10,
-    ),
-    joint_trajectory_controller(
-        control_frequency=100.0,
-    ),
-).transports(
-    {
-        # Shared topics between driver and controller
-        ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState),
-        ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState),
-        ("joint_position_command", JointCommand): LCMTransport(
-            "/xarm/joint_position_command", JointCommand
-        ),
-        # Trajectory input topic
-        ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory),
-    }
-)
-
-# =============================================================================
-# xArm7 Trajectory Control Blueprint
-# =============================================================================
-
-xarm7_trajectory = autoconnect(
-    xarm_driver(
-        ip="192.168.1.210",
-        dof=7,  # XArm7
-        has_gripper=False,
-        has_force_torque=False,
-        control_rate=100,
-        monitor_rate=10,
-    ),
-    joint_trajectory_controller(
-        control_frequency=100.0,
-    ),
-).transports(
-    {
-        ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState),
-        ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState),
-        ("joint_position_command", JointCommand): LCMTransport(
-            "/xarm/joint_position_command", JointCommand
-        ),
-        ("trajectory", JointTrajectory): LCMTransport("/trajectory", JointTrajectory),
-    }
-)
-
-# =============================================================================
-# xArm Cartesian Control Blueprint (Driver + Controller)
-# =============================================================================
-# Combines XArmDriver with CartesianMotionController for Cartesian space control.
-# The controller receives target_pose and converts to joint commands via IK.
-# =============================================================================
-
-xarm_cartesian = autoconnect(
-    xarm_driver(
-        ip="192.168.1.210",
-        dof=6,  # XArm6
-        has_gripper=False,
-        has_force_torque=False,
-        control_rate=100,
-        monitor_rate=10,
-    ),
-    cartesian_motion_controller(
-        control_frequency=20.0,
-        position_kp=5.0,
-        position_ki=0.0,
-        position_kd=0.1,
-        max_linear_velocity=0.2,
-        max_angular_velocity=1.0,
-    ),
-).transports(
-    {
-        # Shared topics between driver and controller
-        ("joint_state", JointState): LCMTransport("/xarm/joint_states", JointState),
-        ("robot_state", RobotState): LCMTransport("/xarm/robot_state", RobotState),
-        ("joint_position_command", JointCommand): LCMTransport(
-            "/xarm/joint_position_command", JointCommand
-        ),
-        # Controller-specific topics
-        ("target_pose", PoseStamped): LCMTransport("/target_pose", PoseStamped),
-        ("current_pose", PoseStamped): LCMTransport("/xarm/current_pose", PoseStamped),
-    }
-)
-
-
-__all__ = [
-    "xarm5_servo",
-    "xarm7_servo",
-    "xarm7_trajectory",
-    "xarm_cartesian",
-    "xarm_servo",
-    "xarm_trajectory",
-]
diff --git a/dimos/hardware/manipulators/xarm/xarm_driver.py b/dimos/hardware/manipulators/xarm/xarm_driver.py
deleted file mode 100644
index f6d950938c..0000000000
--- a/dimos/hardware/manipulators/xarm/xarm_driver.py
+++ /dev/null
@@ -1,174 +0,0 @@
-# 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.
-
-"""XArm driver using the generalized component-based architecture."""
-
-import logging
-from typing import Any
-
-from dimos.hardware.manipulators.base import (
-    BaseManipulatorDriver,
-    StandardMotionComponent,
-    StandardServoComponent,
-    StandardStatusComponent,
-)
-
-from .xarm_wrapper import XArmSDKWrapper
-
-logger = logging.getLogger(__name__)
-
-
-class XArmDriver(BaseManipulatorDriver):
-    """XArm driver using component-based architecture.
-
-    This driver supports XArm5, XArm6, and XArm7 models.
-    All the complex logic is handled by the base class and standard components.
-    This file just assembles the pieces.
-    """
-
-    def __init__(self, **kwargs: Any) -> None:
-        """Initialize the XArm driver.
-
-        Args:
-            **kwargs: Arguments for Module initialization.
-                Driver configuration can be passed via 'config' keyword arg:
-                - ip: IP address of the XArm controller
-                - dof: Degrees of freedom (5, 6, or 7)
-                - has_gripper: Whether gripper is attached
-                - has_force_torque: Whether F/T sensor is attached
-        """
-        # Extract driver-specific config from kwargs
-        config: dict[str, Any] = kwargs.pop("config", {})
-
-        # Extract driver-specific params that might be passed directly
-        driver_params = [
-            "ip",
-            "dof",
-            "has_gripper",
-            "has_force_torque",
-            "control_rate",
-            "monitor_rate",
-        ]
-        for param in driver_params:
-            if param in kwargs:
-                config[param] = kwargs.pop(param)
-
-        logger.info(f"Initializing XArmDriver with config: {config}")
-
-        # Create SDK wrapper
-        sdk = XArmSDKWrapper()
-
-        # Create standard components
-        components = [
-            StandardMotionComponent(sdk),
-            StandardServoComponent(sdk),
-            StandardStatusComponent(sdk),
-        ]
-
-        # Optional: Add gripper component if configured
-        # if config.get('has_gripper', False):
-        #     from dimos.hardware.manipulators.base.components import StandardGripperComponent
-        #     components.append(StandardGripperComponent(sdk))
-
-        # Optional: Add force/torque component if configured
-        # if config.get('has_force_torque', False):
-        #     from dimos.hardware.manipulators.base.components import StandardForceTorqueComponent
-        #     components.append(StandardForceTorqueComponent(sdk))
-
-        # Remove any kwargs that would conflict with explicit arguments
-        kwargs.pop("sdk", None)
-        kwargs.pop("components", None)
-        kwargs.pop("name", None)
-
-        # Initialize base driver with SDK and components
-        super().__init__(sdk=sdk, components=components, config=config, name="XArmDriver", **kwargs)
-
-        logger.info("XArmDriver initialized successfully")
-
-
-# Blueprint configuration for the driver
-def get_blueprint() -> dict[str, Any]:
-    """Get the blueprint configuration for the XArm driver.
-
-    Returns:
-        Dictionary with blueprint configuration
-    """
-    return {
-        "name": "XArmDriver",
-        "class": XArmDriver,
-        "config": {
-            "ip": "192.168.1.210",  # Default IP
-            "dof": 7,  # Default to 7-DOF
-            "has_gripper": False,
-            "has_force_torque": False,
-            "control_rate": 100,  # Hz - control loop + joint feedback
-            "monitor_rate": 10,  # Hz - robot state monitoring
-        },
-        "inputs": {
-            "joint_position_command": "JointCommand",
-            "joint_velocity_command": "JointCommand",
-        },
-        "outputs": {
-            "joint_state": "JointState",
-            "robot_state": "RobotState",
-        },
-        "rpc_methods": [
-            # Motion control
-            "move_joint",
-            "move_joint_velocity",
-            "move_joint_effort",
-            "stop_motion",
-            "get_joint_state",
-            "get_joint_limits",
-            "get_velocity_limits",
-            "set_velocity_scale",
-            "set_acceleration_scale",
-            "move_cartesian",
-            "get_cartesian_state",
-            "execute_trajectory",
-            "stop_trajectory",
-            # Servo control
-            "enable_servo",
-            "disable_servo",
-            "toggle_servo",
-            "get_servo_state",
-            "emergency_stop",
-            "reset_emergency_stop",
-            "set_control_mode",
-            "get_control_mode",
-            "clear_errors",
-            "reset_fault",
-            "home_robot",
-            "brake_release",
-            "brake_engage",
-            # Status monitoring
-            "get_robot_state",
-            "get_system_info",
-            "get_capabilities",
-            "get_error_state",
-            "get_health_metrics",
-            "get_statistics",
-            "check_connection",
-            "get_force_torque",
-            "zero_force_torque",
-            "get_digital_inputs",
-            "set_digital_outputs",
-            "get_analog_inputs",
-            "get_gripper_state",
-        ],
-    }
-
-
-# Expose blueprint for declarative composition (compatible with dimos framework)
-xarm_driver = XArmDriver.blueprint
diff --git a/dimos/hardware/manipulators/xarm/xarm_wrapper.py b/dimos/hardware/manipulators/xarm/xarm_wrapper.py
deleted file mode 100644
index a743c0e3c7..0000000000
--- a/dimos/hardware/manipulators/xarm/xarm_wrapper.py
+++ /dev/null
@@ -1,564 +0,0 @@
-# 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.
-
-"""XArm SDK wrapper implementation."""
-
-import logging
-import math
-from typing import Any
-
-from ..base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo
-
-
-class XArmSDKWrapper(BaseManipulatorSDK):
-    """SDK wrapper for XArm manipulators.
-
-    This wrapper translates XArm's native SDK (which uses degrees and mm)
-    to our standard interface (radians and meters).
-    """
-
-    def __init__(self) -> None:
-        """Initialize the XArm SDK wrapper."""
-        self.logger = logging.getLogger(self.__class__.__name__)
-        self.native_sdk: Any = None
-        self.dof = 7  # Default, will be updated on connect
-        self._connected = False
-
-    # ============= Connection Management =============
-
-    def connect(self, config: dict[str, Any]) -> bool:
-        """Connect to XArm controller.
-
-        Args:
-            config: Configuration with 'ip' and optionally 'dof' (5, 6, or 7)
-
-        Returns:
-            True if connection successful
-        """
-        try:
-            from xarm import XArmAPI
-
-            ip = config.get("ip", "192.168.1.100")
-            self.dof = config.get("dof", 7)
-
-            self.logger.info(f"Connecting to XArm at {ip} (DOF: {self.dof})...")
-
-            # Create XArm API instance
-            # XArm SDK uses degrees by default, we'll convert to radians
-            self.native_sdk = XArmAPI(ip, is_radian=False)
-
-            # Check connection
-            if self.native_sdk.connected:
-                # Initialize XArm
-                self.native_sdk.motion_enable(True)
-                self.native_sdk.set_mode(1)  # Servo mode for high-frequency control
-                self.native_sdk.set_state(0)  # Ready state
-
-                self._connected = True
-                self.logger.info(
-                    f"Successfully connected to XArm (version: {self.native_sdk.version})"
-                )
-                return True
-            else:
-                self.logger.error("Failed to connect to XArm")
-                return False
-
-        except ImportError:
-            self.logger.error("XArm SDK not installed. Please install: pip install xArm-Python-SDK")
-            return False
-        except Exception as e:
-            self.logger.error(f"Connection failed: {e}")
-            return False
-
-    def disconnect(self) -> None:
-        """Disconnect from XArm controller."""
-        if self.native_sdk:
-            try:
-                self.native_sdk.disconnect()
-                self._connected = False
-                self.logger.info("Disconnected from XArm")
-            except:
-                pass
-            finally:
-                self.native_sdk = None
-
-    def is_connected(self) -> bool:
-        """Check if connected to XArm.
-
-        Returns:
-            True if connected
-        """
-        return self._connected and self.native_sdk and self.native_sdk.connected
-
-    # ============= Joint State Query =============
-
-    def get_joint_positions(self) -> list[float]:
-        """Get current joint positions.
-
-        Returns:
-            Joint positions in RADIANS
-        """
-        code, angles = self.native_sdk.get_servo_angle()
-        if code != 0:
-            raise RuntimeError(f"XArm error getting positions: {code}")
-
-        # Convert degrees to radians
-        positions = [math.radians(angle) for angle in angles[: self.dof]]
-        return positions
-
-    def get_joint_velocities(self) -> list[float]:
-        """Get current joint velocities.
-
-        Returns:
-            Joint velocities in RAD/S
-        """
-        # XArm doesn't directly provide velocities in older versions
-        # Try to get from realtime data if available
-        if hasattr(self.native_sdk, "get_joint_speeds"):
-            code, speeds = self.native_sdk.get_joint_speeds()
-            if code == 0:
-                # Convert deg/s to rad/s
-                return [math.radians(speed) for speed in speeds[: self.dof]]
-
-        # Return zeros if not available
-        return [0.0] * self.dof
-
-    def get_joint_efforts(self) -> list[float]:
-        """Get current joint efforts/torques.
-
-        Returns:
-            Joint efforts in Nm
-        """
-        # Try to get joint torques
-        if hasattr(self.native_sdk, "get_joint_torques"):
-            code, torques = self.native_sdk.get_joint_torques()
-            if code == 0:
-                return list(torques[: self.dof])
-
-        # Return zeros if not available
-        return [0.0] * self.dof
-
-    # ============= Joint Motion Control =============
-
-    def set_joint_positions(
-        self,
-        positions: list[float],
-        _velocity: float = 1.0,
-        _acceleration: float = 1.0,
-        _wait: bool = False,
-    ) -> bool:
-        """Move joints to target positions using servo mode.
-
-        Args:
-            positions: Target positions in RADIANS
-            _velocity: UNUSED in servo mode (kept for interface compatibility)
-            _acceleration: UNUSED in servo mode (kept for interface compatibility)
-            _wait: UNUSED in servo mode (kept for interface compatibility)
-
-        Returns:
-            True if command accepted
-        """
-        # Convert radians to degrees
-        degrees = [math.degrees(pos) for pos in positions]
-
-        # Use set_servo_angle_j for high-frequency servo control (100Hz+)
-        # This sends immediate position commands without trajectory planning
-        # Requires mode 1 (servo mode) and executes only the last instruction
-        code = self.native_sdk.set_servo_angle_j(degrees, speed=100, mvacc=500, wait=False)
-
-        return bool(code == 0)
-
-    def set_joint_velocities(self, velocities: list[float]) -> bool:
-        """Set joint velocity targets.
-
-        Args:
-            velocities: Target velocities in RAD/S
-
-        Returns:
-            True if command accepted
-        """
-        # Check if velocity control is supported
-        if not hasattr(self.native_sdk, "vc_set_joint_velocity"):
-            self.logger.warning("Velocity control not supported in this XArm version")
-            return False
-
-        # Convert rad/s to deg/s
-        deg_velocities = [math.degrees(vel) for vel in velocities]
-
-        # Set to velocity control mode if needed
-        if self.native_sdk.mode != 4:
-            self.native_sdk.set_mode(4)  # Joint velocity mode
-
-        # Send velocity command
-        code = self.native_sdk.vc_set_joint_velocity(deg_velocities)
-        return bool(code == 0)
-
-    def set_joint_efforts(self, efforts: list[float]) -> bool:
-        """Set joint effort/torque targets.
-
-        Args:
-            efforts: Target efforts in Nm
-
-        Returns:
-            True if command accepted
-        """
-        # Check if torque control is supported
-        if not hasattr(self.native_sdk, "set_joint_torque"):
-            self.logger.warning("Torque control not supported in this XArm version")
-            return False
-
-        # Send torque command
-        code = self.native_sdk.set_joint_torque(efforts)
-        return bool(code == 0)
-
-    def stop_motion(self) -> bool:
-        """Stop all ongoing motion.
-
-        Returns:
-            True if stop successful
-        """
-        # XArm emergency stop
-        code = self.native_sdk.emergency_stop()
-
-        # Re-enable after stop
-        if code == 0:
-            self.native_sdk.set_state(0)  # Clear stop state
-            self.native_sdk.motion_enable(True)
-
-        return bool(code == 0)
-
-    # ============= Servo Control =============
-
-    def enable_servos(self) -> bool:
-        """Enable motor control.
-
-        Returns:
-            True if servos enabled
-        """
-        code1 = self.native_sdk.motion_enable(True)
-        code2 = self.native_sdk.set_state(0)  # Ready state
-        code3 = self.native_sdk.set_mode(1)  # Servo mode
-        return bool(code1 == 0 and code2 == 0 and code3 == 0)
-
-    def disable_servos(self) -> bool:
-        """Disable motor control.
-
-        Returns:
-            True if servos disabled
-        """
-        code = self.native_sdk.motion_enable(False)
-        return bool(code == 0)
-
-    def are_servos_enabled(self) -> bool:
-        """Check if servos are enabled.
-
-        Returns:
-            True if enabled
-        """
-        # Check motor state
-        return bool(self.native_sdk.mode == 1 and self.native_sdk.mode != 4)
-
-    # ============= System State =============
-
-    def get_robot_state(self) -> dict[str, Any]:
-        """Get current robot state.
-
-        Returns:
-            State dictionary
-        """
-        return {
-            "state": self.native_sdk.state,  # 0=ready, 1=pause, 2=stop, 3=running, 4=error
-            "mode": self.native_sdk.mode,  # 0=position, 1=servo, 4=joint_vel, 5=cart_vel
-            "error_code": self.native_sdk.error_code,
-            "warn_code": self.native_sdk.warn_code,
-            "is_moving": self.native_sdk.state == 3,
-            "cmd_num": self.native_sdk.cmd_num,
-        }
-
-    def get_error_code(self) -> int:
-        """Get current error code.
-
-        Returns:
-            Error code (0 = no error)
-        """
-        return int(self.native_sdk.error_code)
-
-    def get_error_message(self) -> str:
-        """Get human-readable error message.
-
-        Returns:
-            Error message string
-        """
-        if self.native_sdk.error_code == 0:
-            return ""
-
-        # XArm error codes (partial list)
-        error_map = {
-            1: "Emergency stop button pressed",
-            2: "Joint limit exceeded",
-            3: "Command reply timeout",
-            4: "Power supply error",
-            5: "Motor overheated",
-            6: "Motor driver error",
-            7: "Other error",
-            10: "Servo error",
-            11: "Joint collision",
-            12: "Tool IO error",
-            13: "Tool communication error",
-            14: "Kinematic error",
-            15: "Self collision",
-            16: "Joint overheated",
-            17: "Planning error",
-            19: "Force control error",
-            20: "Joint current overlimit",
-            21: "TCP command overlimit",
-            22: "Overspeed",
-        }
-
-        return error_map.get(
-            self.native_sdk.error_code, f"Unknown error {self.native_sdk.error_code}"
-        )
-
-    def clear_errors(self) -> bool:
-        """Clear error states.
-
-        Returns:
-            True if errors cleared
-        """
-        code = self.native_sdk.clean_error()
-        if code == 0:
-            # Reset to ready state
-            self.native_sdk.set_state(0)
-        return bool(code == 0)
-
-    def emergency_stop(self) -> bool:
-        """Execute emergency stop.
-
-        Returns:
-            True if e-stop executed
-        """
-        code = self.native_sdk.emergency_stop()
-        return bool(code == 0)
-
-    # ============= Information =============
-
-    def get_info(self) -> ManipulatorInfo:
-        """Get manipulator information.
-
-        Returns:
-            ManipulatorInfo object
-        """
-        return ManipulatorInfo(
-            vendor="UFACTORY",
-            model=f"xArm{self.dof}",
-            dof=self.dof,
-            firmware_version=self.native_sdk.version if self.native_sdk else None,
-            serial_number=self.native_sdk.get_servo_version()[1][0] if self.native_sdk else None,
-        )
-
-    def get_joint_limits(self) -> tuple[list[float], list[float]]:
-        """Get joint position limits.
-
-        Returns:
-            Tuple of (lower_limits, upper_limits) in RADIANS
-        """
-        # XArm joint limits in degrees (approximate, varies by model)
-        if self.dof == 7:
-            lower_deg = [-360, -118, -360, -233, -360, -97, -360]
-            upper_deg = [360, 118, 360, 11, 360, 180, 360]
-        elif self.dof == 6:
-            lower_deg = [-360, -118, -225, -11, -360, -97]
-            upper_deg = [360, 118, 11, 225, 360, 180]
-        else:  # 5 DOF
-            lower_deg = [-360, -118, -225, -97, -360]
-            upper_deg = [360, 118, 11, 180, 360]
-
-        # Convert to radians
-        lower_rad = [math.radians(d) for d in lower_deg[: self.dof]]
-        upper_rad = [math.radians(d) for d in upper_deg[: self.dof]]
-
-        return (lower_rad, upper_rad)
-
-    def get_velocity_limits(self) -> list[float]:
-        """Get joint velocity limits.
-
-        Returns:
-            Maximum velocities in RAD/S
-        """
-        # XArm max velocities in deg/s (default)
-        max_vel_deg = 180.0
-
-        # Convert to rad/s
-        max_vel_rad = math.radians(max_vel_deg)
-        return [max_vel_rad] * self.dof
-
-    def get_acceleration_limits(self) -> list[float]:
-        """Get joint acceleration limits.
-
-        Returns:
-            Maximum accelerations in RAD/S²
-        """
-        # XArm max acceleration in deg/s² (default)
-        max_acc_deg = 1145.0
-
-        # Convert to rad/s²
-        max_acc_rad = math.radians(max_acc_deg)
-        return [max_acc_rad] * self.dof
-
-    # ============= Optional Methods =============
-
-    def get_cartesian_position(self) -> dict[str, float] | None:
-        """Get current end-effector pose.
-
-        Returns:
-            Pose dict or None if not supported
-        """
-        code, pose = self.native_sdk.get_position()
-        if code != 0:
-            return None
-
-        # XArm returns [x, y, z (mm), roll, pitch, yaw (degrees)]
-        return {
-            "x": pose[0] / 1000.0,  # mm to meters
-            "y": pose[1] / 1000.0,
-            "z": pose[2] / 1000.0,
-            "roll": math.radians(pose[3]),
-            "pitch": math.radians(pose[4]),
-            "yaw": math.radians(pose[5]),
-        }
-
-    def set_cartesian_position(
-        self,
-        pose: dict[str, float],
-        velocity: float = 1.0,
-        acceleration: float = 1.0,
-        wait: bool = False,
-    ) -> bool:
-        """Move end-effector to target pose.
-
-        Args:
-            pose: Target pose dict
-            velocity: Max velocity fraction (0-1)
-            acceleration: Max acceleration fraction (0-1)
-            wait: Block until complete
-
-        Returns:
-            True if command accepted
-        """
-        # Convert to XArm format
-        xarm_pose = [
-            pose["x"] * 1000.0,  # meters to mm
-            pose["y"] * 1000.0,
-            pose["z"] * 1000.0,
-            math.degrees(pose["roll"]),
-            math.degrees(pose["pitch"]),
-            math.degrees(pose["yaw"]),
-        ]
-
-        # XArm max Cartesian speed (default 500 mm/s)
-        max_speed = 500.0
-        speed = max_speed * velocity
-
-        # XArm max Cartesian acceleration (default 2000 mm/s²)
-        max_acc = 2000.0
-        acc = max_acc * acceleration
-
-        code = self.native_sdk.set_position(xarm_pose, radius=-1, speed=speed, mvacc=acc, wait=wait)
-
-        return bool(code == 0)
-
-    def get_force_torque(self) -> list[float] | None:
-        """Get F/T sensor reading.
-
-        Returns:
-            [fx, fy, fz, tx, ty, tz] or None
-        """
-        if hasattr(self.native_sdk, "get_ft_sensor_data"):
-            code, ft_data = self.native_sdk.get_ft_sensor_data()
-            if code == 0:
-                return list(ft_data)
-        return None
-
-    def zero_force_torque(self) -> bool:
-        """Zero the F/T sensor.
-
-        Returns:
-            True if successful
-        """
-        if hasattr(self.native_sdk, "set_ft_sensor_zero"):
-            code = self.native_sdk.set_ft_sensor_zero()
-            return bool(code == 0)
-        return False
-
-    def get_gripper_position(self) -> float | None:
-        """Get gripper position.
-
-        Returns:
-            Position in meters or None
-        """
-        if hasattr(self.native_sdk, "get_gripper_position"):
-            code, pos = self.native_sdk.get_gripper_position()
-            if code == 0:
-                # Convert mm to meters
-                return float(pos / 1000.0)
-        return None
-
-    def set_gripper_position(self, position: float, force: float = 1.0) -> bool:
-        """Set gripper position.
-
-        Args:
-            position: Target position in meters
-            force: Force fraction (0-1)
-
-        Returns:
-            True if successful
-        """
-        if hasattr(self.native_sdk, "set_gripper_position"):
-            # Convert meters to mm
-            pos_mm = position * 1000.0
-            code = self.native_sdk.set_gripper_position(pos_mm, wait=False)
-            return bool(code == 0)
-        return False
-
-    def set_control_mode(self, mode: str) -> bool:
-        """Set control mode.
-
-        Args:
-            mode: 'position', 'velocity', 'torque', or 'impedance'
-
-        Returns:
-            True if successful
-        """
-        mode_map = {
-            "position": 0,
-            "velocity": 4,  # Joint velocity mode
-            "servo": 1,  # Servo mode (for torque control)
-            "impedance": 0,  # Not directly supported, use position
-        }
-
-        if mode not in mode_map:
-            return False
-
-        code = self.native_sdk.set_mode(mode_map[mode])
-        return bool(code == 0)
-
-    def get_control_mode(self) -> str | None:
-        """Get current control mode.
-
-        Returns:
-            Mode string or None
-        """
-        mode_map = {0: "position", 1: "servo", 4: "velocity", 5: "cartesian_velocity"}
-
-        return mode_map.get(self.native_sdk.mode, "unknown")
diff --git a/dimos/manipulation/control/dual_trajectory_setter.py b/dimos/manipulation/control/dual_trajectory_setter.py
new file mode 100644
index 0000000000..4b54f0e3e5
--- /dev/null
+++ b/dimos/manipulation/control/dual_trajectory_setter.py
@@ -0,0 +1,540 @@
+#!/usr/bin/env python3
+# 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.
+
+"""
+Dual-Arm Interactive Trajectory Publisher.
+
+Interactive terminal UI for creating joint trajectories for two arms independently.
+Supports running trajectories on left arm, right arm, or both simultaneously.
+
+Workflow:
+1. Add waypoints to left or right arm (or both)
+2. Generator applies trapezoidal velocity profiles
+3. Preview the generated trajectories
+4. Run on left, right, or both arms
+
+Use with xarm-dual-trajectory blueprint running in another terminal.
+"""
+
+from dataclasses import dataclass
+import math
+import sys
+import time
+
+from dimos import core
+from dimos.manipulation.planning import JointTrajectoryGenerator
+from dimos.msgs.sensor_msgs import JointState
+from dimos.msgs.trajectory_msgs import JointTrajectory
+
+
+@dataclass
+class ArmState:
+    """State for a single arm."""
+
+    name: str
+    num_joints: int | None = None
+    latest_joint_state: JointState | None = None
+    generator: JointTrajectoryGenerator | None = None
+    waypoints: list[list[float]] | None = None
+    generated_trajectory: JointTrajectory | None = None
+
+    def __post_init__(self) -> None:
+        self.waypoints = []
+
+
+class DualTrajectorySetter:
+    """
+    Creates and publishes JointTrajectory for dual-arm setups.
+
+    Manages two arms independently with separate waypoints and trajectories.
+    Supports running trajectories on one or both arms.
+    """
+
+    def __init__(
+        self,
+        left_joint_topic: str = "/xarm/left/joint_states",
+        right_joint_topic: str = "/xarm/right/joint_states",
+        left_trajectory_topic: str = "/xarm/left/trajectory",
+        right_trajectory_topic: str = "/xarm/right/trajectory",
+    ):
+        """
+        Initialize the dual trajectory setter.
+
+        Args:
+            left_joint_topic: Topic for left arm joint states
+            right_joint_topic: Topic for right arm joint states
+            left_trajectory_topic: Topic to publish left arm trajectories
+            right_trajectory_topic: Topic to publish right arm trajectories
+        """
+        # Arm states
+        self.left = ArmState(name="left")
+        self.right = ArmState(name="right")
+
+        # Publishers for trajectories
+        self.left_trajectory_pub: core.LCMTransport[JointTrajectory] = core.LCMTransport(
+            left_trajectory_topic, JointTrajectory
+        )
+        self.right_trajectory_pub: core.LCMTransport[JointTrajectory] = core.LCMTransport(
+            right_trajectory_topic, JointTrajectory
+        )
+
+        # Subscribers for joint states
+        self.left_joint_sub: core.LCMTransport[JointState] = core.LCMTransport(
+            left_joint_topic, JointState
+        )
+        self.right_joint_sub: core.LCMTransport[JointState] = core.LCMTransport(
+            right_joint_topic, JointState
+        )
+
+        print("DualTrajectorySetter initialized")
+        print(f"  Left arm:  {left_joint_topic} -> {left_trajectory_topic}")
+        print(f"  Right arm: {right_joint_topic} -> {right_trajectory_topic}")
+
+    def start(self) -> bool:
+        """Start subscribing to joint states."""
+        self.left_joint_sub.subscribe(self._on_left_joint_state)
+        self.right_joint_sub.subscribe(self._on_right_joint_state)
+        print("  Waiting for joint states...")
+
+        # Wait for both arms
+        left_ready = False
+        right_ready = False
+
+        for _ in range(50):  # 5 second timeout
+            if not left_ready and self.left.latest_joint_state is not None:
+                self.left.num_joints = len(self.left.latest_joint_state.position)
+                self.left.generator = JointTrajectoryGenerator(
+                    num_joints=self.left.num_joints,
+                    max_velocity=1.0,
+                    max_acceleration=2.0,
+                    points_per_segment=50,
+                )
+                print(f"  Left arm ready ({self.left.num_joints} joints)")
+                left_ready = True
+
+            if not right_ready and self.right.latest_joint_state is not None:
+                self.right.num_joints = len(self.right.latest_joint_state.position)
+                self.right.generator = JointTrajectoryGenerator(
+                    num_joints=self.right.num_joints,
+                    max_velocity=1.0,
+                    max_acceleration=2.0,
+                    points_per_segment=50,
+                )
+                print(f"  Right arm ready ({self.right.num_joints} joints)")
+                right_ready = True
+
+            if left_ready and right_ready:
+                return True
+
+            time.sleep(0.1)
+
+        if not left_ready:
+            print("  Warning: Left arm not responding")
+        if not right_ready:
+            print("  Warning: Right arm not responding")
+
+        return left_ready or right_ready
+
+    def _on_left_joint_state(self, msg: JointState) -> None:
+        """Callback for left arm joint state."""
+        self.left.latest_joint_state = msg
+
+    def _on_right_joint_state(self, msg: JointState) -> None:
+        """Callback for right arm joint state."""
+        self.right.latest_joint_state = msg
+
+    def get_current_joints(self, arm: ArmState) -> list[float] | None:
+        """Get current joint positions for an arm."""
+        if arm.latest_joint_state is None or arm.num_joints is None:
+            return None
+        return list(arm.latest_joint_state.position[: arm.num_joints])
+
+    def generate_trajectory(self, arm: ArmState) -> JointTrajectory | None:
+        """Generate trajectory for an arm from its waypoints."""
+        if arm.generator is None or not arm.waypoints or len(arm.waypoints) < 2:
+            return None
+        return arm.generator.generate(arm.waypoints)
+
+    def publish_trajectory(self, arm: ArmState, trajectory: JointTrajectory) -> None:
+        """Publish trajectory to an arm."""
+        if arm.name == "left":
+            self.left_trajectory_pub.broadcast(None, trajectory)
+        else:
+            self.right_trajectory_pub.broadcast(None, trajectory)
+        print(
+            f"  Published to {arm.name}: {len(trajectory.points)} points, "
+            f"duration={trajectory.duration:.2f}s"
+        )
+
+
+def parse_joint_input(line: str, num_joints: int) -> list[float] | None:
+    """Parse joint positions from user input (degrees by default, 'r' suffix for radians)."""
+    parts = line.strip().split()
+    if len(parts) != num_joints:
+        return None
+
+    positions = []
+    for part in parts:
+        try:
+            if part.endswith("r"):
+                positions.append(float(part[:-1]))
+            else:
+                positions.append(math.radians(float(part)))
+        except ValueError:
+            return None
+
+    return positions
+
+
+def preview_waypoints(arm: ArmState) -> None:
+    """Show waypoints for an arm."""
+    if not arm.waypoints or arm.num_joints is None:
+        print(f"  {arm.name.upper()}: No waypoints")
+        return
+
+    joint_headers = " ".join([f"{'J' + str(i + 1):>7}" for i in range(arm.num_joints)])
+    line_width = 6 + 3 + arm.num_joints * 8 + 10
+
+    print(f"\n{arm.name.upper()} Waypoints ({len(arm.waypoints)}):")
+    print("-" * line_width)
+    print(f"  # | {joint_headers} (degrees)")
+    print("-" * line_width)
+    for i, joints in enumerate(arm.waypoints):
+        deg = [f"{math.degrees(j):7.1f}" for j in joints]
+        print(f" {i + 1:2} | {' '.join(deg)}")
+    print("-" * line_width)
+
+
+def preview_trajectory(arm: ArmState) -> None:
+    """Show generated trajectory for an arm."""
+    if arm.generated_trajectory is None or arm.num_joints is None:
+        print(f"  {arm.name.upper()}: No trajectory")
+        return
+
+    traj = arm.generated_trajectory
+    joint_headers = " ".join([f"{'J' + str(i + 1):>7}" for i in range(arm.num_joints)])
+    line_width = 9 + 3 + arm.num_joints * 8 + 10
+
+    print(f"\n{'=' * line_width}")
+    print(f"{arm.name.upper()} TRAJECTORY")
+    print(f"{'=' * line_width}")
+    print(f"Duration: {traj.duration:.3f}s | Points: {len(traj.points)}")
+    print("-" * line_width)
+    print(f"{'Time':>6} | {joint_headers} (degrees)")
+    print("-" * line_width)
+
+    num_samples = min(10, max(len(traj.points) // 10, 5))
+    for i in range(num_samples + 1):
+        t = (i / num_samples) * traj.duration
+        q_ref, _ = traj.sample(t)
+        q_deg = [f"{math.degrees(q):7.1f}" for q in q_ref]
+        print(f"{t:6.2f} | {' '.join(q_deg)}")
+
+    print("-" * line_width)
+
+
+def interactive_mode(setter: DualTrajectorySetter) -> None:
+    """Interactive mode for creating dual-arm trajectories."""
+    left = setter.left
+    right = setter.right
+
+    print("\n" + "=" * 80)
+    print("Dual-Arm Interactive Trajectory Setter")
+    print("=" * 80)
+
+    if left.num_joints:
+        print(f"  Left arm:  {left.num_joints} joints")
+    else:
+        print("  Left arm:  NOT CONNECTED")
+
+    if right.num_joints:
+        print(f"  Right arm: {right.num_joints} joints")
+    else:
+        print("  Right arm: NOT CONNECTED")
+
+    print("\nCommands:")
+    print("  left add <j1> <j2> ...   - Add waypoint to left arm (degrees)")
+    print("  right add <j1> <j2> ...  - Add waypoint to right arm (degrees)")
+    print("  left here                - Add current position as waypoint (left)")
+    print("  right here               - Add current position as waypoint (right)")
+    print("  left current             - Show current left arm joints")
+    print("  right current            - Show current right arm joints")
+    print("  left list                - List left arm waypoints")
+    print("  right list               - List right arm waypoints")
+    print("  left delete <n>          - Delete waypoint n from left")
+    print("  right delete <n>         - Delete waypoint n from right")
+    print("  left clear               - Clear left arm waypoints")
+    print("  right clear              - Clear right arm waypoints")
+    print("  preview                  - Preview both trajectories")
+    print("  run left                 - Run trajectory on left arm only")
+    print("  run right                - Run trajectory on right arm only")
+    print("  run both                 - Run trajectories on both arms")
+    print("  vel <arm> <value>        - Set max velocity (rad/s)")
+    print("  quit                     - Exit")
+    print("=" * 80)
+
+    try:
+        while True:
+            left_wp = len(left.waypoints) if left.waypoints else 0
+            right_wp = len(right.waypoints) if right.waypoints else 0
+            prompt = f"[L:{left_wp} R:{right_wp}] > "
+            line = input(prompt).strip()
+
+            if not line:
+                continue
+
+            parts = line.split()
+            cmd = parts[0].lower()
+
+            # Determine which arm (if applicable)
+            arm: ArmState | None = None
+            if cmd in ("left", "l"):
+                arm = left
+                parts = parts[1:]  # Remove arm selector
+                cmd = parts[0].lower() if parts else ""
+            elif cmd in ("right", "r"):
+                arm = right
+                parts = parts[1:]
+                cmd = parts[0].lower() if parts else ""
+
+            # ARM-SPECIFIC COMMANDS
+            if arm is not None:
+                if arm.num_joints is None:
+                    print(f"  {arm.name.upper()} arm not connected")
+                    continue
+
+                # ADD waypoint
+                if cmd == "add" and len(parts) >= arm.num_joints + 1:
+                    joints = parse_joint_input(
+                        " ".join(parts[1 : arm.num_joints + 1]), arm.num_joints
+                    )
+                    if joints:
+                        arm.waypoints.append(joints)  # type: ignore[union-attr]
+                        arm.generated_trajectory = None
+                        deg = [f"{math.degrees(j):.1f}" for j in joints]
+                        print(
+                            f"  {arm.name.upper()} waypoint {len(arm.waypoints)}: [{', '.join(deg)}] deg"  # type: ignore[arg-type]
+                        )
+                    else:
+                        print(f"  Invalid values (need {arm.num_joints} in degrees)")
+
+                # HERE - add current position
+                elif cmd == "here":
+                    joints = setter.get_current_joints(arm)
+                    if joints:
+                        arm.waypoints.append(joints)  # type: ignore[union-attr]
+                        arm.generated_trajectory = None
+                        deg = [f"{math.degrees(j):.1f}" for j in joints]
+                        print(
+                            f"  {arm.name.upper()} waypoint {len(arm.waypoints)}: [{', '.join(deg)}] deg"  # type: ignore[arg-type]
+                        )
+                    else:
+                        print("  No joint state available")
+
+                # CURRENT
+                elif cmd == "current":
+                    joints = setter.get_current_joints(arm)
+                    if joints:
+                        deg = [f"{math.degrees(j):.1f}" for j in joints]
+                        print(f"  {arm.name.upper()}: [{', '.join(deg)}] deg")
+                    else:
+                        print("  No joint state available")
+
+                # LIST
+                elif cmd == "list":
+                    preview_waypoints(arm)
+
+                # DELETE
+                elif cmd == "delete" and len(parts) >= 2:
+                    try:
+                        idx = int(parts[1]) - 1
+                        if arm.waypoints and 0 <= idx < len(arm.waypoints):
+                            arm.waypoints.pop(idx)
+                            arm.generated_trajectory = None
+                            print(f"  Deleted {arm.name} waypoint {idx + 1}")
+                        else:
+                            wp_count = len(arm.waypoints) if arm.waypoints else 0
+                            print(f"  Invalid index (1-{wp_count})")
+                    except ValueError:
+                        print("  Invalid index")
+
+                # CLEAR
+                elif cmd == "clear":
+                    if arm.waypoints:
+                        arm.waypoints.clear()
+                    arm.generated_trajectory = None
+                    print(f"  {arm.name.upper()} waypoints cleared")
+
+                else:
+                    print(f"  Unknown command for {arm.name}: {cmd}")
+
+            # GLOBAL COMMANDS
+            elif cmd == "preview":
+                # Generate trajectories if needed
+                for a in [left, right]:
+                    if a.waypoints and len(a.waypoints) >= 2:
+                        try:
+                            a.generated_trajectory = setter.generate_trajectory(a)
+                        except Exception as e:
+                            print(f"  Error generating {a.name} trajectory: {e}")
+                            a.generated_trajectory = None
+
+                preview_trajectory(left)
+                preview_trajectory(right)
+
+            elif cmd == "run" and len(parts) >= 2:
+                target = parts[1].lower()
+
+                # Determine which arms to run
+                arms_to_run: list[ArmState] = []
+                if target in ("left", "l"):
+                    arms_to_run = [left]
+                elif target in ("right", "r"):
+                    arms_to_run = [right]
+                elif target == "both":
+                    arms_to_run = [left, right]
+                else:
+                    print("  Usage: run left|right|both")
+                    continue
+
+                # Generate trajectories if needed
+                for a in arms_to_run:
+                    if not a.waypoints or len(a.waypoints) < 2:
+                        print(f"  {a.name.upper()}: Need at least 2 waypoints")
+                        continue
+
+                    if a.generated_trajectory is None:
+                        try:
+                            a.generated_trajectory = setter.generate_trajectory(a)
+                        except Exception as e:
+                            print(f"  Error generating {a.name} trajectory: {e}")
+                            continue
+
+                # Preview and confirm
+                valid_arms = [a for a in arms_to_run if a.generated_trajectory is not None]
+                if not valid_arms:
+                    print("  No valid trajectories to run")
+                    continue
+
+                for a in valid_arms:
+                    preview_trajectory(a)
+
+                arm_names = ", ".join(a.name.upper() for a in valid_arms)
+                confirm = input(f"\n  Run on {arm_names}? [y/N]: ").strip().lower()
+                if confirm == "y":
+                    print("\n  Publishing trajectories...")
+                    for a in valid_arms:
+                        if a.generated_trajectory:
+                            setter.publish_trajectory(a, a.generated_trajectory)
+
+            elif cmd == "vel" and len(parts) >= 3:
+                arm_name = parts[1].lower()
+                target_arm: ArmState | None = (
+                    left
+                    if arm_name in ("left", "l")
+                    else right
+                    if arm_name in ("right", "r")
+                    else None
+                )
+                if target_arm is None or target_arm.generator is None:
+                    print("  Usage: vel left|right <value>")
+                    continue
+                try:
+                    vel = float(parts[2])
+                    if vel <= 0:
+                        print("  Velocity must be positive")
+                    else:
+                        target_arm.generator.set_limits(vel, target_arm.generator.max_acceleration)
+                        target_arm.generated_trajectory = None
+                        print(f"  {target_arm.name.upper()} max velocity: {vel:.2f} rad/s")
+                except ValueError:
+                    print("  Invalid velocity value")
+
+            elif cmd in ("quit", "exit", "q"):
+                break
+
+            else:
+                print(f"  Unknown command: {cmd}")
+
+    except KeyboardInterrupt:
+        print("\n\nExiting...")
+
+
+def main() -> int:
+    """Main entry point."""
+    import argparse
+
+    parser = argparse.ArgumentParser(description="Dual-Arm Interactive Trajectory Setter")
+    parser.add_argument(
+        "--left-joint-topic",
+        type=str,
+        default="/xarm/left/joint_states",
+        help="Left arm joint state topic",
+    )
+    parser.add_argument(
+        "--right-joint-topic",
+        type=str,
+        default="/xarm/right/joint_states",
+        help="Right arm joint state topic",
+    )
+    parser.add_argument(
+        "--left-trajectory-topic",
+        type=str,
+        default="/xarm/left/trajectory",
+        help="Left arm trajectory topic",
+    )
+    parser.add_argument(
+        "--right-trajectory-topic",
+        type=str,
+        default="/xarm/right/trajectory",
+        help="Right arm trajectory topic",
+    )
+    args = parser.parse_args()
+
+    print("\n" + "=" * 80)
+    print("Dual-Arm Trajectory Setter")
+    print("=" * 80)
+    print("\nRun 'dimos run xarm-dual-trajectory' in another terminal first!")
+    print("=" * 80)
+
+    setter = DualTrajectorySetter(
+        left_joint_topic=args.left_joint_topic,
+        right_joint_topic=args.right_joint_topic,
+        left_trajectory_topic=args.left_trajectory_topic,
+        right_trajectory_topic=args.right_trajectory_topic,
+    )
+
+    if not setter.start():
+        print("\nWarning: Could not connect to both arms")
+        response = input("Continue anyway? [y/N]: ").strip().lower()
+        if response != "y":
+            return 0
+
+    interactive_mode(setter)
+    return 0
+
+
+if __name__ == "__main__":
+    try:
+        sys.exit(main())
+    except KeyboardInterrupt:
+        print("\n\nInterrupted by user")
+        sys.exit(0)
+    except Exception as e:
+        print(f"\nError: {e}")
+        import traceback
+
+        traceback.print_exc()
+        sys.exit(1)
diff --git a/dimos/manipulation/control/orchestrator_client.py b/dimos/manipulation/control/orchestrator_client.py
new file mode 100644
index 0000000000..84e85dfb3d
--- /dev/null
+++ b/dimos/manipulation/control/orchestrator_client.py
@@ -0,0 +1,696 @@
+#!/usr/bin/env python3
+# 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.
+
+"""
+Interactive client for the ControlOrchestrator.
+
+Interfaces with a running ControlOrchestrator via RPC to:
+- Query hardware and task status
+- Plan and execute trajectories on single or multiple arms
+- Monitor execution progress
+
+Usage:
+    # Terminal 1: Start the orchestrator
+    dimos run orchestrator-mock          # Single arm
+    dimos run orchestrator-dual-mock     # Dual arm
+
+    # Terminal 2: Run this client
+    python -m dimos.manipulation.control.orchestrator_client
+    python -m dimos.manipulation.control.orchestrator_client --task traj_left
+    python -m dimos.manipulation.control.orchestrator_client --task traj_right
+
+How it works:
+    1. Connects to ControlOrchestrator via LCM RPC
+    2. Queries available hardware/tasks/joints
+    3. You add waypoints (joint positions)
+    4. Generates trajectory with trapezoidal velocity profile
+    5. Sends trajectory to orchestrator via execute_trajectory() RPC
+    6. Orchestrator's tick loop executes it at 100Hz
+"""
+
+from __future__ import annotations
+
+import math
+import sys
+import time
+from typing import TYPE_CHECKING, Any
+
+from dimos.control.orchestrator import ControlOrchestrator
+from dimos.core.rpc_client import RPCClient
+from dimos.manipulation.planning import JointTrajectoryGenerator
+
+if TYPE_CHECKING:
+    from dimos.msgs.trajectory_msgs import JointTrajectory
+
+
+class OrchestratorClient:
+    """
+    RPC client for the ControlOrchestrator.
+
+    Connects to a running orchestrator and provides methods to:
+    - Query state (joints, tasks, hardware)
+    - Execute trajectories on any task
+    - Monitor progress
+
+    Example:
+        client = OrchestratorClient()
+
+        # Query state
+        print(client.list_hardware())  # ['left_arm', 'right_arm']
+        print(client.list_tasks())     # ['traj_left', 'traj_right']
+
+        # Setup for a task
+        client.select_task("traj_left")
+
+        # Get current position and create trajectory
+        current = client.get_current_positions()
+        target = [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        trajectory = client.generate_trajectory([current, target])
+
+        # Execute
+        client.execute_trajectory("traj_left", trajectory)
+    """
+
+    def __init__(self) -> None:
+        """Initialize connection to orchestrator via RPC."""
+        self._rpc = RPCClient(None, ControlOrchestrator)
+
+        # Per-task state
+        self._current_task: str | None = None
+        self._task_joints: dict[str, list[str]] = {}  # task_name -> joint_names
+        self._generators: dict[str, JointTrajectoryGenerator] = {}  # task_name -> generator
+
+    def stop(self) -> None:
+        """Stop the RPC client."""
+        self._rpc.stop_rpc_client()
+
+    # =========================================================================
+    # Query methods (RPC calls)
+    # =========================================================================
+
+    def list_hardware(self) -> list[str]:
+        """List all hardware IDs."""
+        return self._rpc.list_hardware() or []
+
+    def list_joints(self) -> list[str]:
+        """List all joint names across all hardware."""
+        return self._rpc.list_joints() or []
+
+    def list_tasks(self) -> list[str]:
+        """List all task names."""
+        return self._rpc.list_tasks() or []
+
+    def get_active_tasks(self) -> list[str]:
+        """Get currently active task names."""
+        return self._rpc.get_active_tasks() or []
+
+    def get_joint_positions(self) -> dict[str, float]:
+        """Get current joint positions for all joints."""
+        return self._rpc.get_joint_positions() or {}
+
+    def get_trajectory_status(self, task_name: str) -> dict[str, Any]:
+        """Get status of a trajectory task."""
+        return self._rpc.get_trajectory_status(task_name) or {}
+
+    # =========================================================================
+    # Trajectory execution (RPC calls)
+    # =========================================================================
+
+    def execute_trajectory(self, task_name: str, trajectory: JointTrajectory) -> bool:
+        """Execute a trajectory on a task."""
+        return self._rpc.execute_trajectory(task_name, trajectory) or False
+
+    def cancel_trajectory(self, task_name: str) -> bool:
+        """Cancel an active trajectory."""
+        return self._rpc.cancel_trajectory(task_name) or False
+
+    # =========================================================================
+    # Task selection and setup
+    # =========================================================================
+
+    def select_task(self, task_name: str) -> bool:
+        """
+        Select a task and setup its trajectory generator.
+
+        This queries the orchestrator to find which joints the task controls,
+        then creates a trajectory generator for those joints.
+        """
+        tasks = self.list_tasks()
+        if task_name not in tasks:
+            print(f"Task '{task_name}' not found. Available: {tasks}")
+            return False
+
+        self._current_task = task_name
+
+        # Get joints for this task (infer from task name pattern)
+        # e.g., "traj_left" -> joints starting with "left_"
+        # e.g., "traj_arm" -> joints starting with "arm_"
+        all_joints = self.list_joints()
+
+        # Try to infer prefix from task name
+        if "_" in task_name:
+            prefix = task_name.split("_", 1)[1]  # "traj_left" -> "left"
+            task_joints = [j for j in all_joints if j.startswith(prefix + "_")]
+        else:
+            task_joints = all_joints
+
+        if not task_joints:
+            # Fallback: use all joints
+            task_joints = all_joints
+
+        self._task_joints[task_name] = task_joints
+
+        # Create generator if not exists
+        if task_name not in self._generators:
+            self._generators[task_name] = JointTrajectoryGenerator(
+                num_joints=len(task_joints),
+                max_velocity=1.0,
+                max_acceleration=2.0,
+                points_per_segment=50,
+            )
+
+        return True
+
+    def get_task_joints(self, task_name: str | None = None) -> list[str]:
+        """Get joint names for a task."""
+        task = task_name or self._current_task
+        if task is None:
+            return []
+        return self._task_joints.get(task, [])
+
+    def get_current_positions(self, task_name: str | None = None) -> list[float] | None:
+        """Get current joint positions for a task as a list."""
+        task = task_name or self._current_task
+        if task is None:
+            return None
+
+        joints = self._task_joints.get(task, [])
+        if not joints:
+            return None
+
+        positions = self.get_joint_positions()
+        if not positions:
+            return None
+
+        return [positions.get(j, 0.0) for j in joints]
+
+    def generate_trajectory(
+        self, waypoints: list[list[float]], task_name: str | None = None
+    ) -> JointTrajectory | None:
+        """Generate trajectory from waypoints using trapezoidal velocity profile."""
+        task = task_name or self._current_task
+        if task is None:
+            print("Error: No task selected")
+            return None
+
+        generator = self._generators.get(task)
+        if generator is None:
+            print(f"Error: No generator for task '{task}'. Call select_task() first.")
+            return None
+
+        return generator.generate(waypoints)
+
+    def set_velocity_limit(self, velocity: float, task_name: str | None = None) -> None:
+        """Set max velocity for trajectory generation."""
+        task = task_name or self._current_task
+        if task and task in self._generators:
+            gen = self._generators[task]
+            gen.set_limits(velocity, gen.max_acceleration)
+
+    def set_acceleration_limit(self, acceleration: float, task_name: str | None = None) -> None:
+        """Set max acceleration for trajectory generation."""
+        task = task_name or self._current_task
+        if task and task in self._generators:
+            gen = self._generators[task]
+            gen.set_limits(gen.max_velocity, acceleration)
+
+
+# =============================================================================
+# Interactive CLI
+# =============================================================================
+
+
+def parse_joint_input(line: str, num_joints: int) -> list[float] | None:
+    """Parse joint positions from user input (degrees by default, 'r' suffix for radians)."""
+    parts = line.strip().split()
+    if len(parts) != num_joints:
+        return None
+
+    positions = []
+    for part in parts:
+        try:
+            if part.endswith("r"):
+                positions.append(float(part[:-1]))
+            else:
+                positions.append(math.radians(float(part)))
+        except ValueError:
+            return None
+
+    return positions
+
+
+def format_positions(positions: list[float], as_degrees: bool = True) -> str:
+    """Format positions for display."""
+    if as_degrees:
+        return "[" + ", ".join(f"{math.degrees(p):.1f}" for p in positions) + "] deg"
+    return "[" + ", ".join(f"{p:.3f}" for p in positions) + "] rad"
+
+
+def preview_waypoints(waypoints: list[list[float]], joint_names: list[str]) -> None:
+    """Show waypoints list."""
+    if not waypoints:
+        print("No waypoints")
+        return
+
+    print(f"\nWaypoints ({len(waypoints)}):")
+    print("-" * 70)
+
+    # Header with joint names (truncated)
+    headers = [j.split("_")[-1][:6] for j in joint_names]  # e.g., "joint1" -> "joint1"
+    header_str = " ".join(f"{h:>7}" for h in headers)
+    print(f"  # | {header_str} (degrees)")
+    print("-" * 70)
+
+    for i, joints in enumerate(waypoints):
+        deg = [f"{math.degrees(j):7.1f}" for j in joints]
+        print(f" {i + 1:2} | {' '.join(deg)}")
+    print("-" * 70)
+
+
+def preview_trajectory(trajectory: JointTrajectory, joint_names: list[str]) -> None:
+    """Show generated trajectory preview."""
+    headers = [j.split("_")[-1][:6] for j in joint_names]
+    header_str = " ".join(f"{h:>7}" for h in headers)
+
+    print("\n" + "=" * 70)
+    print("GENERATED TRAJECTORY")
+    print("=" * 70)
+    print(f"Duration: {trajectory.duration:.3f}s")
+    print(f"Points: {len(trajectory.points)}")
+    print("-" * 70)
+    print(f"{'Time':>6} | {header_str} (degrees)")
+    print("-" * 70)
+
+    num_samples = min(10, max(len(trajectory.points) // 10, 5))
+    for i in range(num_samples + 1):
+        t = (i / num_samples) * trajectory.duration
+        q_ref, _ = trajectory.sample(t)
+        q_deg = [f"{math.degrees(q):7.1f}" for q in q_ref]
+        print(f"{t:6.2f} | {' '.join(q_deg)}")
+
+    print("=" * 70)
+
+
+def wait_for_completion(client: OrchestratorClient, task_name: str, timeout: float = 60.0) -> bool:
+    """Wait for trajectory to complete with progress display."""
+    start = time.time()
+    last_progress = -1.0
+
+    while time.time() - start < timeout:
+        status = client.get_trajectory_status(task_name)
+        if not status.get("active", False):
+            state: str = status.get("state", "UNKNOWN")
+            print(f"\nTrajectory finished: {state}")
+            return state == "COMPLETED"
+
+        progress = status.get("progress", 0.0)
+        if progress != last_progress:
+            bar_len = 30
+            filled = int(bar_len * progress)
+            bar = "=" * filled + "-" * (bar_len - filled)
+            print(f"\r[{bar}] {progress * 100:.1f}%", end="", flush=True)
+            last_progress = progress
+
+        time.sleep(0.05)
+
+    print("\nTimeout waiting for trajectory")
+    return False
+
+
+class OrchestratorShell:
+    """IPython shell interface for orchestrator control."""
+
+    def __init__(self, client: OrchestratorClient, initial_task: str) -> None:
+        self._client = client
+        self._current_task = initial_task
+        self._waypoints: list[list[float]] = []
+        self._generated_trajectory: JointTrajectory | None = None
+
+        if not client.select_task(initial_task):
+            raise ValueError(f"Failed to select task: {initial_task}")
+
+    def _joints(self) -> list[str]:
+        return self._client.get_task_joints(self._current_task)
+
+    def _num_joints(self) -> int:
+        return len(self._joints())
+
+    def help(self) -> None:
+        """Show available commands."""
+        print("\nOrchestrator Client Commands:")
+        print("=" * 60)
+        print("Waypoint Commands:")
+        print("  here()                 - Add current position as waypoint")
+        print("  add(j1, j2, ...)       - Add waypoint (degrees)")
+        print("  waypoints()            - List all waypoints")
+        print("  delete(n)              - Delete waypoint n")
+        print("  clear()                - Clear all waypoints")
+        print("\nTrajectory Commands:")
+        print("  preview()              - Preview generated trajectory")
+        print("  run()                  - Execute trajectory")
+        print("  status()               - Show task status")
+        print("  cancel()               - Cancel active trajectory")
+        print("\nMulti-Arm Commands:")
+        print("  tasks()                - List all tasks")
+        print("  switch('task_name')    - Switch to different task")
+        print("  hw()                   - List hardware")
+        print("  joints()               - List joints for current task")
+        print("\nSettings:")
+        print("  current()              - Show current joint positions")
+        print("  vel(value)             - Set max velocity (rad/s)")
+        print("  accel(value)           - Set max acceleration (rad/s^2)")
+        print("=" * 60)
+
+    def here(self) -> None:
+        """Add current position as waypoint."""
+        positions = self._client.get_current_positions(self._current_task)
+        if positions:
+            self._waypoints.append(positions)
+            self._generated_trajectory = None
+            print(f"Added waypoint {len(self._waypoints)}: {format_positions(positions)}")
+        else:
+            print("Could not get current positions")
+
+    def add(self, *joints: float) -> None:
+        """Add waypoint with specified joint values (in degrees)."""
+        num_joints = self._num_joints()
+        if len(joints) != num_joints:
+            print(f"Need {num_joints} joint values, got {len(joints)}")
+            return
+
+        rad_joints = [math.radians(j) for j in joints]
+        self._waypoints.append(rad_joints)
+        self._generated_trajectory = None
+        print(f"Added waypoint {len(self._waypoints)}: {format_positions(rad_joints)}")
+
+    def waypoints(self) -> None:
+        """List all waypoints."""
+        preview_waypoints(self._waypoints, self._joints())
+
+    def delete(self, index: int) -> None:
+        """Delete a waypoint by index (1-based)."""
+        idx = index - 1
+        if 0 <= idx < len(self._waypoints):
+            self._waypoints.pop(idx)
+            self._generated_trajectory = None
+            print(f"Deleted waypoint {index}")
+        else:
+            print(f"Invalid index (1-{len(self._waypoints)})")
+
+    def clear(self) -> None:
+        """Clear all waypoints."""
+        self._waypoints.clear()
+        self._generated_trajectory = None
+        print("Cleared waypoints")
+
+    def preview(self) -> None:
+        """Preview generated trajectory."""
+        if len(self._waypoints) < 2:
+            print("Need at least 2 waypoints")
+            return
+        try:
+            self._generated_trajectory = self._client.generate_trajectory(
+                self._waypoints, self._current_task
+            )
+            if self._generated_trajectory:
+                preview_trajectory(self._generated_trajectory, self._joints())
+        except Exception as e:
+            print(f"Error: {e}")
+
+    def run(self) -> None:
+        """Execute trajectory."""
+        if len(self._waypoints) < 2:
+            print("Need at least 2 waypoints")
+            return
+
+        if self._generated_trajectory is None:
+            self._generated_trajectory = self._client.generate_trajectory(
+                self._waypoints, self._current_task
+            )
+
+        if self._generated_trajectory is None:
+            print("Failed to generate trajectory")
+            return
+
+        preview_trajectory(self._generated_trajectory, self._joints())
+        confirm = input("\nExecute? [y/N]: ").strip().lower()
+        if confirm == "y":
+            if self._client.execute_trajectory(self._current_task, self._generated_trajectory):
+                print("Trajectory started...")
+                wait_for_completion(self._client, self._current_task)
+            else:
+                print("Failed to start trajectory")
+
+    def status(self) -> None:
+        """Show task status."""
+        status = self._client.get_trajectory_status(self._current_task)
+        print(f"\nTask: {self._current_task}")
+        print(f"  Active: {status.get('active', False)}")
+        print(f"  State: {status.get('state', 'UNKNOWN')}")
+        if "progress" in status:
+            print(f"  Progress: {status['progress'] * 100:.1f}%")
+
+    def cancel(self) -> None:
+        """Cancel active trajectory."""
+        if self._client.cancel_trajectory(self._current_task):
+            print("Cancelled")
+        else:
+            print("Cancel failed")
+
+    def tasks(self) -> None:
+        """List all tasks."""
+        all_tasks = self._client.list_tasks()
+        active = self._client.get_active_tasks()
+        print("\nTasks:")
+        for t in all_tasks:
+            marker = "* " if t == self._current_task else "  "
+            active_marker = " [ACTIVE]" if t in active else ""
+            t_joints = self._client.get_task_joints(t)
+            joint_count = len(t_joints) if t_joints else "?"
+            print(f"{marker}{t} ({joint_count} joints){active_marker}")
+
+    def switch(self, task_name: str) -> None:
+        """Switch to a different task."""
+        if self._client.select_task(task_name):
+            self._current_task = task_name
+            self._waypoints.clear()
+            self._generated_trajectory = None
+            joints = self._joints()
+            print(f"Switched to {self._current_task} ({len(joints)} joints)")
+            print(f"Joints: {', '.join(joints)}")
+        else:
+            print(f"Failed to switch to {task_name}")
+
+    def hw(self) -> None:
+        """List hardware."""
+        hardware = self._client.list_hardware()
+        print(f"\nHardware: {', '.join(hardware)}")
+
+    def joints(self) -> None:
+        """List joints for current task."""
+        joints = self._joints()
+        print(f"\nJoints for {self._current_task}:")
+        for i, j in enumerate(joints):
+            pos = self._client.get_joint_positions().get(j, 0.0)
+            print(f"  {i + 1}. {j}: {math.degrees(pos):.1f} deg")
+
+    def current(self) -> None:
+        """Show current joint positions."""
+        positions = self._client.get_current_positions(self._current_task)
+        if positions:
+            print(f"Current: {format_positions(positions)}")
+        else:
+            print("Could not get positions")
+
+    def vel(self, value: float | None = None) -> None:
+        """Set or show max velocity (rad/s)."""
+        if value is None:
+            gen = self._client._generators.get(self._current_task)
+            if gen:
+                print(f"Max velocity: {gen.max_velocity[0]:.2f} rad/s")
+            return
+
+        if value <= 0:
+            print("Velocity must be positive")
+            return
+
+        self._client.set_velocity_limit(value, self._current_task)
+        self._generated_trajectory = None
+        print(f"Max velocity: {value:.2f} rad/s")
+
+    def accel(self, value: float | None = None) -> None:
+        """Set or show max acceleration (rad/s^2)."""
+        if value is None:
+            gen = self._client._generators.get(self._current_task)
+            if gen:
+                print(f"Max acceleration: {gen.max_acceleration[0]:.2f} rad/s^2")
+            return
+
+        if value <= 0:
+            print("Acceleration must be positive")
+            return
+
+        self._client.set_acceleration_limit(value, self._current_task)
+        self._generated_trajectory = None
+        print(f"Max acceleration: {value:.2f} rad/s^2")
+
+
+def interactive_mode(client: OrchestratorClient, initial_task: str) -> None:
+    """Start IPython interactive mode."""
+    import IPython
+
+    shell = OrchestratorShell(client, initial_task)
+
+    print("\n" + "=" * 60)
+    print(f"Orchestrator Client (IPython) - Task: {initial_task}")
+    print("=" * 60)
+    print(f"Joints: {', '.join(shell._joints())}")
+    print("\nType help() for available commands")
+    print("=" * 60 + "\n")
+
+    IPython.start_ipython(  # type: ignore[no-untyped-call]
+        argv=[],
+        user_ns={
+            "help": shell.help,
+            "here": shell.here,
+            "add": shell.add,
+            "waypoints": shell.waypoints,
+            "delete": shell.delete,
+            "clear": shell.clear,
+            "preview": shell.preview,
+            "run": shell.run,
+            "status": shell.status,
+            "cancel": shell.cancel,
+            "tasks": shell.tasks,
+            "switch": shell.switch,
+            "hw": shell.hw,
+            "joints": shell.joints,
+            "current": shell.current,
+            "vel": shell.vel,
+            "accel": shell.accel,
+            "client": client,
+            "shell": shell,
+        },
+    )
+
+
+def _run_client(client: OrchestratorClient, task: str, vel: float, accel: float) -> int:
+    """Run the client with the given configuration."""
+    try:
+        hardware = client.list_hardware()
+        tasks = client.list_tasks()
+
+        if not hardware:
+            print("\nWarning: No hardware found. Is the orchestrator running?")
+            print("Start with: dimos run orchestrator-mock")
+            response = input("Continue anyway? [y/N]: ").strip().lower()
+            if response != "y":
+                return 0
+        else:
+            print(f"Hardware: {', '.join(hardware)}")
+            print(f"Tasks: {', '.join(tasks)}")
+
+    except Exception as e:
+        print(f"\nConnection error: {e}")
+        print("Make sure orchestrator is running: dimos run orchestrator-mock")
+        return 1
+
+    if task not in tasks and tasks:
+        print(f"\nTask '{task}' not found.")
+        print(f"Available: {', '.join(tasks)}")
+        task = tasks[0]
+        print(f"Using '{task}'")
+
+    if client.select_task(task):
+        client.set_velocity_limit(vel, task)
+        client.set_acceleration_limit(accel, task)
+
+    interactive_mode(client, task)
+    return 0
+
+
+def main() -> int:
+    """Main entry point."""
+    import argparse
+
+    parser = argparse.ArgumentParser(
+        description="Interactive client for ControlOrchestrator",
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+        epilog="""
+Examples:
+  # Single arm (with orchestrator-mock running)
+  python -m dimos.manipulation.control.orchestrator_client
+
+  # Dual arm - control left arm
+  python -m dimos.manipulation.control.orchestrator_client --task traj_left
+
+  # Dual arm - control right arm
+  python -m dimos.manipulation.control.orchestrator_client --task traj_right
+        """,
+    )
+    parser.add_argument(
+        "--task",
+        type=str,
+        default="traj_arm",
+        help="Initial task to control (default: traj_arm)",
+    )
+    parser.add_argument(
+        "--vel",
+        type=float,
+        default=1.0,
+        help="Max velocity in rad/s (default: 1.0)",
+    )
+    parser.add_argument(
+        "--accel",
+        type=float,
+        default=2.0,
+        help="Max acceleration in rad/s^2 (default: 2.0)",
+    )
+    args = parser.parse_args()
+
+    print("\n" + "=" * 70)
+    print("Orchestrator Client")
+    print("=" * 70)
+    print("\nConnecting to ControlOrchestrator via RPC...")
+
+    client = OrchestratorClient()
+    try:
+        return _run_client(client, args.task, args.vel, args.accel)
+    finally:
+        client.stop()
+
+
+if __name__ == "__main__":
+    try:
+        sys.exit(main())
+    except KeyboardInterrupt:
+        print("\n\nInterrupted")
+        sys.exit(0)
+    except Exception as e:
+        print(f"\nError: {e}")
+        import traceback
+
+        traceback.print_exc()
+        sys.exit(1)
diff --git a/dimos/manipulation/control/servo_control/cartesian_motion_controller.py b/dimos/manipulation/control/servo_control/cartesian_motion_controller.py
index cfbdb77cbf..f5a0810803 100644
--- a/dimos/manipulation/control/servo_control/cartesian_motion_controller.py
+++ b/dimos/manipulation/control/servo_control/cartesian_motion_controller.py
@@ -34,7 +34,6 @@
 
 from dimos.core import In, Module, Out, rpc
 from dimos.core.module import ModuleConfig
-from dimos.hardware.manipulators.xarm.spec import ArmDriverSpec
 from dimos.msgs.geometry_msgs import Pose, PoseStamped, Quaternion, Twist, Vector3
 from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState
 from dimos.utils.logging_config import setup_logger
@@ -90,7 +89,7 @@ class CartesianMotionController(Module):
     5. Publishing joint commands to the driver
 
     The controller is hardware-agnostic: it works with any arm driver that
-    implements the ArmDriverSpec protocol (provides IK/FK RPC methods).
+    provides IK/FK RPC methods and JointState/RobotState outputs.
     """
 
     default_config = CartesianMotionControllerConfig
@@ -112,12 +111,12 @@ class CartesianMotionController(Module):
     cartesian_velocity: Out[Twist] = None  # type: ignore[assignment]
     current_pose: Out[PoseStamped] = None  # type: ignore[assignment]
 
-    def __init__(self, arm_driver: ArmDriverSpec | None = None, *args: Any, **kwargs: Any) -> None:
+    def __init__(self, arm_driver: Any = None, *args: Any, **kwargs: Any) -> None:
         """
         Initialize the Cartesian motion controller.
 
         Args:
-            arm_driver: (Optional) Hardware driver implementing ArmDriverSpec protocol.
+            arm_driver: (Optional) Hardware driver reference (legacy mode).
                        When using blueprints, this is resolved automatically via rpc_calls.
         """
         super().__init__(*args, **kwargs)
diff --git a/dimos/manipulation/control/servo_control/example_cartesian_control.py b/dimos/manipulation/control/servo_control/example_cartesian_control.py
deleted file mode 100644
index eeff04e424..0000000000
--- a/dimos/manipulation/control/servo_control/example_cartesian_control.py
+++ /dev/null
@@ -1,194 +0,0 @@
-#!/usr/bin/env python3
-# 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.
-
-"""
-Example: Topic-Based Cartesian Motion Control with xArm
-
-Demonstrates topic-based Cartesian space motion control. The controller
-subscribes to /target_pose and automatically moves to received targets.
-
-This example shows:
-1. Deploy xArm driver with LCM transports
-2. Deploy CartesianMotionController with LCM transports
-3. Configure controller to subscribe to /target_pose topic
-4. Keep system running to process incoming targets
-
-Use target_setter.py to publish target poses to /target_pose topic.
-
-Pattern matches: interactive_control.py + sample_trajectory_generator.py
-"""
-
-import signal
-import time
-
-from dimos import core
-from dimos.hardware.manipulators.xarm import XArmDriver
-from dimos.manipulation.control import CartesianMotionController
-from dimos.msgs.geometry_msgs import PoseStamped
-from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState
-
-# Global flag for graceful shutdown
-shutdown_requested = False
-
-
-def signal_handler(sig, frame):  # type: ignore[no-untyped-def]
-    """Handle Ctrl+C for graceful shutdown."""
-    global shutdown_requested
-    print("\n\nShutdown requested...")
-    shutdown_requested = True
-
-
-def main():  # type: ignore[no-untyped-def]
-    """
-    Deploy and run topic-based Cartesian motion control system.
-
-    The system subscribes to /target_pose and automatically moves
-    the robot to received target poses.
-    """
-
-    # Register signal handler for graceful shutdown
-    signal.signal(signal.SIGINT, signal_handler)
-    signal.signal(signal.SIGTERM, signal_handler)
-
-    # =========================================================================
-    # Step 1: Start dimos cluster
-    # =========================================================================
-    print("=" * 80)
-    print("Topic-Based Cartesian Motion Control")
-    print("=" * 80)
-    print("\nStarting dimos cluster...")
-    dimos = core.start(1)  # Start with 1 worker
-
-    try:
-        # =========================================================================
-        # Step 2: Deploy xArm driver
-        # =========================================================================
-        print("\nDeploying xArm driver...")
-        arm_driver = dimos.deploy(  # type: ignore[attr-defined]
-            XArmDriver,
-            ip_address="192.168.1.210",
-            xarm_type="xarm6",
-            report_type="dev",
-            enable_on_start=True,
-        )
-
-        # Set up driver transports
-        arm_driver.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState)
-        arm_driver.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState)
-        arm_driver.joint_position_command.transport = core.LCMTransport(
-            "/xarm/joint_position_command", JointCommand
-        )
-        arm_driver.joint_velocity_command.transport = core.LCMTransport(
-            "/xarm/joint_velocity_command", JointCommand
-        )
-
-        print("Starting xArm driver...")
-        arm_driver.start()
-
-        # =========================================================================
-        # Step 3: Deploy Cartesian motion controller
-        # =========================================================================
-        print("\nDeploying Cartesian motion controller...")
-        controller = dimos.deploy(  # type: ignore[attr-defined]
-            CartesianMotionController,
-            arm_driver=arm_driver,
-            control_frequency=20.0,
-            position_kp=1.0,
-            position_kd=0.1,
-            orientation_kp=2.0,
-            orientation_kd=0.2,
-            max_linear_velocity=0.15,
-            max_angular_velocity=0.8,
-            position_tolerance=0.002,
-            orientation_tolerance=0.02,
-            velocity_control_mode=True,
-        )
-
-        # Set up controller transports
-        controller.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState)
-        controller.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState)
-        controller.joint_position_command.transport = core.LCMTransport(
-            "/xarm/joint_position_command", JointCommand
-        )
-
-        # IMPORTANT: Configure controller to subscribe to /target_pose topic
-        controller.target_pose.transport = core.LCMTransport("/target_pose", PoseStamped)
-
-        # Publish current pose for target setters to use
-        controller.current_pose.transport = core.LCMTransport("/xarm/current_pose", PoseStamped)
-
-        print("Starting controller...")
-        controller.start()
-
-        # =========================================================================
-        # Step 4: Keep system running
-        # =========================================================================
-        print("\n" + "=" * 80)
-        print("✓ System ready!")
-        print("=" * 80)
-        print("\nController is now listening to /target_pose topic")
-        print("Use target_setter.py to publish target poses")
-        print("\nPress Ctrl+C to shutdown")
-        print("=" * 80 + "\n")
-
-        # Keep running until shutdown requested
-        while not shutdown_requested:
-            time.sleep(0.5)
-
-        # =========================================================================
-        # Step 5: Clean shutdown
-        # =========================================================================
-        print("\nShutting down...")
-        print("Stopping controller...")
-        controller.stop()
-        print("Stopping driver...")
-        arm_driver.stop()
-        print("✓ Shutdown complete")
-
-    finally:
-        # Always stop dimos cluster
-        print("Stopping dimos cluster...")
-        dimos.stop()  # type: ignore[attr-defined]
-
-
-if __name__ == "__main__":
-    """
-    Topic-Based Cartesian Control for xArm.
-
-    Usage:
-        # Terminal 1: Start the controller (this script)
-        python3 example_cartesian_control.py
-
-        # Terminal 2: Publish target poses
-        python3 target_setter.py --world 0.4 0.0 0.5  # Absolute world coordinates
-        python3 target_setter.py --relative 0.05 0 0  # Relative movement (50mm in X)
-
-    The controller subscribes to /target_pose topic and automatically moves
-    the robot to received target poses.
-
-    Requirements:
-        - xArm robot connected at 192.168.2.235
-        - Robot will be automatically enabled in servo mode
-        - Proper network configuration
-    """
-    try:
-        main()  # type: ignore[no-untyped-call]
-    except KeyboardInterrupt:
-        print("\n\nInterrupted by user")
-    except Exception as e:
-        print(f"\nError: {e}")
-        import traceback
-
-        traceback.print_exc()
diff --git a/dimos/manipulation/control/trajectory_controller/example_trajectory_control.py b/dimos/manipulation/control/trajectory_controller/example_trajectory_control.py
deleted file mode 100644
index 100e095a45..0000000000
--- a/dimos/manipulation/control/trajectory_controller/example_trajectory_control.py
+++ /dev/null
@@ -1,189 +0,0 @@
-#!/usr/bin/env python3
-# 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.
-
-"""
-Example: Joint Trajectory Control with xArm
-
-Demonstrates joint-space trajectory execution. The controller
-executes trajectories by sampling at 100Hz and sending joint commands.
-
-This example shows:
-1. Deploy xArm driver with LCM transports
-2. Deploy JointTrajectoryController with LCM transports
-3. Execute trajectories via RPC or topic
-4. Monitor execution status
-
-Use trajectory_setter.py to interactively create and execute trajectories.
-"""
-
-import signal
-import time
-
-from dimos import core
-from dimos.hardware.manipulators.xarm import XArmDriver
-from dimos.manipulation.control import JointTrajectoryController
-from dimos.msgs.sensor_msgs import JointCommand, JointState, RobotState
-from dimos.msgs.trajectory_msgs import JointTrajectory, TrajectoryState
-
-# Global flag for graceful shutdown
-shutdown_requested = False
-
-
-def signal_handler(sig, frame):  # type: ignore[no-untyped-def]
-    """Handle Ctrl+C for graceful shutdown."""
-    global shutdown_requested
-    print("\n\nShutdown requested...")
-    shutdown_requested = True
-
-
-def main():  # type: ignore[no-untyped-def]
-    """
-    Deploy and run joint trajectory control system.
-
-    The system executes joint trajectories at 100Hz by sampling
-    and forwarding joint positions to the arm driver.
-    """
-
-    # Register signal handler for graceful shutdown
-    signal.signal(signal.SIGINT, signal_handler)
-    signal.signal(signal.SIGTERM, signal_handler)
-
-    # =========================================================================
-    # Step 1: Start dimos cluster
-    # =========================================================================
-    print("=" * 80)
-    print("Joint Trajectory Control")
-    print("=" * 80)
-    print("\nStarting dimos cluster...")
-    dimos = core.start(1)  # Start with 1 worker
-
-    try:
-        # =========================================================================
-        # Step 2: Deploy xArm driver
-        # =========================================================================
-        print("\nDeploying xArm driver...")
-        arm_driver = dimos.deploy(  # type: ignore[attr-defined]
-            XArmDriver,
-            ip_address="192.168.1.210",
-            xarm_type="xarm6",
-            report_type="dev",
-            enable_on_start=True,
-        )
-
-        # Set up driver transports
-        arm_driver.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState)
-        arm_driver.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState)
-        arm_driver.joint_position_command.transport = core.LCMTransport(
-            "/xarm/joint_position_command", JointCommand
-        )
-
-        print("Starting xArm driver...")
-        arm_driver.start()
-
-        # =========================================================================
-        # Step 3: Deploy Joint Trajectory Controller
-        # =========================================================================
-        print("\nDeploying Joint Trajectory Controller...")
-        controller = dimos.deploy(  # type: ignore[attr-defined]
-            JointTrajectoryController,
-            control_frequency=100.0,  # 100Hz execution
-        )
-
-        # Set up controller transports
-        controller.joint_state.transport = core.LCMTransport("/xarm/joint_states", JointState)
-        controller.robot_state.transport = core.LCMTransport("/xarm/robot_state", RobotState)
-        controller.joint_position_command.transport = core.LCMTransport(
-            "/xarm/joint_position_command", JointCommand
-        )
-
-        # Subscribe to trajectory topic (from trajectory_setter.py)
-        controller.trajectory.transport = core.LCMTransport("/trajectory", JointTrajectory)
-
-        print("Starting controller...")
-        controller.start()
-
-        # Wait for joint state
-        print("\nWaiting for joint state...")
-        time.sleep(1.0)
-
-        # =========================================================================
-        # Step 4: Keep system running
-        # =========================================================================
-        print("\n" + "=" * 80)
-        print("System ready!")
-        print("=" * 80)
-        print("\nJoint Trajectory Controller is running at 100Hz")
-        print("Listening on /trajectory topic")
-        print("\nUse trajectory_setter.py in another terminal to publish trajectories")
-        print("\nPress Ctrl+C to shutdown")
-        print("=" * 80 + "\n")
-
-        # Keep running until shutdown requested
-        while not shutdown_requested:
-            # Print status periodically
-            status = controller.get_status()
-            if status.state == TrajectoryState.EXECUTING:
-                print(
-                    f"\rExecuting: {status.progress:.1%} | "
-                    f"elapsed={status.time_elapsed:.2f}s | "
-                    f"remaining={status.time_remaining:.2f}s",
-                    end="",
-                )
-            time.sleep(0.5)
-
-        # =========================================================================
-        # Step 5: Clean shutdown
-        # =========================================================================
-        print("\n\nShutting down...")
-        print("Stopping controller...")
-        controller.stop()
-        print("Stopping driver...")
-        arm_driver.stop()
-        print("Shutdown complete")
-
-    finally:
-        # Always stop dimos cluster
-        print("Stopping dimos cluster...")
-        dimos.stop()  # type: ignore[attr-defined]
-
-
-if __name__ == "__main__":
-    """
-    Joint Trajectory Control for xArm.
-
-    Usage:
-        # Terminal 1: Start the controller (this script)
-        python3 example_trajectory_control.py
-
-        # Terminal 2: Create and execute trajectories
-        python3 trajectory_setter.py
-
-    The controller executes joint trajectories at 100Hz by sampling
-    and forwarding joint positions to the arm driver.
-
-    Requirements:
-        - xArm robot connected at 192.168.1.210
-        - Robot will be automatically enabled in servo mode
-        - Proper network configuration
-    """
-    try:
-        main()  # type: ignore[no-untyped-call]
-    except KeyboardInterrupt:
-        print("\n\nInterrupted by user")
-    except Exception as e:
-        print(f"\nError: {e}")
-        import traceback
-
-        traceback.print_exc()
diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py
index f989098f05..5b51caebef 100644
--- a/dimos/robot/all_blueprints.py
+++ b/dimos/robot/all_blueprints.py
@@ -36,16 +36,14 @@
     "unitree-g1-joystick": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:with_joystick",
     "unitree-g1-full": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:full_featured",
     "unitree-g1-detection": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:detection",
-    # xArm manipulator blueprints
-    "xarm-servo": "dimos.hardware.manipulators.xarm.xarm_blueprints:xarm_servo",
-    "xarm5-servo": "dimos.hardware.manipulators.xarm.xarm_blueprints:xarm5_servo",
-    "xarm7-servo": "dimos.hardware.manipulators.xarm.xarm_blueprints:xarm7_servo",
-    "xarm-cartesian": "dimos.hardware.manipulators.xarm.xarm_blueprints:xarm_cartesian",
-    "xarm-trajectory": "dimos.hardware.manipulators.xarm.xarm_blueprints:xarm_trajectory",
-    # Piper manipulator blueprints
-    "piper-servo": "dimos.hardware.manipulators.piper.piper_blueprints:piper_servo",
-    "piper-cartesian": "dimos.hardware.manipulators.piper.piper_blueprints:piper_cartesian",
-    "piper-trajectory": "dimos.hardware.manipulators.piper.piper_blueprints:piper_trajectory",
+    # Control orchestrator blueprints
+    "orchestrator-mock": "dimos.control.blueprints:orchestrator_mock",
+    "orchestrator-xarm7": "dimos.control.blueprints:orchestrator_xarm7",
+    "orchestrator-xarm6": "dimos.control.blueprints:orchestrator_xarm6",
+    "orchestrator-piper": "dimos.control.blueprints:orchestrator_piper",
+    "orchestrator-dual-mock": "dimos.control.blueprints:orchestrator_dual_mock",
+    "orchestrator-dual-xarm": "dimos.control.blueprints:orchestrator_dual_xarm",
+    "orchestrator-piper-xarm": "dimos.control.blueprints:orchestrator_piper_xarm",
     # Demo blueprints
     "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm",
     "demo-skill": "dimos.agents.skills.demo_skill:demo_skill",
@@ -83,10 +81,8 @@
     "wavefront_frontier_explorer": "dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector",
     "websocket_vis": "dimos.web.websocket_vis.websocket_vis_module",
     "web_input": "dimos.agents.cli.web",
-    # xArm manipulator modules
-    "xarm_driver": "dimos.hardware.manipulators.xarm.xarm_driver",
-    "cartesian_motion_controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller",
-    "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller",
+    # Control orchestrator module
+    "control_orchestrator": "dimos.control.orchestrator",
 }