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

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
55 changes: 34 additions & 21 deletions dimos/control/README.md
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
# Control Orchestrator
# Control Coordinator

Centralized control system for multi-arm robots with per-joint arbitration.

## Architecture

```
┌─────────────────────────────────────────────────────────────┐
ControlOrchestrator
ControlCoordinator
│ │
│ ┌──────────────────────────────────────────────────────┐ │
│ │ TickLoop (100Hz) │ │
Expand All @@ -16,22 +16,22 @@ Centralized control system for multi-arm robots with per-joint arbitration.
│ │ │ │ │ │
│ ▼ ▼ ▼ ▼ │
│ ┌─────────┐ ┌───────┐ ┌─────────┐ ┌──────────┐ │
│ │Hardware │ │ Tasks │ │Priority │ │ Backends │ │
│ │Interface│ │ │ │ Winners │ │ │ │
│ │Connected│ │ Tasks │ │Priority │ │ Adapters │ │
│ │Hardware │ │ │ │ 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 1: Run coordinator
dimos run coordinator-mock # Single 7-DOF mock arm
dimos run coordinator-dual-mock # Dual arms (7+6 DOF)
dimos run coordinator-piper-xarm # Real hardware

# Terminal 2: Control via CLI
python -m dimos.manipulation.control.orchestrator_client
python -m dimos.manipulation.control.coordinator_client
```

## Core Concepts
Expand All @@ -42,17 +42,17 @@ Single deterministic loop at 100Hz:
2. **Compute** - Each task calculates desired output
3. **Arbitrate** - Per-joint, highest priority wins
4. **Route** - Group commands by hardware
5. **Write** - Send commands to backends
5. **Write** - Send commands to adapters

### Tasks (Controllers)
Tasks are passive controllers called by the orchestrator:
Tasks are passive controllers called by the coordinator:

```python
class MyController:
def claim(self) -> ResourceClaim:
return ResourceClaim(joints={"joint1", "joint2"}, priority=10)

def compute(self, state: OrchestratorState) -> JointCommandOutput:
def compute(self, state: CoordinatorState) -> JointCommandOutput:
# Your control law here (PID, impedance, etc.)
return JointCommandOutput(
joint_names=["joint1", "joint2"],
Expand Down Expand Up @@ -83,10 +83,11 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:

```
dimos/control/
├── orchestrator.py # Module + RPC interface
├── coordinator.py # Module + RPC interface
├── tick_loop.py # 100Hz control loop
├── task.py # ControlTask protocol + types
├── hardware_interface.py # Backend wrapper
├── hardware_interface.py # ConnectedHardware wrapper
├── components.py # HardwareComponent config + type aliases
├── blueprints.py # Pre-configured setups
└── tasks/
└── trajectory_task.py # Joint trajectory controller
Expand All @@ -95,13 +96,25 @@ dimos/control/
## Configuration

