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

[mustafab0]

1. Single 100Hz tick loop for multi-arm control (read → compute → arbitrate → write)
2. Per-joint priority arbitration across concurrent tasks, allows for multiple controllers to access specific joints
3. Only one file is required to integrate a new arm or robot
4. New ManipulatorBackend protocol separating SDK code from control logic

Added:

• dimos/control/ - orchestrator, tick loop, trajectory task
• dimos/hardware/manipulators/spec.py - backend protocol
• xarm/, piper/, mock/ backend implementations
• 25 unit tests + 5 e2e tests
• Interactive CLI client for testing

Old drivers moved to manipulators_old/

Below is the comparison between the old and the new driver architecture.

[greptile-apps]

Greptile Overview

Greptile Summary

Overview

This PR introduces a unified Control Orchestrator for multi-arm and full-body robot control. The architecture replaces per-driver control loops with a single centralized 100Hz tick loop featuring per-joint priority arbitration. The design separates hardware backends from control logic through the ManipulatorBackend protocol, enabling easy integration of new robots.

Key Changes

New Control Architecture:

• Single deterministic tick loop (read → compute → arbitrate → route → write) at 100Hz
• Per-joint priority-based arbitration allowing multiple controllers to share joints
• Centralized time management (tasks use orchestrator time, not time.time())
• Support for partial joint commands with hold-last-value behavior

Hardware Abstraction:

• ManipulatorBackend protocol for vendor-agnostic hardware interfaces
• Implementations for XArm, Piper, and Mock backends
• BackendHardwareInterface wrapper providing namespaced joint names and partial command support

Task System:

• ControlTask protocol for passive controllers called by orchestrator
• JointTrajectoryTask for trajectory execution
• Preemption notifications when higher-priority tasks take control

New Files:

• Control orchestrator core (orchestrator.py, tick_loop.py, task.py, hardware_interface.py)
• Backend implementations (xarm/backend.py, piper/backend.py, mock/backend.py)
• Pre-configured blueprints for common setups
• Interactive RPC client for trajectory control
• Comprehensive unit and E2E tests

Deprecated:

• Old manipulators architecture moved to manipulators_old/

Critical Issues Found

Logic Errors (Must Fix)

1. Mode mixing bug in tick_loop.py: When routing commands to hardware, if multiple joints on the same hardware interface require different control modes (e.g., one joint needs POSITION, another needs VELOCITY), only the last mode is used for all joints. This will cause incorrect robot behavior.

2. Race condition in orchestrator.py: The execute_trajectory() RPC method reads time.perf_counter() outside the task lock, but the tick loop thread may be concurrently accessing task state. This creates a race condition on the trajectory start time.

3. Incomplete preemption handling: When mode conflicts occur at the same priority level, commands are dropped but tasks aren't notified via on_preempted(), leaving them unaware their commands are being ignored.

4. Piper enable retry bug: The enable loop can succeed on the 100th attempt but still return False due to an off-by-one error in the success check.

Style/Best Practice Issues

5. Lost preemption source: Preemption notifications use hardcoded string instead of tracking which specific task caused the preemption, making debugging harder.

6. Trajectory completion: Final waypoint may not be precisely reached if the tick occurs slightly after trajectory end.

