feat: add stateful control task API

dimos/control/README.md

@@ -48,7 +48,9 @@ Single deterministic loop at 100Hz:
 Tasks are passive controllers called by the coordinator:
 
 ```python
-class MyController:
+from dimos.control.task import BaseControlTask
+
+class MyController(BaseControlTask):
     def claim(self) -> ResourceClaim:
         return ResourceClaim(joints={"joint1", "joint2"}, priority=10)
 
@@ -61,14 +63,23 @@ class MyController:
         )
 ```
 
+Registering a `BaseControlTask` grants it a frozen, stateless
+`ControlTaskContext` for the lifetime of that registration. A command handler
+can transparently call `self.context.get_state()` to read the coordinator's
+latest complete tick observation. Removing the task revokes access; stopping
+and restarting the coordinator preserves it. The read returns `None` before
+the first tick and after stop or runtime reset; it does not perform a hardware
+read or a freshness check. `CoordinatorState`, `JointStateSnapshot`, and their
+mappings are read-only.
+
 ### 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
+joint_trajectory (priority=10) wants joint1 = 0.5
+safety           (priority=100) wants joint1 = 0.0
                               ↓
-                    safety wins, traj_arm preempted
+                    safety wins, joint_trajectory preempted
 ```
 
 ### Preemption
@@ -118,7 +129,7 @@ my_robot = ControlCoordinator.blueprint(
     ],
     tasks=[
         TaskConfig(
-            name="trajectory",
+            name="joint_trajectory",
             type="trajectory",
             joint_names=[...],  # union of both arms
             priority=10,
@@ -136,8 +147,24 @@ my_robot = ControlCoordinator.blueprint(
 | `list_joints()` | List all joint names |
 | `list_tasks()` | List task names |
 | `get_joint_positions()` | Get current positions |
-| `execute_trajectory(traj)` | Execute through the sole trajectory task |
-| `cancel_trajectory()` | Cancel the sole trajectory task |
+| `task_invoke(task, command, kwargs)` | Invoke a command declared by a task card |
+
+Trajectory execution is an optional task capability, not a coordinator RPC.
+The trajectory package enforces the canonical task name `joint_trajectory`:
+
+```python
+result = coordinator.task_invoke(
+    "joint_trajectory",
+    "execute",
+    {"trajectory": trajectory},
+)
+coordinator.task_invoke("joint_trajectory", "cancel")
+```
+
+`TASK_EXPOSES` in a task package's `_registry.py` is a strict remote-command
+allowlist. Missing tasks raise an error, undeclared commands raise
+`AttributeError`, invalid arguments raise `TypeError` before the handler runs,
+and handler results or exceptions pass through unchanged.
 
 ## Control Modes
 
@@ -152,9 +179,9 @@ Tasks output commands in one of three modes:
 ## Writing a Custom Task
 
 ```python
-from dimos.control.task import ControlTask, ResourceClaim, JointCommandOutput, ControlMode
+from dimos.control.task import BaseControlTask, ResourceClaim, JointCommandOutput, ControlMode
 
-class PIDController:
+class PIDController(BaseControlTask):
     def __init__(self, joints: list[str], priority: int = 10):
         self._name = "pid_controller"
         self._claim = ResourceClaim(joints=frozenset(joints), priority=priority)

dimos/control/blueprints/mobile.py

