Control per instance addressing

dimos/control/README.md

@@ -222,17 +222,18 @@ back to the caller. `describe_task()` reports those signatures live.
 |------|---------------|---------|
 | `claim_overlap` | the message names a joint the task currently claims | `joint_command` |
 | `broadcast` | always, to every task on the port | `teleop_buttons`, `gripper_command` |
-| `direct` | always, but the port is meant for one task (a second logs a warning) | `path`, `speed` |
-| `by_task_name` | `msg.frame_id == task.name` | `coordinator_cartesian_command`, `coordinator_ee_twist_command` |
+| `direct` | always, but the port is meant for one task (a second logs a warning) | `path`, `speed`, `cartesian_command`, `ee_twist_command` |
 
-`by_task_name` uses `frame_id` as an address rather than a coordinate frame. It
-is legacy, slated for replacement by a targeted message; don't add new bindings.
+Addressing is topology: which task gets a message is decided by which port it
+reads (wired once at startup), never by message content. Multi-instance
+deployments give each instance its own port via `stream_bind`.
 
 ## Deployment I/O
 
 `ControlCoordinator` declares the shared streams (`joint_command`,
-`twist_command`, `teleop_buttons`, `gripper_command`, cartesian and EEF twist).
-A deployment needing more subclasses it and annotates the extra ports:
+`twist_command`, `teleop_buttons`, `gripper_command`). Per-instance inputs —
+`cartesian_command`, `ee_twist_command` — are deployment I/O: a deployment
+subclasses the coordinator and annotates one port per consuming task instance:
 
 ```python
 class _Go2Coordinator(PathFollowingCoordinator):
@@ -249,10 +250,13 @@ registry, since a subclass can declare ports the registry never sees. A missing
 port fails `add_task()` with the annotation to add, before any route registers.
 
 `TaskConfig.stream_bind` remaps a card input per instance, so two tasks of the
-same type can read different ports (task-level remapping, ROS sense):
+same type can read different ports (task-level remapping, ROS sense). The
+dual-arm teleop coordinator declares `left_cartesian` / `right_cartesian` and
+binds each arm's `teleop_ik` task to its side:
 
 ```python
