Control Orchestrator - Unified Controller for multi-arm and full body controller

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`

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",
+]