@@ -199,7 +199,7 @@ def _flowbase_twist_base(
     hardware=[_mock_arm_hw, _mock_twist_base()],
     tasks=[
         TaskConfig(
-            name="traj_arm",
+            name="joint_trajectory",
             type="trajectory",
             joint_names=_mock_arm_hw.joints,
             priority=10,

dimos/control/coordinator.py

@@ -47,13 +47,12 @@
     ConnectedWholeBody,
 )
 from dimos.control.routing import Routing
-from dimos.control.task import ControlTask
-from dimos.control.tasks.trajectory_task.trajectory_task import (
-    JointTrajectoryTask,
-    TrajectoryCancellationResult,
-    TrajectoryCancellationStatus,
-    TrajectoryExecutionResult,
-    TrajectoryExecutionStatus,
+from dimos.control.task import (
+    BaseControlTask,
+    ControlTask,
+    ControlTaskContext,
+    CoordinatorState,
+    _TaskRegistration,
 )
 from dimos.control.tick_loop import TickLoop
 from dimos.core.core import rpc
@@ -69,7 +68,6 @@
 from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped
 from dimos.msgs.sensor_msgs.JointState import JointState
 from dimos.msgs.std_msgs.Bool import Bool
-from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory
 from dimos.teleop.quest.quest_types import (
     Buttons,
 )
@@ -147,7 +145,7 @@ class ControlCoordinator(Module):
         ...     ],
         ...     tasks=[
         ...         TaskConfig(
-        ...             name="traj_arm",
+        ...             name="joint_trajectory",
         ...             type="trajectory",
         ...             joint_names=make_joints("arm", 7),
         ...             priority=10,
@@ -195,8 +193,15 @@ def __init__(self, *args: Any, **kwargs: Any) -> None:
 
         # Registered tasks
         self._tasks: dict[TaskName, ControlTask] = {}
+        self._task_registrations: dict[TaskName, _TaskRegistration] = {}
         self._task_lock = threading.Lock()
-        self._trajectory_task: JointTrajectoryTask | None = None
+
+        # The tick loop atomically replaces the latest complete observation.
+        # This lock is deliberately independent from task dispatch so task
+        # commands can read state without coupling tick and command progress.
+        self._latest_state: CoordinatorState | None = None
+        self._state_lock = threading.Lock()
+        self._task_context = ControlTaskContext(get_state=self._get_latest_state)
 
         # Card-declared stream routes, keyed by the port stream_bind resolved to:
         # port -> (task, handler, routing). Guarded by _task_lock; entries are
@@ -223,6 +228,21 @@ def __init__(self, *args: Any, **kwargs: Any) -> None:
 
         logger.info(f"ControlCoordinator initialized at {self.config.tick_rate}Hz")
 
+    def _get_latest_state(self) -> CoordinatorState | None:
+        """Return the latest complete tick observation, if a tick has run."""
+        with self._state_lock:
+            return self._latest_state
+
+    def _set_latest_state(self, state: CoordinatorState) -> None:
+        """Atomically publish one complete tick observation."""
+        with self._state_lock:
+            self._latest_state = state
+
+    def _clear_latest_state(self) -> None:
+        """Clear observations across runtime discontinuities."""
+        with self._state_lock:
+            self._latest_state = None
+
     def _setup_from_config(self) -> None:
         """Create hardware and tasks from config (called on start)."""
         hardware_added: list[str] = []
@@ -458,29 +478,6 @@ def get_joint_positions(self) -> dict[str, float]:
                     positions[joint_name] = joint_state.position
             return positions
 
-    @rpc
-    def execute_trajectory(self, trajectory: JointTrajectory) -> TrajectoryExecutionResult:
-        """Execute a trajectory through the coordinator's sole trajectory task."""
-        current_positions = self.get_joint_positions()
-        with self._task_lock:
-            if self._trajectory_task is None:
-                return TrajectoryExecutionResult(
-                    TrajectoryExecutionStatus.NO_TRAJECTORY_TASK,
-                    "Control coordinator has no trajectory task",
-                )
-            return self._trajectory_task.execute(trajectory, current_positions)
-
-    @rpc
-    def cancel_trajectory(self) -> TrajectoryCancellationResult:
-        """Cancel the coordinator's sole trajectory task."""
-        with self._task_lock:
-            if self._trajectory_task is None:
-                return TrajectoryCancellationResult(
-                    TrajectoryCancellationStatus.NO_TRAJECTORY_TASK,
-                    "Control coordinator has no trajectory task",
-                )
-            return self._trajectory_task.cancel()
-
     @rpc
     def add_task(
         self,
@@ -504,21 +501,26 @@ def add_task(
             if task.name in self._tasks:
                 logger.warning(f"Task {task.name} already registered")
                 return False
-            if isinstance(task, JointTrajectoryTask):
-                if self._trajectory_task is not None:
-                    raise ValueError("ControlCoordinator supports exactly one JointTrajectoryTask")
-                self._trajectory_task = task
+            registration = (
+                _TaskRegistration.acquire(task, self._task_context)
+                if isinstance(task, BaseControlTask)
+                else None
+            )
             if task_type is not None:
                 try:
                     self._register_routes(task, task_type, stream_bind)
                     self._task_commands[task.name] = self._commands_for(task_type)
                 except Exception:
-                    if task is self._trajectory_task:
-                        self._trajectory_task = None
+                    for entries in self._routes.values():
+                        entries[:] = [entry for entry in entries if entry[0] is not task]
+                    self._task_commands.pop(task.name, None)
+                    registration = None
                     raise
             else:
                 self._task_commands[task.name] = frozenset()
             self._tasks[task.name] = task
+            if registration is not None:
+                self._task_registrations[task.name] = registration
             logger.info(f"Added task {task.name}")
         self._sync_stream_subscriptions()
         return True
@@ -533,8 +535,7 @@ def remove_task(self, task_name: TaskName) -> bool:
             for entries in self._routes.values():
                 entries[:] = [entry for entry in entries if entry[0] is not task]
             self._task_commands.pop(task_name, None)
-            if task is self._trajectory_task:
-                self._trajectory_task = None
+            self._task_registrations.pop(task_name, None)
             logger.info(f"Removed task {task_name}")
         self._sync_stream_subscriptions()
         return True
@@ -783,6 +784,7 @@ def reset_runtime_state(self, reactivate: bool | None = None) -> dict[str, bool]
         without tearing down the coordinator. The result covers declaring
         tasks only.
         """
+        self._clear_latest_state()
         results: dict[str, bool] = {}
         with self._task_lock:
             for name, task in self._tasks.items():
@@ -803,41 +805,30 @@ def reset_runtime_state(self, reactivate: bool | None = None) -> dict[str, bool]
     def task_invoke(
         self, task_name: TaskName, method: str, kwargs: dict[str, Any] | None = None
     ) -> Any:
-        """Invoke a task command. Pass t_now=None to auto-inject current time.
+        """Invoke a command declared by the task type's ``TASK_EXPOSES`` card.
 
-        Commands declared in the task's TASK_EXPOSES card are validated
-        against the method's own signature before dispatch; a bad kwarg name
-        or missing required argument raises to the caller. Undeclared methods
-        still dispatch exactly as before but log a nudge to declare them.
+        Arguments are validated against the method's own signature before
+        dispatch. Missing tasks, undeclared methods, invalid arguments, and
+        handler exceptions propagate to the caller.
         """
         with self._task_lock:
             task = self._tasks.get(task_name)
             if task is None:
-                logger.warning(f"Task {task_name} not found")
-                return None
+                raise KeyError(f"task_invoke({task_name!r}, {method!r}): task not found")
 
             kwargs = dict(kwargs or {})
+            declared = self._task_commands.get(task_name, frozenset())
+            if method not in declared:
+                raise AttributeError(
+                    f"task_invoke({task_name!r}, {method!r}): command is not exposed; "
+                    f"declared commands: {sorted(declared)}"
+                )
 
             # Auto-inject t_now if requested (None means "use current time")
             if "t_now" in kwargs and kwargs["t_now"] is None:
                 kwargs["t_now"] = time.perf_counter()
 
-            if method in self._task_commands.get(task_name, frozenset()):
-                return self._invoke_declared(task, task_name, method, kwargs)
-
-            if not hasattr(task, method):
-                raise AttributeError(
-                    f"task_invoke({task_name!r}, {method!r}): task has no such method; "
-                    f"declared commands: {sorted(self._task_commands.get(task_name, frozenset()))}"
-                )
-            # TODO: Make TASK_EXPOSES an enforced RPC allowlist after migrating callers
-            # that rely on reflective access to undeclared task methods.
-            logger.warning(
-                "undeclared task_invoke; declare it in TASK_EXPOSES",
-                task_name=task_name,
-                method=method,
-            )
-            return getattr(task, method)(**kwargs)
+            return self._invoke_declared(task, task_name, method, kwargs)
 
     def _invoke_declared(
         self, task: ControlTask, task_name: TaskName, method: str, kwargs: dict[str, Any]
@@ -968,6 +959,7 @@ def start(self) -> None:
             tasks=self._tasks,
             task_lock=self._task_lock,
             joint_to_hardware=self._joint_to_hardware,
+            observation_callback=self._set_latest_state,
             publish_callback=publish_cb,
             publish_robot_callback=publish_robot_cb,
             frame_id=self.config.joint_state_frame_id,
@@ -997,6 +989,7 @@ def stop(self) -> None:
 
         if self._tick_loop:
             self._tick_loop.stop()
+        self._clear_latest_state()
 
         with self._hardware_lock:
             for hw_id, interface in self._hardware.items():