diff --git a/dimos/control/README.md b/dimos/control/README.md index 66c64b418d..03a3b1a30b 100644 --- a/dimos/control/README.md +++ b/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 diff --git a/dimos/control/_control_test_helpers.py b/dimos/control/_control_test_helpers.py index c0a37ecef2..9bf5bb945c 100644 --- a/dimos/control/_control_test_helpers.py +++ b/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 diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index eeed8dead0..c212a50c75 100644 --- a/dimos/control/coordinator.py +++ b/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] = [] diff --git a/dimos/control/examples/cartesian_ik_jogger.py b/dimos/control/examples/cartesian_ik_jogger.py index 09e30acb0b..8a48c91133 100644 --- a/dimos/control/examples/cartesian_ik_jogger.py +++ b/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)) diff --git a/dimos/control/routing.py b/dimos/control/routing.py index b90800d07b..347dc515f3 100644 --- a/dimos/control/routing.py +++ b/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 diff --git a/dimos/control/tasks/cartesian_ik_task/_registry.py b/dimos/control/tasks/cartesian_ik_task/_registry.py index a8a684af33..eea153bb68 100644 --- a/dimos/control/tasks/cartesian_ik_task/_registry.py +++ b/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"], +} diff --git a/dimos/control/tasks/eef_twist_task/_registry.py b/dimos/control/tasks/eef_twist_task/_registry.py index 72ceb439cc..d02b7bb460 100644 --- a/dimos/control/tasks/eef_twist_task/_registry.py +++ b/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"), }, } diff --git a/dimos/control/tasks/g1_groot_wbc_task/_registry.py b/dimos/control/tasks/g1_groot_wbc_task/_registry.py index 9ac1d91df5..d38545151a 100644 --- a/dimos/control/tasks/g1_groot_wbc_task/_registry.py +++ b/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"], } diff --git a/dimos/control/tasks/servo_task/_registry.py b/dimos/control/tasks/servo_task/_registry.py index 0e5d1c72be..feac45d3f7 100644 --- a/dimos/control/tasks/servo_task/_registry.py +++ b/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"], +} diff --git a/dimos/control/tasks/teleop_task/_registry.py b/dimos/control/tasks/teleop_task/_registry.py index effce16866..cd0c5127c0 100644 --- a/dimos/control/tasks/teleop_task/_registry.py +++ b/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"), }, } diff --git a/dimos/control/tasks/test_registry.py b/dimos/control/tasks/test_registry.py index 9166804da7..fee5cc5e86 100644 --- a/dimos/control/tasks/test_registry.py +++ b/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: diff --git a/dimos/control/tasks/velocity_task/_registry.py b/dimos/control/tasks/velocity_task/_registry.py index b866135d30..4d6c438faa 100644 --- a/dimos/control/tasks/velocity_task/_registry.py +++ b/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"], +} diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index cbd3dd9f48..12c7641c69 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -34,7 +34,6 @@ ) from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.control.hardware_interface import ConnectedHardware, ConnectedTwistBase -from dimos.control.routing import Routing from dimos.control.task import ( BaseControlTask, ControlMode, @@ -50,6 +49,7 @@ TrajectoryExecutionStatus, ) from dimos.control.tick_loop import TickLoop +from dimos.core.stream import In from dimos.hardware.manipulators.spec import ManipulatorAdapter from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped @@ -240,8 +240,10 @@ def make_coordinator() -> Iterator[Callable[..., ControlCoordinator]]: """Factory for real coordinators, all stopped on teardown.""" coordinators: list[ControlCoordinator] = [] - def make(**kwargs: Any) -> ControlCoordinator: - coordinator = ControlCoordinator(publish_joint_state=False, **kwargs) + def make( + cls: type[ControlCoordinator] = ControlCoordinator, **kwargs: Any + ) -> ControlCoordinator: + coordinator = cls(publish_joint_state=False, **kwargs) coordinators.append(coordinator) return coordinator @@ -252,36 +254,19 @@ def make(**kwargs: Any) -> ControlCoordinator: coordinator.stop() -class TestControlCoordinatorLifecycle: - def test_dispatch_routes_ee_twist_only_to_matching_frame_id(self, make_coordinator): - coordinator = make_coordinator() - matching_task = RecordingTask("eef") - other_task = RecordingTask("other") - coordinator._tasks = {"eef": matching_task, "other": other_task} - coordinator._routes = { - "coordinator_ee_twist_command": [ - (matching_task, "on_ee_twist_command", Routing.BY_TASK_NAME), - (other_task, "on_ee_twist_command", Routing.BY_TASK_NAME), - ] - } +class _EEFTwistCoordinator(ControlCoordinator): + ee_twist_command: In[TwistStamped] - for frame_id in ("eef", "missing", ""): - coordinator._dispatch( - "coordinator_ee_twist_command", - TwistStamped(frame_id=frame_id, linear=[0.1, 0.0, 0.0], angular=[0.0, 0.0, 0.0]), - ) - - assert len(matching_task.ee_twist_calls) == 1 - assert other_task.ee_twist_calls == [] +class TestControlCoordinatorLifecycle: def test_start_subscribes_ee_twist_only_for_eef_twist_tasks(self, make_coordinator, mocker): mocker.patch("dimos.core.module.Module.start") mocker.patch("dimos.control.coordinator.TickLoop") def start_coordinator(tasks): - coordinator = make_coordinator(tasks=tasks) + coordinator = make_coordinator(cls=_EEFTwistCoordinator, tasks=tasks) coordinator._create_task_from_config = lambda cfg: RecordingTask(cfg.name) - subscribe = mocker.patch.object(coordinator.coordinator_ee_twist_command, "subscribe") + subscribe = mocker.patch.object(coordinator.ee_twist_command, "subscribe") coordinator.start() return coordinator, subscribe @@ -305,7 +290,7 @@ def start_coordinator(tasks): def test_stop_unsubscribes_ee_twist_subscription(self, make_coordinator, mocker): coordinator = make_coordinator() unsubscribe = mocker.Mock() - coordinator._stream_unsubs = {"coordinator_ee_twist_command": unsubscribe} + coordinator._stream_unsubs = {"ee_twist_command": unsubscribe} coordinator.stop() @@ -352,6 +337,11 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: pass + def on_twist_command(self, msg: Any, t_now: float) -> None: + # The g1_groot_wbc card binds twist_command; add_task now + # resolves handlers at registration, so the stub needs it. + pass + def reset_runtime_state(self, reactivate: bool | None = None) -> bool: self.reset_reactivate_args.append(reactivate) return True diff --git a/dimos/control/test_coordinator_commands.py b/dimos/control/test_coordinator_commands.py index 1750fe16cc..65a71d7379 100644 --- a/dimos/control/test_coordinator_commands.py +++ b/dimos/control/test_coordinator_commands.py @@ -79,6 +79,14 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: pass + # Card-consumed streams: add_task resolves handlers at registration, + # so a stub registered under servo / g1_groot_wbc must carry them. + def on_joint_command(self, msg: Any, t_now: float) -> None: + pass + + def on_twist_command(self, msg: Any, t_now: float) -> None: + pass + # Trajectory commands def execute(self, trajectory: Any) -> bool: self.executed = trajectory diff --git a/dimos/control/test_coordinator_routing.py b/dimos/control/test_coordinator_routing.py index 60733c468a..19d3a5b603 100644 --- a/dimos/control/test_coordinator_routing.py +++ b/dimos/control/test_coordinator_routing.py @@ -15,11 +15,11 @@ """Characterization tests for coordinator input-stream routing. These pin the observable routing behavior of the coordinator's input -streams (joint_command, coordinator_cartesian_command, -coordinator_ee_twist_command, twist_command, teleop_buttons) so the -card-routing refactor can prove it preserves them. They intentionally -avoid coordinator internals: messages enter through the ports' -``subscribe`` seam and effects are observed on the tasks. +streams (joint_command, twist_command, teleop_buttons, plus the +per-instance command ports subclasses declare) so routing refactors can +prove they preserve them. They intentionally avoid coordinator +internals: messages enter through the ports' ``subscribe`` seam and +effects are observed on the tasks. """ from __future__ import annotations @@ -52,10 +52,9 @@ STREAMS = ( "joint_command", - "coordinator_cartesian_command", - "coordinator_ee_twist_command", "twist_command", "teleop_buttons", + "gripper_command", ) @@ -199,78 +198,95 @@ def test_empty_message_routes_to_nobody(self, make_coordinator): assert coordinator.get_task("vel1")._velocities is None -class TestByTaskNameRouting: - @staticmethod - def _cartesian_coordinator(make_coordinator): - coordinator, taps = make_coordinator( - stub_task_types=True, - tasks=[ - TaskConfig(name="cart_a", type="cartesian_ik", joint_names=ARM_JOINTS), - TaskConfig(name="cart_b", type="cartesian_ik", joint_names=ARM_JOINTS), - ], - ) - coordinator.start() - return coordinator, taps +class SingleArmControlCoordinator(ControlCoordinator): + """Single-instance deployment: ports named like the cards' logical inputs.""" - def test_cartesian_delivered_only_to_named_task(self, make_coordinator): - coordinator, taps = self._cartesian_coordinator(make_coordinator) + cartesian_command: In[PoseStamped] + ee_twist_command: In[TwistStamped] - taps["coordinator_cartesian_command"].emit(PoseStamped(frame_id="cart_a")) - cart_a = coordinator.get_task("cart_a") - cart_b = coordinator.get_task("cart_b") - assert len(cart_a.cartesian_calls) == 1 - msg, t_now = cart_a.cartesian_calls[0] - assert msg.frame_id == "cart_a" - assert isinstance(t_now, float) - assert cart_b.cartesian_calls == [] +class DualArmControlCoordinator(ControlCoordinator): + """One cartesian port per arm, as in the dual-arm quest teleop.""" - @pytest.mark.parametrize("frame_id", ["unknown_task", ""]) - def test_cartesian_unmatched_frame_id_delivers_nothing(self, make_coordinator, frame_id): - coordinator, taps = self._cartesian_coordinator(make_coordinator) + left_cartesian: In[PoseStamped] + right_cartesian: In[PoseStamped] - taps["coordinator_cartesian_command"].emit(PoseStamped(frame_id=frame_id)) - assert coordinator.get_task("cart_a").cartesian_calls == [] - assert coordinator.get_task("cart_b").cartesian_calls == [] +class TestPerInstanceCommandRouting: + """Cartesian/EEF-twist commands address tasks by port, not payload.""" - @staticmethod - def _ee_twist_coordinator(make_coordinator): + def test_dual_arm_ports_isolate_left_from_right(self, make_coordinator): coordinator, taps = make_coordinator( + coordinator_cls=DualArmControlCoordinator, stub_task_types=True, tasks=[ - TaskConfig(name="eef_a", type="eef_twist", joint_names=ARM_JOINTS), - TaskConfig(name="eef_b", type="eef_twist", joint_names=ARM_JOINTS), + TaskConfig( + name="teleop_left", + type="teleop_ik", + joint_names=ARM_JOINTS, + stream_bind={"cartesian_command": "left_cartesian"}, + ), + TaskConfig( + name="teleop_right", + type="teleop_ik", + joint_names=ARM_JOINTS, + stream_bind={"cartesian_command": "right_cartesian"}, + ), ], ) coordinator.start() - return coordinator, taps - def test_ee_twist_delivered_only_to_named_task(self, make_coordinator): - coordinator, taps = self._ee_twist_coordinator(make_coordinator) + taps["left_cartesian"].emit(PoseStamped()) + + left = coordinator.get_task("teleop_left") + right = coordinator.get_task("teleop_right") + assert len(left.cartesian_calls) == 1 + assert right.cartesian_calls == [] + + taps["right_cartesian"].emit(PoseStamped()) - taps["coordinator_ee_twist_command"].emit( - TwistStamped(frame_id="eef_a", linear=[0.1, 0.0, 0.0], angular=[0.0, 0.0, 0.0]) + assert len(left.cartesian_calls) == 1 + assert len(right.cartesian_calls) == 1 + + def test_cartesian_name_match_needs_no_stream_bind(self, make_coordinator): + coordinator, taps = make_coordinator( + coordinator_cls=SingleArmControlCoordinator, + stub_task_types=True, + tasks=[TaskConfig(name="cart", type="cartesian_ik", joint_names=ARM_JOINTS)], ) + coordinator.start() - assert len(coordinator.get_task("eef_a").ee_twist_calls) == 1 - assert coordinator.get_task("eef_b").ee_twist_calls == [] + taps["cartesian_command"].emit(PoseStamped()) - @pytest.mark.parametrize("frame_id", ["unknown_task", ""]) - def test_ee_twist_unmatched_frame_id_delivers_nothing(self, make_coordinator, frame_id): - coordinator, taps = self._ee_twist_coordinator(make_coordinator) + calls = coordinator.get_task("cart").cartesian_calls + assert len(calls) == 1 + assert isinstance(calls[0][1], float) - taps["coordinator_ee_twist_command"].emit( - TwistStamped(frame_id=frame_id, linear=[0.1, 0.0, 0.0], angular=[0.0, 0.0, 0.0]) + def test_frame_id_is_not_consulted(self, make_coordinator, mocker): + # Intentional delta from frame_id addressing: frame_id means a + # coordinate frame again — a stale task-name stamp neither routes, + # blocks, nor warns, and the payload reaches the handler untouched. + coordinator, taps = make_coordinator( + coordinator_cls=SingleArmControlCoordinator, + stub_task_types=True, + tasks=[TaskConfig(name="cart", type="cartesian_ik", joint_names=ARM_JOINTS)], ) + coordinator.start() + warn = mocker.patch.object(coord_mod.logger, "warning") + + taps["cartesian_command"].emit(PoseStamped(frame_id="some_other_task")) + taps["cartesian_command"].emit(PoseStamped(frame_id="")) - assert coordinator.get_task("eef_a").ee_twist_calls == [] - assert coordinator.get_task("eef_b").ee_twist_calls == [] + calls = coordinator.get_task("cart").cartesian_calls + assert len(calls) == 2 + assert calls[0][0].frame_id == "some_other_task" + assert not warn.called class TestButtonsRouting: def test_buttons_reach_teleop_task(self, make_coordinator): coordinator, taps = make_coordinator( + coordinator_cls=SingleArmControlCoordinator, stub_task_types=True, tasks=[TaskConfig(name="teleop1", type="teleop_ik", joint_names=ARM_JOINTS)], ) @@ -587,6 +603,7 @@ class TestCardRoutingContract: def test_buttons_skip_card_less_tasks(self, make_coordinator): coordinator, taps = make_coordinator( + coordinator_cls=SingleArmControlCoordinator, stub_task_types=True, tasks=[TaskConfig(name="teleop1", type="teleop_ik", joint_names=ARM_JOINTS)], ) diff --git a/dimos/imitation/collection/test_blueprint.py b/dimos/imitation/collection/test_blueprint.py index 18757c8c1c..ba3b4e31c5 100644 --- a/dimos/imitation/collection/test_blueprint.py +++ b/dimos/imitation/collection/test_blueprint.py @@ -40,7 +40,8 @@ def _joint_streams(blueprint: Blueprint) -> dict[tuple[str, str], str]: def test_recorder_reads_aggregate_joint_state(blueprint: Blueprint) -> None: streams = _joint_streams(blueprint) - # Plain name pairing on both ends, no remap in between. + # Plain name pairing on both ends, no remap in between. The coordinator + # atom carries its explicit instance_name (the RPC lookup contract). assert streams[("collectionrecorder", AGGREGATE)] == AGGREGATE - assert streams[("controlcoordinator", AGGREGATE)] == AGGREGATE + assert streams[("ControlCoordinator", AGGREGATE)] == AGGREGATE assert not [port for _instance, port in streams if port.endswith("_joints")] diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 27040d927e..bfaa8019fd 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -166,7 +166,10 @@ all_modules = { "alfred-high-level": "dimos.robot.diy.alfred.effector_high_level.AlfredHighLevel", "arm-command-module": "dimos.teleop.hosted.arm_command.ArmCommandModule", + "arm-pose-coordinator": "dimos.robot.manipulators.common.coordinators.ArmPoseCoordinator", + "arm-pose-twist-coordinator": "dimos.robot.manipulators.common.coordinators.ArmPoseTwistCoordinator", "arm-teleop-module": "dimos.teleop.quest.quest_extensions.ArmTeleopModule", + "arm-twist-coordinator": "dimos.robot.manipulators.common.coordinators.ArmTwistCoordinator", "b-box-navigation-module": "dimos.navigation.bbox_navigation.BBoxNavigationModule", "b1-connection-module": "dimos.robot.unitree.b1.connection.B1ConnectionModule", "basic-path-follower": "dimos.navigation.basic_path_follower.module.BasicPathFollower", diff --git a/dimos/robot/manipulators/a1z/blueprints/teleop.py b/dimos/robot/manipulators/a1z/blueprints/teleop.py index 4cc2a1a8da..67d67f0cef 100644 --- a/dimos/robot/manipulators/a1z/blueprints/teleop.py +++ b/dimos/robot/manipulators/a1z/blueprints/teleop.py @@ -16,7 +16,7 @@ from __future__ import annotations -from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.control.coordinator import TaskConfig from dimos.core.coordination.blueprints import autoconnect from dimos.manipulation.manipulation_module import ManipulationModule from dimos.robot.manipulators.a1z.config import ( @@ -28,6 +28,10 @@ teleop_ik_task, trajectory_task, ) +from dimos.robot.manipulators.common.coordinators import ( + ArmPoseCoordinator, + ArmTwistCoordinator, +) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule _a1z_keyboard_hw = a1z_hardware("arm") @@ -35,7 +39,8 @@ keyboard_teleop_a1z = autoconnect( KeyboardTeleopModule.blueprint(), - ControlCoordinator.blueprint( + ArmTwistCoordinator.blueprint( + instance_name="ControlCoordinator", hardware=[_a1z_keyboard_hw], tasks=[ eef_twist_task( @@ -63,7 +68,8 @@ _a1z_quest_model = make_a1z_model_config() coordinator_teleop_a1z = autoconnect( - ControlCoordinator.blueprint( + ArmPoseCoordinator.blueprint( + instance_name="ControlCoordinator", hardware=[_a1z_quest_hw], tasks=[ teleop_ik_task( diff --git a/dimos/robot/manipulators/a1z/blueprints/test_teleop.py b/dimos/robot/manipulators/a1z/blueprints/test_teleop.py index a7b8b51cc1..86c9aa2892 100644 --- a/dimos/robot/manipulators/a1z/blueprints/test_teleop.py +++ b/dimos/robot/manipulators/a1z/blueprints/test_teleop.py @@ -22,11 +22,14 @@ from dimos.robot.manipulators.a1z.blueprints.teleop import coordinator_teleop_a1z from dimos.robot.manipulators.a1z.config import a1z_hardware from dimos.teleop.quest.blueprints import teleop_quest_a1z -from dimos.teleop.quest.quest_extensions import ArmTeleopModule def _coordinator_kwargs(blueprint: Blueprint) -> dict[str, Any]: - return next(atom.kwargs for atom in blueprint.blueprints if atom.module is ControlCoordinator) + return next( + atom.kwargs + for atom in blueprint.blueprints + if isinstance(atom.module, type) and issubclass(atom.module, ControlCoordinator) + ) def test_quest_teleop_uses_mock_a1z_hardware_and_gripper_by_default() -> None: @@ -46,13 +49,8 @@ def test_quest_teleop_uses_mock_a1z_hardware_and_gripper_by_default() -> None: def test_quest_left_controller_routes_to_a1z_teleop() -> None: - arm_kwargs = next( - atom.kwargs for atom in teleop_quest_a1z.blueprints if atom.module is ArmTeleopModule - ) - - assert arm_kwargs["task_names"] == {"left": "teleop_a1z"} assert teleop_quest_a1z.remapping_map == { - ("armteleopmodule", "left_controller_output"): "coordinator_cartesian_command" + ("armteleopmodule", "left_controller_output"): "cartesian_command" } diff --git a/dimos/robot/manipulators/a750/blueprints/teleop.py b/dimos/robot/manipulators/a750/blueprints/teleop.py index 90435cb267..6bb57a202d 100644 --- a/dimos/robot/manipulators/a750/blueprints/teleop.py +++ b/dimos/robot/manipulators/a750/blueprints/teleop.py @@ -16,14 +16,18 @@ from __future__ import annotations -from dimos.control.coordinator import ControlCoordinator from dimos.core.coordination.blueprints import autoconnect from dimos.manipulation.manipulation_module import ManipulationModule from dimos.robot.manipulators.a750.config import ( a750_hardware, make_a750_model_config, ) -from dimos.robot.manipulators.common.blueprints import eef_twist_task +from dimos.robot.manipulators.common.blueprints import ( + eef_twist_task, +) +from dimos.robot.manipulators.common.coordinators import ( + ArmTwistCoordinator, +) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule _a750_hw = a750_hardware("arm", mock_without_address=True) @@ -31,7 +35,8 @@ keyboard_teleop_a750 = autoconnect( KeyboardTeleopModule.blueprint(), - ControlCoordinator.blueprint( + ArmTwistCoordinator.blueprint( + instance_name="ControlCoordinator", tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py index 725b1255ff..1ac38a468b 100644 --- a/dimos/robot/manipulators/common/blueprints.py +++ b/dimos/robot/manipulators/common/blueprints.py @@ -174,6 +174,7 @@ def teleop_ik_task( max_dt: float = 0.05, control_ik: PinkControlIKOverrides | None = None, params: GripperTaskOverrides | None = None, + stream_bind: dict[str, str] | None = None, ) -> TaskConfig: resolved_control_ik = _resolve_control_ik(hardware, robot_model, control_ik) task_params: dict[str, Any] = { @@ -193,6 +194,7 @@ def teleop_ik_task( joint_names=hardware.joints, priority=priority, params=task_params, + stream_bind=stream_bind or {}, ) diff --git a/dimos/robot/manipulators/common/coordinators.py b/dimos/robot/manipulators/common/coordinators.py new file mode 100644 index 0000000000..78c5d36072 --- /dev/null +++ b/dimos/robot/manipulators/common/coordinators.py @@ -0,0 +1,47 @@ +# Copyright 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. + +"""Coordinators named for the command a single arm accepts. + +Pose and twist commands are point-to-point: each consuming task instance +reads its own port. A single-arm deployment needs one port named like the +card's input, which is all these add — no control behavior. Multi-arm +deployments declare a port per arm and bind each task with +``TaskConfig.stream_bind`` instead. +""" + +from __future__ import annotations + +from dimos.control.coordinator import ControlCoordinator +from dimos.core.stream import In +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped + + +class ArmPoseCoordinator(ControlCoordinator): + """Arm driven by target poses; the cartesian_ik / teleop_ik cards name-match.""" + + cartesian_command: In[PoseStamped] + + +class ArmTwistCoordinator(ControlCoordinator): + """Arm driven by EEF twists; the eef_twist card name-matches.""" + + ee_twist_command: In[TwistStamped] + + +class ArmPoseTwistCoordinator(ArmPoseCoordinator): + """Arm accepting both, e.g. VR poses preempting a browser twist jog.""" + + ee_twist_command: In[TwistStamped] diff --git a/dimos/robot/manipulators/common/mixed.py b/dimos/robot/manipulators/common/mixed.py index fcd0615c2a..a9db3f3b24 100644 --- a/dimos/robot/manipulators/common/mixed.py +++ b/dimos/robot/manipulators/common/mixed.py @@ -18,6 +18,8 @@ from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.core.global_config import global_config +from dimos.core.stream import In +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.robot.manipulators.common.blueprints import teleop_ik_task from dimos.robot.manipulators.piper.config import ( make_piper_hardware, @@ -69,7 +71,16 @@ _xarm6_teleop_model = make_xarm6_model_config(name="xarm_arm", add_gripper=False) _piper_teleop_model = make_piper_model_config(name="piper_arm") -coordinator_teleop_dual = ControlCoordinator.blueprint( + +class _DualTeleopCoordinator(ControlCoordinator): + """One cartesian port per arm; stream_bind gives each task its own.""" + + left_cartesian: In[PoseStamped] + right_cartesian: In[PoseStamped] + + +coordinator_teleop_dual = _DualTeleopCoordinator.blueprint( + instance_name="ControlCoordinator", hardware=[_xarm6_teleop_hw, _piper_teleop_hw], tasks=[ teleop_ik_task( @@ -78,6 +89,7 @@ hand="left", robot_model=_xarm6_teleop_model, priority=10, + stream_bind={"cartesian_command": "left_cartesian"}, ), teleop_ik_task( _piper_teleop_hw, @@ -85,6 +97,7 @@ hand="right", robot_model=_piper_teleop_model, priority=10, + stream_bind={"cartesian_command": "right_cartesian"}, ), ], ) diff --git a/dimos/robot/manipulators/openarm/blueprints/teleop.py b/dimos/robot/manipulators/openarm/blueprints/teleop.py index eb6baae066..17000394bd 100644 --- a/dimos/robot/manipulators/openarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/openarm/blueprints/teleop.py @@ -16,10 +16,14 @@ from __future__ import annotations -from dimos.control.coordinator import ControlCoordinator from dimos.core.coordination.blueprints import autoconnect from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.robot.manipulators.common.blueprints import eef_twist_task +from dimos.robot.manipulators.common.blueprints import ( + eef_twist_task, +) +from dimos.robot.manipulators.common.coordinators import ( + ArmTwistCoordinator, +) from dimos.robot.manipulators.openarm.config import ( LEFT_CAN, openarm_single_hardware, @@ -32,7 +36,8 @@ keyboard_teleop_openarm_mock = autoconnect( KeyboardTeleopModule.blueprint(), - ControlCoordinator.blueprint( + ArmTwistCoordinator.blueprint( + instance_name="ControlCoordinator", hardware=[_teleop_hw], tasks=[ eef_twist_task( @@ -51,7 +56,8 @@ keyboard_teleop_openarm = autoconnect( KeyboardTeleopModule.blueprint(), - ControlCoordinator.blueprint( + ArmTwistCoordinator.blueprint( + instance_name="ControlCoordinator", hardware=[_teleop_real_hw], tasks=[ eef_twist_task( diff --git a/dimos/robot/manipulators/openyam/blueprints/teleop.py b/dimos/robot/manipulators/openyam/blueprints/teleop.py index 16a5fdffff..8d221577dc 100644 --- a/dimos/robot/manipulators/openyam/blueprints/teleop.py +++ b/dimos/robot/manipulators/openyam/blueprints/teleop.py @@ -16,10 +16,14 @@ from __future__ import annotations -from dimos.control.coordinator import ControlCoordinator from dimos.core.coordination.blueprints import autoconnect from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.robot.manipulators.common.blueprints import eef_twist_task +from dimos.robot.manipulators.common.blueprints import ( + eef_twist_task, +) +from dimos.robot.manipulators.common.coordinators import ( + ArmTwistCoordinator, +) from dimos.robot.manipulators.openyam.config import ( make_openyam_hardware, make_openyam_model_config, @@ -31,7 +35,8 @@ keyboard_teleop_openyam = autoconnect( KeyboardTeleopModule.blueprint(), - ControlCoordinator.blueprint( + ArmTwistCoordinator.blueprint( + instance_name="ControlCoordinator", hardware=[_openyam_keyboard_hw], tasks=[ eef_twist_task( diff --git a/dimos/robot/manipulators/piper/blueprints/teleop.py b/dimos/robot/manipulators/piper/blueprints/teleop.py index 3a35e468d4..2a0ea613c3 100644 --- a/dimos/robot/manipulators/piper/blueprints/teleop.py +++ b/dimos/robot/manipulators/piper/blueprints/teleop.py @@ -17,7 +17,7 @@ from __future__ import annotations from dimos.control.components import make_gripper_joints -from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.control.coordinator import TaskConfig from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.manipulation.manipulation_module import ManipulationModule @@ -27,6 +27,10 @@ teleop_ik_task, trajectory_task, ) +from dimos.robot.manipulators.common.coordinators import ( + ArmPoseCoordinator, + ArmTwistCoordinator, +) from dimos.robot.manipulators.common.sim import mujoco_if_sim from dimos.robot.manipulators.piper.config import ( PIPER_SIM_PATH, @@ -48,7 +52,8 @@ keyboard_teleop_piper = autoconnect( KeyboardTeleopModule.blueprint(), - ControlCoordinator.blueprint( + ArmTwistCoordinator.blueprint( + instance_name="ControlCoordinator", tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", @@ -76,7 +81,8 @@ gripper=False, ) -coordinator_cartesian_ik_mock = ControlCoordinator.blueprint( +coordinator_cartesian_ik_mock = ArmPoseCoordinator.blueprint( + instance_name="ControlCoordinator", hardware=[_piper_mock_cartesian_hw], tasks=[cartesian_ik_task(_piper_mock_cartesian_hw, robot_model=_piper_model)], ) @@ -85,7 +91,8 @@ coordinator_teleop_piper = autoconnect( - ControlCoordinator.blueprint( + ArmPoseCoordinator.blueprint( + instance_name="ControlCoordinator", hardware=[_piper_teleop_hw], tasks=[ teleop_ik_task( @@ -116,7 +123,8 @@ gripper=True, ) -coordinator_cartesian_ik_piper = ControlCoordinator.blueprint( +coordinator_cartesian_ik_piper = ArmPoseCoordinator.blueprint( + instance_name="ControlCoordinator", hardware=[_piper_cartesian_hw], tasks=[cartesian_ik_task(_piper_cartesian_hw, robot_model=_piper_model)], ) diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py index 7245269af4..30d0373a5c 100644 --- a/dimos/robot/manipulators/xarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py @@ -28,6 +28,10 @@ teleop_ik_task, trajectory_task, ) +from dimos.robot.manipulators.common.coordinators import ( + ArmPoseTwistCoordinator, + ArmTwistCoordinator, +) from dimos.robot.manipulators.common.sim import mujoco_if_sim from dimos.robot.manipulators.xarm.config import ( XARM6_SIM_PATH, @@ -49,7 +53,8 @@ keyboard_teleop_xarm6 = autoconnect( KeyboardTeleopModule.blueprint(), - ControlCoordinator.blueprint( + ArmTwistCoordinator.blueprint( + instance_name="ControlCoordinator", tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", @@ -71,7 +76,8 @@ keyboard_teleop_xarm7 = autoconnect( KeyboardTeleopModule.blueprint(), - ControlCoordinator.blueprint( + ArmTwistCoordinator.blueprint( + instance_name="ControlCoordinator", tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", @@ -165,7 +171,8 @@ coordinator_teleop_xarm7 = autoconnect( - ControlCoordinator.blueprint( + ArmPoseTwistCoordinator.blueprint( + instance_name="ControlCoordinator", hardware=[_xarm7_teleop_hw], tasks=[ teleop_ik_task( @@ -194,7 +201,8 @@ ) coordinator_teleop_xarm6 = autoconnect( - ControlCoordinator.blueprint( + ArmPoseTwistCoordinator.blueprint( + instance_name="ControlCoordinator", hardware=[_xarm6_teleop_hw], tasks=[ teleop_ik_task( diff --git a/dimos/teleop/hosted/arm_command.py b/dimos/teleop/hosted/arm_command.py index 166a2ecb34..a947fcd3e4 100644 --- a/dimos/teleop/hosted/arm_command.py +++ b/dimos/teleop/hosted/arm_command.py @@ -33,9 +33,9 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped from dimos.msgs.std_msgs.Bool import Bool -from dimos.robot.manipulators.common.topics import EEF_TWIST_TASK_NAME from dimos.teleop.hosted.command_executor import SerializedCommandExecutor -from dimos.teleop.quest.quest_extensions import ArmTeleopConfig, ArmTeleopModule +from dimos.teleop.quest.quest_extensions import ArmTeleopModule +from dimos.teleop.quest.quest_teleop_module import QuestTeleopConfig from dimos.teleop.quest.quest_types import Hand from dimos.teleop.utils.teleop_transforms import webxr_to_robot from dimos.utils.logging_config import setup_logger @@ -43,7 +43,7 @@ logger = setup_logger() -class ArmCommandConfig(ArmTeleopConfig): +class ArmCommandConfig(QuestTeleopConfig): cmd_stale_after_sec: float = 0.5 @@ -59,7 +59,7 @@ class ArmCommandModule(ArmTeleopModule): cmd_ack: Out[bytes] robot_state: Out[bytes] - coordinator_ee_twist_command: Out[TwistStamped] + ee_twist_command: Out[TwistStamped] gripper_command: Out[Bool] def __init__(self, **kwargs: Any) -> None: @@ -166,9 +166,8 @@ def _on_twist_bytes(self, data: bytes) -> None: if ts <= self._last_twist_ts: # out-of-order return self._last_twist_ts = ts - self.coordinator_ee_twist_command.publish( + self.ee_twist_command.publish( TwistStamped( - frame_id=EEF_TWIST_TASK_NAME, linear=[msg.linear.x, msg.linear.y, msg.linear.z], angular=[msg.angular.x, msg.angular.y, msg.angular.z], ts=msg.ts, diff --git a/dimos/teleop/hosted/blueprints/cloudflare.py b/dimos/teleop/hosted/blueprints/cloudflare.py index 9b5febb880..4ca2626327 100644 --- a/dimos/teleop/hosted/blueprints/cloudflare.py +++ b/dimos/teleop/hosted/blueprints/cloudflare.py @@ -162,7 +162,7 @@ class WristCamera(RealSenseCamera): teleop_hosted_xarm6 = ( autoconnect( - ArmCommandModule.blueprint(task_names={"right": "teleop_xarm"}), + ArmCommandModule.blueprint(), HostedStatsModule.blueprint(), CameraMuxModule.blueprint(cameras=["cam1", "cam2"]), coordinator_teleop_xarm6, @@ -173,7 +173,7 @@ class WristCamera(RealSenseCamera): [ (FrontCamera, "color_image", "cam1"), (WristCamera, "color_image", "cam2"), - (ArmCommandModule, "right_controller_output", "coordinator_cartesian_command"), + (ArmCommandModule, "right_controller_output", "cartesian_command"), ] ) .transports( @@ -194,7 +194,7 @@ class WristCamera(RealSenseCamera): teleop_hosted_xarm7 = ( autoconnect( - ArmCommandModule.blueprint(task_names={"right": "teleop_xarm"}), + ArmCommandModule.blueprint(), HostedStatsModule.blueprint(), CameraMuxModule.blueprint(cameras=["cam1", "cam2"]), coordinator_teleop_xarm7, @@ -205,7 +205,7 @@ class WristCamera(RealSenseCamera): [ (FrontCamera, "color_image", "cam1"), (WristCamera, "color_image", "cam2"), - (ArmCommandModule, "right_controller_output", "coordinator_cartesian_command"), + (ArmCommandModule, "right_controller_output", "cartesian_command"), ] ) .transports( diff --git a/dimos/teleop/hosted/test_arm_command.py b/dimos/teleop/hosted/test_arm_command.py index 8d0aabe865..553895de31 100644 --- a/dimos/teleop/hosted/test_arm_command.py +++ b/dimos/teleop/hosted/test_arm_command.py @@ -35,7 +35,6 @@ from dimos.core.module import Module from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.robot.manipulators.common.topics import EEF_TWIST_TASK_NAME from dimos.teleop.hosted.arm_command import ArmCommandModule from dimos.teleop.quest.quest_types import Hand, QuestControllerState from dimos.utils.testing.waiting import wait_until @@ -46,12 +45,10 @@ def module(monkeypatch: pytest.MonkeyPatch) -> Iterator[ArmCommandModule]: """A real ArmCommandModule with only the framework ``Module.__init__`` skipped — the quest-layer and command-plane inits (engage state, decoder table, estop/twist gates) run for real. Ports / coordinator ref / config - are mocked; config is seeded by the patched init since the quest layer - reads ``config.task_names`` while constructing.""" + are mocked; config is seeded by the patched init.""" def _fake_init(self: Any, **kwargs: Any) -> None: self.config = SimpleNamespace( - task_names={"right": "teleop_xarm"}, control_loop_hz=50.0, cmd_stale_after_sec=0.5, ) @@ -64,7 +61,7 @@ def _fake_init(self: Any, **kwargs: Any) -> None: "buttons", "cmd_ack", "robot_state", - "coordinator_ee_twist_command", + "ee_twist_command", "gripper_command", "coordinator", ): @@ -157,38 +154,38 @@ def test_pose_watermark_is_per_hand(module: ArmCommandModule) -> None: # ─── Browser keyboard EE-twist → coordinator eef_twist ───────────────── -def test_twist_routes_to_eef_twist_task(module: ArmCommandModule) -> None: +def test_twist_republished_without_task_address(module: ArmCommandModule) -> None: module._on_cmd_raw(_twist_bytes(0.2)) - module.coordinator_ee_twist_command.publish.assert_called_once() - out = module.coordinator_ee_twist_command.publish.call_args.args[0] - assert out.frame_id == EEF_TWIST_TASK_NAME + module.ee_twist_command.publish.assert_called_once() + out = module.ee_twist_command.publish.call_args.args[0] + assert out.frame_id == "" # addressing is the port wiring, not the payload assert out.linear.x == pytest.approx(0.2) def test_twist_dropped_while_estopped(module: ArmCommandModule) -> None: module._estopped = True module._on_cmd_raw(_twist_bytes(0.2)) - module.coordinator_ee_twist_command.publish.assert_not_called() + module.ee_twist_command.publish.assert_not_called() def test_stale_twist_dropped(module: ArmCommandModule) -> None: module._on_cmd_raw(_twist_bytes(0.2, ts=time.time() - 1.0)) # > cmd_stale_after_sec - module.coordinator_ee_twist_command.publish.assert_not_called() + module.ee_twist_command.publish.assert_not_called() def test_future_stamped_twist_dropped(module: ArmCommandModule) -> None: module._on_cmd_raw(_twist_bytes(0.2, ts=time.time() + 5.0)) - module.coordinator_ee_twist_command.publish.assert_not_called() + module.ee_twist_command.publish.assert_not_called() # ...and it must not advance the ordering watermark (would stall real cmds). module._on_cmd_raw(_twist_bytes(0.3)) - module.coordinator_ee_twist_command.publish.assert_called_once() + module.ee_twist_command.publish.assert_called_once() def test_out_of_order_twist_dropped(module: ArmCommandModule) -> None: t = time.time() module._on_cmd_raw(_twist_bytes(0.2, ts=t)) module._on_cmd_raw(_twist_bytes(0.3, ts=t - 0.1)) # older than the last accepted - assert module.coordinator_ee_twist_command.publish.call_count == 1 + assert module.ee_twist_command.publish.call_count == 1 def test_stale_twist_warning_rate_limited(module: ArmCommandModule) -> None: @@ -216,15 +213,15 @@ def test_gripper_dropped_while_estopped(module: ArmCommandModule) -> None: module.gripper_command.publish.assert_not_called() -# ─── Engage → publish with task-name routing ─────────────────────────── +# ─── Engage → publish on the hand's own port ─────────────────────────── -def test_engage_publishes_task_routed_pose(module: ArmCommandModule) -> None: +def test_engage_publishes_on_hand_port(module: ArmCommandModule) -> None: _engage_right(module) assert module._is_engaged[Hand.RIGHT] module.right_controller_output.publish.assert_called() out = module.right_controller_output.publish.call_args.args[0] - assert out.frame_id == "teleop_xarm" + assert out.frame_id == "right" # handedness preserved; no task-name overwrite module.left_controller_output.publish.assert_not_called() diff --git a/dimos/teleop/keyboard/keyboard_teleop_module.py b/dimos/teleop/keyboard/keyboard_teleop_module.py index 80ad6e1471..3bde07808b 100644 --- a/dimos/teleop/keyboard/keyboard_teleop_module.py +++ b/dimos/teleop/keyboard/keyboard_teleop_module.py @@ -49,7 +49,6 @@ from dimos.core.stream import Out from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.robot.manipulators.common.topics import EEF_TWIST_TASK_NAME from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -70,7 +69,6 @@ class KeyboardTeleopConfig(ModuleConfig): - task_name: str = EEF_TWIST_TASK_NAME linear_speed: float = DEFAULT_LINEAR_SPEED angular_speed: float = DEFAULT_ANGULAR_SPEED gripper_open_position: float = GRIPPER_OPEN_POSITION @@ -112,7 +110,7 @@ class KeyboardTeleopModule(Module): config: KeyboardTeleopConfig - coordinator_ee_twist_command: Out[TwistStamped] + ee_twist_command: Out[TwistStamped] joint_command: Out[JointState] _stop_event: threading.Event @@ -143,11 +141,9 @@ def stop(self) -> None: super().stop() def _pygame_loop(self) -> None: - task_name = self.config.task_name - pygame.init() screen = pygame.display.set_mode((600, 400), pygame.SWSURFACE) - pygame.display.set_caption(f"Keyboard Teleop — {task_name}") + pygame.display.set_caption("Keyboard Teleop") font = pygame.font.Font(None, 28) clock = pygame.time.Clock() held_motion_keys: set[int] = set() @@ -155,7 +151,7 @@ def _pygame_loop(self) -> None: while not self._stop_event.is_set(): for event in pygame.event.get(): - if self._handle_pygame_event(event, held_motion_keys, task_name): + if self._handle_pygame_event(event, held_motion_keys): self._stop_event.set() linear, angular = _twist_from_keys( @@ -168,18 +164,14 @@ def _pygame_loop(self) -> None: is_moving = any(value != 0.0 for value in (*linear, *angular)) if is_moving or was_moving: - self._publish_twist( - task_name, - linear=linear, - angular=angular, - ) + self._publish_twist(linear=linear, angular=angular) was_moving = is_moving # Draw UI screen.fill((30, 30, 30)) y_pos = 20 - title = font.render(f"Keyboard Teleop — {task_name}", True, (255, 255, 255)) + title = font.render("Keyboard Teleop", True, (255, 255, 255)) screen.blit(title, (20, y_pos)) y_pos += 40 @@ -210,14 +202,13 @@ def _pygame_loop(self) -> None: pygame.display.flip() clock.tick(50) - self._publish_twist(task_name) + self._publish_twist() pygame.quit() def _handle_pygame_event( self, event: Any, held_motion_keys: set[int], - task_name: str, ) -> bool: """Apply one pygame event and synchronously stop motion on KEYUP.""" if pygame is None: @@ -241,19 +232,16 @@ def _handle_pygame_event( linear_speed=self.config.linear_speed, angular_speed=self.config.angular_speed, ) - self._publish_twist(task_name, linear=linear, angular=angular) + self._publish_twist(linear=linear, angular=angular) return False def _publish_twist( self, - task_name: str, *, linear: TwistVector = (0.0, 0.0, 0.0), angular: TwistVector = (0.0, 0.0, 0.0), ) -> None: - self.coordinator_ee_twist_command.publish( - TwistStamped(frame_id=task_name, linear=list(linear), angular=list(angular)) - ) + self.ee_twist_command.publish(TwistStamped(linear=list(linear), angular=list(angular))) def _set_gripper_position(self, position: float) -> None: """Latch and publish a changed gripper endpoint command.""" diff --git a/dimos/teleop/keyboard/test_keyboard_teleop_module.py b/dimos/teleop/keyboard/test_keyboard_teleop_module.py index 4c59f906a2..833a0582b6 100644 --- a/dimos/teleop/keyboard/test_keyboard_teleop_module.py +++ b/dimos/teleop/keyboard/test_keyboard_teleop_module.py @@ -19,7 +19,6 @@ import pytest from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.robot.manipulators.common.topics import EEF_TWIST_TASK_NAME import dimos.teleop.keyboard.keyboard_teleop_module as keyboard_mod from dimos.teleop.keyboard.keyboard_teleop_module import ( GRIPPER_CLOSED_POSITION, @@ -46,25 +45,26 @@ def module() -> Iterator[KeyboardTeleopModule]: module.stop() -def test_publish_twist_emits_routed_twist_stamped(module: KeyboardTeleopModule, mocker) -> None: - publish = mocker.patch.object(module.coordinator_ee_twist_command, "publish") +def test_publish_twist_emits_unaddressed_twist_stamped( + module: KeyboardTeleopModule, mocker +) -> None: + publish = mocker.patch.object(module.ee_twist_command, "publish") - module._publish_twist("custom_eef", linear=(0.1, 0.2, 0.3), angular=(0.4, 0.5, 0.6)) + module._publish_twist(linear=(0.1, 0.2, 0.3), angular=(0.4, 0.5, 0.6)) msg = publish.call_args.args[0] assert isinstance(msg, TwistStamped) - assert msg.frame_id == "custom_eef" + assert msg.frame_id == "" # no task-name address in the payload assert [msg.linear.x, msg.linear.y, msg.linear.z] == [0.1, 0.2, 0.3] assert [msg.angular.x, msg.angular.y, msg.angular.z] == [0.4, 0.5, 0.6] def test_publish_twist_defaults_to_zero_twist(module: KeyboardTeleopModule, mocker) -> None: - publish = mocker.patch.object(module.coordinator_ee_twist_command, "publish") + publish = mocker.patch.object(module.ee_twist_command, "publish") - module._publish_twist(EEF_TWIST_TASK_NAME) + module._publish_twist() msg = publish.call_args.args[0] - assert msg.frame_id == EEF_TWIST_TASK_NAME assert [msg.linear.x, msg.linear.y, msg.linear.z] == [0.0, 0.0, 0.0] assert [msg.angular.x, msg.angular.y, msg.angular.z] == [0.0, 0.0, 0.0] @@ -92,11 +92,11 @@ def test_twist_from_keys_maps_rotation_keys_to_eef_angular_twist() -> None: def test_final_key_release_publishes_zero_velocity(module: KeyboardTeleopModule, mocker) -> None: - publish = mocker.patch.object(module.coordinator_ee_twist_command, "publish") + publish = mocker.patch.object(module.ee_twist_command, "publish") held = {keyboard_mod.pygame.K_w} event = keyboard_mod.pygame.event.Event(keyboard_mod.pygame.KEYUP, key=keyboard_mod.pygame.K_w) - assert not module._handle_pygame_event(event, held, EEF_TWIST_TASK_NAME) + assert not module._handle_pygame_event(event, held) assert held == set() assert publish.call_count == 1 @@ -106,11 +106,11 @@ def test_final_key_release_publishes_zero_velocity(module: KeyboardTeleopModule, def test_keyup_preserves_remaining_motion_key(module: KeyboardTeleopModule, mocker) -> None: - publish = mocker.patch.object(module.coordinator_ee_twist_command, "publish") + publish = mocker.patch.object(module.ee_twist_command, "publish") held = {keyboard_mod.pygame.K_w, keyboard_mod.pygame.K_a} event = keyboard_mod.pygame.event.Event(keyboard_mod.pygame.KEYUP, key=keyboard_mod.pygame.K_w) - module._handle_pygame_event(event, held, EEF_TWIST_TASK_NAME) + module._handle_pygame_event(event, held) assert held == {keyboard_mod.pygame.K_a} assert publish.call_count == 1 @@ -121,11 +121,11 @@ def test_keyup_preserves_remaining_motion_key(module: KeyboardTeleopModule, mock def test_keyup_publishes_directly_without_timeout_wait( module: KeyboardTeleopModule, mocker ) -> None: - publish = mocker.patch.object(module.coordinator_ee_twist_command, "publish") + publish = mocker.patch.object(module.ee_twist_command, "publish") held = {keyboard_mod.pygame.K_w} event = keyboard_mod.pygame.event.Event(keyboard_mod.pygame.KEYUP, key=keyboard_mod.pygame.K_w) - module._handle_pygame_event(event, held, EEF_TWIST_TASK_NAME) + module._handle_pygame_event(event, held) publish.assert_called_once() diff --git a/dimos/teleop/quest/blueprints.py b/dimos/teleop/quest/blueprints.py index 904c3680b4..b6e1966048 100644 --- a/dimos/teleop/quest/blueprints.py +++ b/dimos/teleop/quest/blueprints.py @@ -55,27 +55,25 @@ # XArm7 teleop (sim with --simulation, real otherwise): right controller -> xarm7 teleop_quest_xarm7 = autoconnect( - ArmTeleopModule.blueprint(task_names={"right": "teleop_xarm"}), + ArmTeleopModule.blueprint(), coordinator_teleop_xarm7, -).remappings([(ArmTeleopModule, "right_controller_output", "coordinator_cartesian_command")]) +).remappings([(ArmTeleopModule, "right_controller_output", "cartesian_command")]) # XArm7 hand teleop: thumb-and-index pinch toggles tracking for each hand. teleop_quest_hand_xarm7 = autoconnect( - HandTeleopModule.blueprint(task_names={"right": "teleop_xarm"}), + HandTeleopModule.blueprint(), coordinator_teleop_xarm7, -).remappings([(HandTeleopModule, "right_controller_output", "coordinator_cartesian_command")]) +).remappings([(HandTeleopModule, "right_controller_output", "cartesian_command")]) # XArm7 teleop + camera streaming into the Quest scene as a panel. teleop_quest_xarm7_video = ( autoconnect( - VideoArmTeleopModule.blueprint(task_names={"right": "teleop_xarm"}), + VideoArmTeleopModule.blueprint(), coordinator_teleop_xarm7, ) - .remappings( - [(VideoArmTeleopModule, "right_controller_output", "coordinator_cartesian_command")] - ) + .remappings([(VideoArmTeleopModule, "right_controller_output", "cartesian_command")]) .transports( { ("color_image", Image): LCMTransport("/teleop/color_image", Image), @@ -86,33 +84,33 @@ # Piper teleop (sim with --simulation, real otherwise): left controller -> piper arm teleop_quest_piper = autoconnect( - ArmTeleopModule.blueprint(task_names={"left": "teleop_piper"}), + ArmTeleopModule.blueprint(), coordinator_teleop_piper, -).remappings([(ArmTeleopModule, "left_controller_output", "coordinator_cartesian_command")]) +).remappings([(ArmTeleopModule, "left_controller_output", "cartesian_command")]) # A1Z mock teleop: left controller -> A1Z arm teleop_quest_a1z = autoconnect( - ArmTeleopModule.blueprint(task_names={"left": "teleop_a1z"}), + ArmTeleopModule.blueprint(), coordinator_teleop_a1z, -).remappings([(ArmTeleopModule, "left_controller_output", "coordinator_cartesian_command")]) +).remappings([(ArmTeleopModule, "left_controller_output", "cartesian_command")]) # XArm6 teleop (sim with --simulation, real otherwise): right controller -> xarm6 teleop_quest_xarm6 = autoconnect( - ArmTeleopModule.blueprint(task_names={"right": "teleop_xarm"}), + ArmTeleopModule.blueprint(), coordinator_teleop_xarm6, -).remappings([(ArmTeleopModule, "right_controller_output", "coordinator_cartesian_command")]) +).remappings([(ArmTeleopModule, "right_controller_output", "cartesian_command")]) # Dual arm teleop: right -> piper, left -> xarm6 (TeleopIK, real-only) teleop_quest_dual = autoconnect( - ArmTeleopModule.blueprint(task_names={"right": "teleop_piper", "left": "teleop_xarm"}), + ArmTeleopModule.blueprint(), coordinator_teleop_dual, ).remappings( [ - (ArmTeleopModule, "right_controller_output", "coordinator_cartesian_command"), - (ArmTeleopModule, "left_controller_output", "coordinator_cartesian_command"), + (ArmTeleopModule, "right_controller_output", "right_cartesian"), + (ArmTeleopModule, "left_controller_output", "left_cartesian"), ] ) diff --git a/dimos/teleop/quest/quest_extensions.py b/dimos/teleop/quest/quest_extensions.py index 2af66b1693..1922a42236 100644 --- a/dimos/teleop/quest/quest_extensions.py +++ b/dimos/teleop/quest/quest_extensions.py @@ -15,7 +15,7 @@ """Quest teleop module extensions and subclasses. Available subclasses: - - ArmTeleopModule: Per-hand press-and-hold engage (X/A hold to track), task name routing + - ArmTeleopModule: Per-hand press-and-hold engage (X/A hold to track) - HandTeleopModule: Pinch-to-toggle arm teleop using WebXR hand tracking - TwistTeleopModule: Outputs Twist instead of PoseStamped - VideoArmTeleopModule: ArmTeleopModule + JPEG frames pushed to the Quest over /ws @@ -26,7 +26,6 @@ from typing import Any from fastapi import WebSocket -from pydantic import Field from dimos.core.core import rpc from dimos.core.stream import In, Out @@ -125,27 +124,13 @@ def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: self.right_twist.publish(twist) -class ArmTeleopConfig(QuestTeleopConfig): - """Configuration for ArmTeleopModule. - - Attributes: - task_names: Mapping of Hand -> coordinator task name. Used to set - frame_id on output PoseStamped so the coordinator routes each - hand's commands to the correct TeleopIKTask. - """ - - task_names: dict[str, str] = Field(default_factory=dict) - - class ArmTeleopModule(QuestTeleopModule): - """Quest teleop with per-hand press-and-hold engage and task name routing. + """Quest teleop with per-hand press-and-hold engage. Each controller's primary button (X for left, A for right) - engages that hand while held, disengages on release. - - When task_names is configured, output PoseStamped messages have their - frame_id set to the task name, enabling the coordinator to route - each hand's commands to the correct TeleopIKTask. + engages that hand while held, disengages on release. Each hand's + output port is wired to its consuming task's coordinator port in + the blueprint; no addressing happens in the message. Outputs: - left_controller_output: PoseStamped (inherited) @@ -153,8 +138,6 @@ class ArmTeleopModule(QuestTeleopModule): - buttons: Buttons (inherited) """ - config: ArmTeleopConfig - @rpc def start(self) -> None: super().start() @@ -163,25 +146,6 @@ def start(self) -> None: def stop(self) -> None: super().stop() - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - - self._task_names: dict[Hand, str] = { - Hand[k.upper()]: v for k, v in self.config.task_names.items() - } - - def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: - """Stamp frame_id with task name and publish.""" - task_name = self._task_names.get(hand) - if task_name: - output_msg = PoseStamped( - position=output_msg.position, - orientation=output_msg.orientation, - ts=output_msg.ts, - frame_id=task_name, - ) - super()._publish_msg(hand, output_msg) - def _publish_button_state( self, left: QuestControllerState | None, @@ -236,7 +200,7 @@ def _publish_button_state( self.teleop_buttons.publish(buttons) -class VideoArmTeleopConfig(ArmTeleopConfig): +class VideoArmTeleopConfig(QuestTeleopConfig): """Configuration for VideoArmTeleopModule.""" video_jpeg_quality: int = 70