7. TORQUE mode silently dropped: While acceptable (most hardware doesn't support it), this should be better documented.

8. Hardware setup error handling: Partial initialization can occur if enable fails after connection succeeds.

9. Unused variables: Dead code in orchestrator_client.py.

Architecture Assessment

Strengths:

• Clean separation of concerns with Protocol-based design
• Well-thought-out arbitration system for multi-controller scenarios
• Centralized timing prevents common distributed control issues
• Good test coverage (unit + E2E)
• Clear documentation and examples

Concerns:

• The mode mixing bug in routing is critical and will cause runtime failures
• Thread safety needs more attention, especially around task state transitions
• Missing validation that all joints on a hardware interface use the same control mode

Testing

The PR includes solid test coverage:

• Unit tests for core components (arbitration, tasks, hardware interface)
• E2E tests for single and dual-arm scenarios
• Tests for trajectory execution, cancellation, and status querying

However, tests don't cover:

• Mode conflict scenarios
• Concurrent execution edge cases
• The specific race conditions identified

Confidence Score: 2/5

• This PR has critical logic errors in the core control loop that will cause incorrect robot behavior with multiple control modes
• Score of 2 reflects serious issues that must be fixed before merge: (1) Mode mixing bug in tick_loop routing will cause wrong control modes to be applied to joints, (2) Race condition in trajectory execution timing, (3) Incomplete preemption notifications leaving tasks in inconsistent states, (4) Off-by-one error in Piper enable logic. While the architecture is well-designed and most code is solid, these bugs affect core functionality and could cause dangerous robot behavior in production.
• Pay close attention to dimos/control/tick_loop.py (mode routing bug), dimos/control/orchestrator.py (race condition), and dimos/hardware/manipulators/piper/backend.py (enable retry logic)

Important Files Changed

File Analysis

Filename	Score	Overview
dimos/control/tick_loop.py	2/5	Core control loop with critical mode mixing bug in routing, incomplete preemption handling for mode conflicts, and loss of preemption source information
dimos/control/orchestrator.py	3/5	Main orchestrator module with race condition in execute_trajectory time synchronization and error handling issue in hardware setup
dimos/control/hardware_interface.py	3/5	Hardware abstraction with TORQUE mode silently dropped and minor initialization race condition
dimos/control/tasks/trajectory_task.py	4/5	Trajectory task implementation with minor edge case where final position may not be held at completion
dimos/hardware/manipulators/piper/backend.py	3/5	Piper hardware backend with potential infinite loop bug in enable retry logic
dimos/manipulation/control/orchestrator_client.py	4/5	Interactive RPC client with two unused variable issues (syntax errors)

Sequence Diagram

sequenceDiagram
    participant User
    participant Orchestrator
    participant TickLoop
    participant Task1
    participant Task2
    participant HWInterface
    participant Backend
    
    User->>Orchestrator: execute_trajectory(task_name, traj)
    Orchestrator->>Task1: execute(trajectory, t_now)
    Task1-->>Orchestrator: True (accepted)
    
    loop Every 10ms (100Hz)
        TickLoop->>HWInterface: read_state()
        HWInterface->>Backend: read_joint_positions()
        Backend-->>HWInterface: positions
        HWInterface-->>TickLoop: joint states
        
        TickLoop->>Task1: is_active()
        Task1-->>TickLoop: True
        TickLoop->>Task1: claim()
        Task1-->>TickLoop: ResourceClaim(joints, priority=10)
        
        TickLoop->>Task2: is_active()
        Task2-->>TickLoop: True
        TickLoop->>Task2: claim()
        Task2-->>TickLoop: ResourceClaim(joints, priority=100)
        
        TickLoop->>Task1: compute(state)
        Task1-->>TickLoop: JointCommandOutput
        
        TickLoop->>Task2: compute(state)
        Task2-->>TickLoop: JointCommandOutput
        
        Note over TickLoop: Arbitrate per-joint<br/>(higher priority wins)
        
        alt Task2 preempted Task1
            TickLoop->>Task1: on_preempted(by_task, joints)
        end
        
        Note over TickLoop: Route commands<br/>to hardware
        
        TickLoop->>HWInterface: write_command(commands, mode)
        HWInterface->>Backend: write_joint_positions(ordered)
        Backend-->>HWInterface: success
        HWInterface-->>TickLoop: success
        
        TickLoop->>Orchestrator: publish joint_state
    end
    
    User->>Orchestrator: get_trajectory_status(task_name)
    Orchestrator->>Task1: get_state()
    Task1-->>Orchestrator: EXECUTING
    Orchestrator-->>User: status dict

Loading

[spomichter]

@greptile final final review

[greptile-apps]

_{66 files reviewed, 6 comments}

_{Edit Code Review Agent Settings | Greptile}

[spomichter]

Horrible fix ASAP after this PR. No modularity to other robots

[mustafab0]

#1025

[spomichter]

Very bad ASAP move out of this folder why would we have general manipulator utils in xarm backend

[mustafab0]

Will address in the second pass

[spomichter]

Needs to be moved to some orchestrator visualizer class

[spomichter]

Generally bad file. Shell class should use our CLI standard among other things

[mustafab0]

#1029