-TaskConfig(name="left", type="path_follower", stream_bind={"path": "left_path"})
+TaskConfig(name="teleop_xarm", type="teleop_ik",
+           stream_bind={"cartesian_command": "left_cartesian"})
 ```
 
 ## Joint State Views

dimos/control/_control_test_helpers.py

@@ -40,6 +40,7 @@ def __init__(self, name: str, joints: frozenset[str] = frozenset()) -> None:
         self.cartesian_calls: list[tuple[Any, float]] = []
         self.ee_twist_calls: list[tuple[Any, float]] = []
         self.buttons_calls: list[Any] = []
+        self.gripper_calls: list[tuple[Any, float]] = []
 
     def claim(self) -> ResourceClaim:
         return ResourceClaim(joints=self._joints)
@@ -61,6 +62,10 @@ def on_ee_twist_command(self, twist: Any, t_now: float) -> bool:
         self.ee_twist_calls.append((twist, t_now))
         return True
 
+    def on_gripper_command(self, msg: Any, t_now: float) -> bool:
+        self.gripper_calls.append((msg, t_now))
+        return True
+
     def on_buttons(self, msg: Any) -> bool:
         self.buttons_calls.append(msg)
         return True

dimos/control/coordinator.py

@@ -64,9 +64,7 @@
 )
 from dimos.hardware.manipulators.spec import ManipulatorAdapter
 from dimos.hardware.whole_body.spec import WholeBodyAdapter
-from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.geometry_msgs.Twist import Twist
-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
@@ -164,14 +162,6 @@ class ControlCoordinator(Module):
     # Input: Streaming joint commands for real-time control
     joint_command: In[JointState]
 
-    # Input: Streaming cartesian commands for CartesianIKTask
-    # Uses frame_id as task name for routing
-    coordinator_cartesian_command: In[PoseStamped]
-
-    # Input: Routed spatial EEF twist commands for EEFTwistTask.
-    # Uses frame_id as task name for routing.
-    coordinator_ee_twist_command: In[TwistStamped]
-
     # Input: Streaming twist commands for velocity-commanded platforms
     twist_command: In[Twist]
 
@@ -199,9 +189,9 @@ def __init__(self, *args: Any, **kwargs: Any) -> None:
         self._trajectory_task: JointTrajectoryTask | None = None
 
         # Card-declared stream routes, keyed by the port stream_bind resolved to:
-        # port -> (task, handler, routing). Guarded by _task_lock; entries are
-        # added/pruned with their task.
-        self._routes: dict[str, list[tuple[ControlTask, str, Routing]]] = {}
+        # port -> (task, bound handler, routing). Guarded by _task_lock; entries
+        # are added/pruned with their task.
+        self._routes: dict[str, list[tuple[ControlTask, Callable[[Any, float], Any], Routing]]] = {}
 
         # Card-declared command names per task, keyed by task name.
         # Guarded by _task_lock; added/pruned with their task.
@@ -238,9 +228,7 @@ def _setup_from_config(self) -> None:
                 if self.add_task(task, task_type=task_cfg.type, stream_bind=task_cfg.stream_bind):
                     tasks_added.append(task.name)
                 if task_cfg.auto_start:
-                    start = getattr(task, "start", None)
-                    if callable(start):
-                        start()
+                    self.task_invoke(task.name, "start")
 
         except Exception:
             # Roll back everything this call added, tasks first: an active task
@@ -576,6 +564,12 @@ def _register_routes(
 
         for binding in bindings.consumes:
             port = ports[binding.stream]
+            handler = getattr(task, binding.handler, None)
+            if not callable(handler):
+                raise TypeError(
+                    f"{where}: card binds stream {binding.stream!r} to handler "
+                    f"{binding.handler!r}, but the task has no callable {binding.handler!r}"
+                )
             if binding.routing is Routing.DIRECT:
                 sharing = [t.name for t, _h, r in self._routes.get(port, ()) if r is Routing.DIRECT]
                 if sharing:
@@ -586,7 +580,7 @@ def _register_routes(
                         task_name=task.name,
                         also_bound=sharing,
                     )
-            self._routes.setdefault(port, []).append((task, binding.handler, binding.routing))
+            self._routes.setdefault(port, []).append((task, handler, binding.routing))
 
     def _commands_for(self, task_type: str) -> frozenset[str]:
         """The command names the task type declares in its TASK_EXPOSES card."""
@@ -667,7 +661,7 @@ def get_active_tasks(self) -> list[str]:
     def _dispatch(self, stream: str, msg: Any) -> None:
         """Deliver a stream message to its card-routed tasks per each entry's routing rule.
 
-        BROADCAST and DIRECT are ungated, so only the other two rules appear below.
+        BROADCAST and DIRECT are ungated, so only CLAIM_OVERLAP appears below.
         """
         t_now = time.perf_counter()
         with self._task_lock:
@@ -676,39 +670,23 @@ def _dispatch(self, stream: str, msg: Any) -> None:
                 return
 
             claimable: set[str] | None = None
-            frame_id = getattr(msg, "frame_id", "")
-            by_name_bound = False
-            by_name_matched = False
 
-            for task, handler_name, routing in entries:
+            for task, handler, routing in entries:
                 if routing is Routing.CLAIM_OVERLAP:
                     if claimable is None:
                         claimable = set(getattr(msg, "name", ()) or ())
                     if not claimable or not (task.claim().joints & claimable):
                         continue
-                elif routing is Routing.BY_TASK_NAME:
-                    by_name_bound = True
-                    if not frame_id or task.name != frame_id:
-                        continue
-                    by_name_matched = True
                 try:
-                    getattr(task, handler_name)(msg, t_now)
+                    handler(msg, t_now)
                 except Exception:
                     logger.exception(
                         "Stream handler raised on task",
-                        handler=handler_name,
+                        handler=handler.__name__,
                         task_name=task.name,
                         stream=stream,
                     )
 
-            if by_name_bound and not by_name_matched:
-                if not frame_id:
-                    logger.warning("Stream message with empty frame_id (task name)", stream=stream)
-                else:
-                    logger.warning(
-                        "Stream message for unknown task", stream=stream, task_name=frame_id
-                    )
-
     def _map_twist_to_base_joints(self, msg: Twist) -> None:
         """Map Twist onto BASE virtual joints (base/vx ← linear.x, ...) via joint_command."""
         names: list[str] = []

dimos/control/examples/cartesian_ik_jogger.py

@@ -14,8 +14,8 @@
 
 """Pygame-based cartesian jogger for CartesianIKTask.
 
-Publishes PoseStamped commands to the coordinator via LCM.
-The frame_id is used as the task name for routing.
+Publishes PoseStamped commands to the coordinator's cartesian_command
+port via LCM.
 
 Keyboard controls for jogging robot end-effector in world frame:
     W/S: +X/-X (forward/backward)
@@ -123,12 +123,8 @@ def from_fk(
             yaw=float(rpy[2]),
         )
 
-    def to_pose_stamped(self, task_name: str) -> Any:
-        """Convert to PoseStamped for LCM publishing.
-
-        Args:
-            task_name: Task name to use as frame_id for routing
-        """
+    def to_pose_stamped(self) -> Any:
+        """Convert to PoseStamped for LCM publishing."""
         from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
         from dimos.msgs.geometry_msgs.Quaternion import Quaternion
         from dimos.msgs.geometry_msgs.Vector3 import Vector3
@@ -138,7 +134,6 @@ def to_pose_stamped(self, task_name: str) -> Any:
 
         return PoseStamped(
             ts=time.time(),
-            frame_id=task_name,  # Used for task routing
             position=position,
             orientation=orientation,
         )
@@ -153,9 +148,6 @@ def to_pose_stamped(self, task_name: str) -> Any:
 Y_LIMITS = (-0.5, 0.5)
 Z_LIMITS = (-0.2, 0.6)
 
-# Task name for routing (must match blueprint config)
-TASK_NAME = "cartesian_ik_arm"
-
 
 def clamp(value: float, min_val: float, max_val: float) -> float:
     return max(min_val, min(max_val, value))
@@ -188,13 +180,11 @@ def run_jogger_ui(model_path: str | None = None, ee_joint_id: int = 6) -> None:
         model_path = _get_piper_model_path()
 
     print("Starting Cartesian IK Jogger UI...")
-    print("Publishing to /coordinator_cartesian_command")
+    print("Publishing to /cartesian_command")
     print("(Coordinator must be running separately to receive commands)")
 
     # Create LCM publisher for sending cartesian commands
-    transport: LCMTransport[PoseStamped] = LCMTransport(
-        "/coordinator_cartesian_command", PoseStamped
-    )
+    transport: LCMTransport[PoseStamped] = LCMTransport("/cartesian_command", PoseStamped)
 
     # Initialize pygame
     pygame.init()
@@ -209,7 +199,7 @@ def run_jogger_ui(model_path: str | None = None, ee_joint_id: int = 6) -> None:
     current_pose = home_pose.copy()
 
     # Send initial pose via LCM
-    transport.publish(current_pose.to_pose_stamped(TASK_NAME))
+    transport.publish(current_pose.to_pose_stamped())
 
     running = True
     last_time = time.perf_counter()
@@ -276,8 +266,8 @@ def run_jogger_ui(model_path: str | None = None, ee_joint_id: int = 6) -> None:
         current_pose.y = clamp(current_pose.y, *Y_LIMITS)
         current_pose.z = clamp(current_pose.z, *Z_LIMITS)
 
-        # Publish pose via LCM (frame_id = task name for routing)
-        transport.publish(current_pose.to_pose_stamped(TASK_NAME))
+        # Publish pose via LCM
+        transport.publish(current_pose.to_pose_stamped())
 
         # Draw UI
         screen.fill((30, 30, 30))

dimos/control/routing.py

@@ -29,7 +29,6 @@ class Routing(str, Enum):
     """How the coordinator matches an input message to a consuming task."""
 
     CLAIM_OVERLAP = "claim_overlap"  # deliver when msg names joints the task claims
-    BY_TASK_NAME = "by_task_name"  # deliver when msg.frame_id == task.name
     BROADCAST = "broadcast"  # deliver to every task consuming this stream
     DIRECT = "direct"  # like broadcast, but the port is meant to have one task on it
 

dimos/control/tasks/cartesian_ik_task/_registry.py

@@ -18,6 +18,10 @@
 
 TASK_CONSUMES = {
     "cartesian_ik": {
-        "coordinator_cartesian_command": ("on_cartesian_command", "by_task_name"),
+        "cartesian_command": ("on_cartesian_command", "direct"),
     },
 }
+
+TASK_EXPOSES = {
+    "cartesian_ik": ["start"],
+}

dimos/control/tasks/eef_twist_task/_registry.py

@@ -18,7 +18,7 @@
 
 TASK_CONSUMES = {
     "eef_twist": {
-        "coordinator_ee_twist_command": ("on_ee_twist_command", "by_task_name"),
+        "ee_twist_command": ("on_ee_twist_command", "direct"),
         "gripper_command": ("on_gripper_command", "broadcast"),
     },
 }

dimos/control/tasks/g1_groot_wbc_task/_registry.py

@@ -21,5 +21,5 @@
 }
 
 TASK_EXPOSES: dict[str, list[str]] = {
-    "g1_groot_wbc": ["arm", "disarm", "set_dry_run", "reset_runtime_state"],
+    "g1_groot_wbc": ["arm", "disarm", "set_dry_run", "reset_runtime_state", "start"],
 }

dimos/control/tasks/servo_task/_registry.py

@@ -19,3 +19,7 @@
 TASK_CONSUMES = {
     "servo": {"joint_command": ("on_joint_command", "claim_overlap")},
 }
+
+TASK_EXPOSES = {
+    "servo": ["start"],
+}

dimos/control/tasks/teleop_task/_registry.py

@@ -18,7 +18,7 @@
 
 TASK_CONSUMES = {
     "teleop_ik": {
-        "coordinator_cartesian_command": ("on_cartesian_command", "by_task_name"),
+        "cartesian_command": ("on_cartesian_command", "direct"),
         "teleop_buttons": ("on_teleop_buttons", "broadcast"),
     },
 }

dimos/control/tasks/test_registry.py

@@ -144,29 +144,33 @@ def test_seeded_cards_load_into_registry() -> None:
     assert servo.consumes == (
         StreamBinding("joint_command", "on_joint_command", Routing.CLAIM_OVERLAP),
     )
+    assert servo.exposes == frozenset({"start"})
     velocity = control_task_registry.bindings_for("velocity")
     assert velocity.consumes == (
         StreamBinding("joint_command", "on_joint_command", Routing.CLAIM_OVERLAP),
     )
+    assert velocity.exposes == frozenset({"start"})
     cartesian = control_task_registry.bindings_for("cartesian_ik")
     assert cartesian.consumes == (
-        StreamBinding(
-            "coordinator_cartesian_command", "on_cartesian_command", Routing.BY_TASK_NAME
-        ),
+        StreamBinding("cartesian_command", "on_cartesian_command", Routing.DIRECT),
     )
+    assert cartesian.exposes == frozenset({"start"})
     teleop = control_task_registry.bindings_for("teleop_ik")
     assert teleop.consumes == (
-        StreamBinding(
-            "coordinator_cartesian_command", "on_cartesian_command", Routing.BY_TASK_NAME
-        ),
+        StreamBinding("cartesian_command", "on_cartesian_command", Routing.DIRECT),
         StreamBinding("teleop_buttons", "on_teleop_buttons", Routing.BROADCAST),
     )
+    eef_twist = control_task_registry.bindings_for("eef_twist")
+    assert eef_twist.consumes == (
+        StreamBinding("ee_twist_command", "on_ee_twist_command", Routing.DIRECT),
+        StreamBinding("gripper_command", "on_gripper_command", Routing.BROADCAST),
+    )
     trajectory = control_task_registry.bindings_for("trajectory")
     assert trajectory.consumes == ()  # command-driven only
     assert trajectory.exposes == frozenset({"execute", "cancel", "get_state"})
     g1 = control_task_registry.bindings_for("g1_groot_wbc")
     assert g1.consumes == (StreamBinding("twist_command", "on_twist_command", Routing.BROADCAST),)
-    assert g1.exposes == frozenset({"arm", "disarm", "set_dry_run", "reset_runtime_state"})
+    assert g1.exposes == frozenset({"arm", "disarm", "set_dry_run", "reset_runtime_state", "start"})
 
 
 def _scannable_task_classes(task_type: str) -> list[type] | None:

dimos/control/tasks/velocity_task/_registry.py

@@ -19,3 +19,7 @@
 TASK_CONSUMES = {
     "velocity": {"joint_command": ("on_joint_command", "claim_overlap")},
 }
+
+TASK_EXPOSES = {
+    "velocity": ["start"],
+}