```python
from dimos.control import control_orchestrator, HardwareConfig, TaskConfig
from dimos.control import control_coordinator, HardwareComponent, TaskConfig

my_robot = control_orchestrator(
my_robot = control_coordinator(
tick_rate=100.0,
hardware=[
HardwareConfig(id="left", type="xarm", dof=7, joint_prefix="left", ip="192.168.1.100"),
HardwareConfig(id="right", type="piper", dof=6, joint_prefix="right", can_port="can0"),
HardwareComponent(
hardware_id="left_arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("left_arm", 7),
adapter_type="xarm",
address="192.168.1.100",
),
HardwareComponent(
hardware_id="right_arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("right_arm", 6),
adapter_type="piper",
address="can0",
),
],
tasks=[
TaskConfig(name="traj_left", type="trajectory", joint_names=[...], priority=10),
Expand Down Expand Up @@ -181,15 +194,15 @@ class PIDController:

## Joint State Output

The orchestrator publishes one aggregated `JointState` message containing all joints:
The coordinator publishes one aggregated `JointState` message containing all joints:

```python
JointState(
name=["left_joint1", ..., "right_joint1", ...], # All joints
name=["left_arm_joint1", ..., "right_arm_joint1", ...], # All joints
position=[...],
velocity=[...],
effort=[...],
)
```

Subscribe via: `/orchestrator/joint_state`
Subscribe via: `/coordinator/joint_state`
20 changes: 8 additions & 12 deletions dimos/control/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
per-driver/per-controller loops with a single deterministic tick-based system.

Features:
- Single tick loop (read compute arbitrate route write)
- Single tick loop (read -> compute -> arbitrate -> route -> write)
- Per-joint arbitration (highest priority wins)
- Mode conflict detection
- Partial command support (hold last value)
Expand All @@ -27,15 +27,15 @@
Example:
>>> from dimos.control import ControlCoordinator
>>> from dimos.control.tasks import JointTrajectoryTask, JointTrajectoryTaskConfig
>>> from dimos.hardware.manipulators.xarm import XArmBackend
>>> from dimos.hardware.manipulators.xarm import XArmAdapter
>>>
>>> # Create coordinator
>>> coord = ControlCoordinator(tick_rate=100.0)
>>>
>>> # Add hardware
>>> backend = XArmBackend(ip="192.168.1.185", dof=7)
>>> backend.connect()
>>> coord.add_hardware("left_arm", backend)
>>> adapter = XArmAdapter(ip="192.168.1.185", dof=7)
>>> adapter.connect()
>>> coord.add_hardware("left_arm", adapter)
>>>
>>> # Add task
>>> joints = [f"left_arm_joint{i+1}" for i in range(7)]
Expand Down Expand Up @@ -63,10 +63,7 @@
TaskConfig,
control_coordinator,
)
from dimos.control.hardware_interface import (
BackendHardwareInterface,
HardwareInterface,
)
from dimos.control.hardware_interface import ConnectedHardware
from dimos.control.task import (
ControlMode,
ControlTask,
Expand All @@ -78,8 +75,8 @@
from dimos.control.tick_loop import TickLoop

__all__ = [
# Hardware interface
"BackendHardwareInterface",
# Connected hardware
"ConnectedHardware",
# Coordinator
"ControlCoordinator",
"ControlCoordinatorConfig",
Expand All @@ -89,7 +86,6 @@
"CoordinatorState",
"HardwareComponent",
"HardwareId",
"HardwareInterface",
"HardwareType",
"JointCommandOutput",
"JointName",
Expand Down
22 changes: 11 additions & 11 deletions dimos/control/blueprints.py
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]:
hardware_id="arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("arm", 7),
backend_type="mock",
adapter_type="mock",
),
],
tasks=[
Expand All @@ -93,7 +93,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]:
hardware_id="arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("arm", 7),
backend_type="xarm",
adapter_type="xarm",
address="192.168.2.235", # Default IP, override via env or config
auto_enable=True,
),
Expand Down Expand Up @@ -122,7 +122,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]:
hardware_id="arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("arm", 6),
backend_type="xarm",
adapter_type="xarm",
address="192.168.1.210",
auto_enable=True,
),
Expand Down Expand Up @@ -151,7 +151,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]:
hardware_id="arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("arm", 6),
backend_type="piper",
adapter_type="piper",
address="can0",
auto_enable=True,
),
Expand Down Expand Up @@ -184,13 +184,13 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]:
hardware_id="left_arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("left_arm", 7),
backend_type="mock",
adapter_type="mock",
),
HardwareComponent(
hardware_id="right_arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("right_arm", 6),
backend_type="mock",
adapter_type="mock",
),
],
tasks=[
Expand Down Expand Up @@ -223,15 +223,15 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]:
hardware_id="left_arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("left_arm", 7),
backend_type="xarm",
adapter_type="xarm",
address="192.168.2.235",
auto_enable=True,
),
HardwareComponent(
hardware_id="right_arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("right_arm", 6),
backend_type="xarm",
adapter_type="xarm",
address="192.168.1.210",
auto_enable=True,
),
Expand Down Expand Up @@ -266,15 +266,15 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]:
hardware_id="xarm_arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("xarm_arm", 6),
backend_type="xarm",
adapter_type="xarm",
address="192.168.1.210",
auto_enable=True,
),
HardwareComponent(
hardware_id="piper_arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("piper_arm", 6),
backend_type="piper",
adapter_type="piper",
address="can0",
auto_enable=True,
),
Expand Down Expand Up @@ -313,7 +313,7 @@ def _joint_names(hardware_id: str, dof: int) -> list[str]:
hardware_id="arm",
hardware_type=HardwareType.MANIPULATOR,
joints=make_joints("arm", 7),
backend_type="mock",
adapter_type="mock",
),
],
tasks=[
Expand Down
8 changes: 5 additions & 3 deletions dimos/control/components.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,14 @@
# See the License for the specific language governing permissions and
# limitations under the License.

"""Hardware component schema for the ControlCoordnator."""
"""Hardware component schema for the ControlCoordinator."""

from dataclasses import dataclass, field
from enum import Enum

HardwareId = str
JointName = str
TaskName = str


class HardwareType(Enum):
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where is this used and what is difference between manipulator and gripper.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Grippers are parallel jaw grippers, dextrous hands etc. manipulators are just the 6-dof arms.

I have kept the current xarm gripper integrated with the manipulator for now. Can be separated later.

Expand All @@ -44,15 +45,15 @@ class HardwareComponent:
hardware_id: Unique identifier, also used as joint name prefix
hardware_type: Type of hardware (MANIPULATOR, BASE, GRIPPER)
joints: List of joint names (e.g., ["arm_joint1", "arm_joint2", ...])
backend_type: Backend type ("mock", "xarm", "piper")
adapter_type: Adapter type ("mock", "xarm", "piper")
address: Connection address - IP for TCP, port for CAN
auto_enable: Whether to auto-enable servos
"""

hardware_id: HardwareId
hardware_type: HardwareType
joints: list[JointName] = field(default_factory=list)
backend_type: str = "mock"
adapter_type: str = "mock"
address: str | None = None
auto_enable: bool = True

Expand All @@ -76,5 +77,6 @@ def make_joints(hardware_id: HardwareId, dof: int) -> list[JointName]:
"HardwareType",
"JointName",
"JointState",
"TaskName",
"make_joints",
]
Loading
Loading