diff --git a/dimos/control/README.md b/dimos/control/README.md index f24303bc1c..c0fb3f348a 100644 --- a/dimos/control/README.md +++ b/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) diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 703fe5d6a8..2f2e8284b4 100644 --- a/dimos/control/blueprints/mobile.py +++ b/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, diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index eeed8dead0..9fa01e571a 100644 --- a/dimos/control/coordinator.py +++ b/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(): diff --git a/dimos/control/task.py b/dimos/control/task.py index 84bbb38896..349a8c6ee2 100644 --- a/dimos/control/task.py +++ b/dimos/control/task.py @@ -27,8 +27,13 @@ from __future__ import annotations -from dataclasses import dataclass, field +from collections.abc import Callable, Mapping +from dataclasses import dataclass +from types import MappingProxyType from typing import TYPE_CHECKING, Protocol, runtime_checkable +from weakref import ReferenceType, ref + +import attrs from dimos.control.components import JointName from dimos.hardware.manipulators.spec import ControlMode as ControlMode @@ -41,6 +46,18 @@ from dimos.teleop.quest.quest_types import Buttons +def _immutable_joint_mapping( + value: Mapping[JointName, float], +) -> Mapping[JointName, float]: + """Detach a mapping from its caller and expose a read-only copy.""" + return MappingProxyType(dict(value)) + + +def _immutable_imu_mapping(value: Mapping[str, IMUState]) -> Mapping[str, IMUState]: + """Detach an IMU mapping from its caller and expose a read-only copy.""" + return MappingProxyType(dict(value)) + + @dataclass(frozen=True) class ResourceClaim: """Declares which joints a task wants to control. @@ -65,7 +82,7 @@ def conflicts_with(self, other: ResourceClaim) -> bool: return bool(self.joints & other.joints) -@dataclass +@attrs.frozen(slots=False) class JointStateSnapshot: """Aggregated joint states from all hardware. @@ -79,9 +96,18 @@ class JointStateSnapshot: timestamp: Unix timestamp when state was read """ - joint_positions: dict[JointName, float] = field(default_factory=dict) - joint_velocities: dict[JointName, float] = field(default_factory=dict) - joint_efforts: dict[JointName, float] = field(default_factory=dict) + joint_positions: Mapping[JointName, float] = attrs.field( + factory=dict, + converter=_immutable_joint_mapping, + ) + joint_velocities: Mapping[JointName, float] = attrs.field( + factory=dict, + converter=_immutable_joint_mapping, + ) + joint_efforts: Mapping[JointName, float] = attrs.field( + factory=dict, + converter=_immutable_joint_mapping, + ) timestamp: float = 0.0 def get_position(self, joint_name: JointName) -> float | None: @@ -97,7 +123,7 @@ def get_effort(self, joint_name: JointName) -> float | None: return self.joint_efforts.get(joint_name) -@dataclass +@attrs.frozen(slots=False) class CoordinatorState: """Complete state snapshot for tasks to read. @@ -115,11 +141,48 @@ class CoordinatorState: """ joints: JointStateSnapshot - imu: dict[str, IMUState] = field(default_factory=dict) + imu: Mapping[str, IMUState] = attrs.field( + factory=dict, + converter=_immutable_imu_mapping, + ) t_now: float = 0.0 # Coordinator time (perf_counter) - USE THIS, NOT time.time()! dt: float = 0.0 # Time since last tick +@attrs.frozen(slots=False) +class ControlTaskContext: + """Coordinator-owned services available to a registered base task. + + The context itself carries no runtime state. Its callbacks always resolve + against the coordinator that owns the task registration. + """ + + get_state: Callable[[], CoordinatorState | None] + + +@attrs.frozen(slots=True, weakref_slot=True) +class _TaskRegistration: + """Registration-owned lease for one task's coordinator context access.""" + + context: ControlTaskContext + + @classmethod + def acquire( + cls, + task: BaseControlTask, + context: ControlTaskContext, + ) -> _TaskRegistration: + """Create the task's exclusive registration lease.""" + existing = task._registration() if task._registration is not None else None + if existing is not None: + raise RuntimeError( + f"Control task {task.name!r} is already registered with another coordinator" + ) + registration = cls(context=context) + task._registration = ref(registration) + return registration + + @dataclass class JointCommandOutput: """Joint-centric command output from a task. @@ -328,12 +391,25 @@ class BaseControlTask(ControlTask): """ _name: str + _registration: ReferenceType[_TaskRegistration] | None = None @property def name(self) -> str: """Unique task identifier, backed by ``self._name``.""" return self._name + @property + def context(self) -> ControlTaskContext: + """Return the registered coordinator's context. + + A task can be configured before registration, but coordinator services + are intentionally unavailable until the task is registered. + """ + registration = self._registration() if self._registration is not None else None + if registration is None: + raise RuntimeError(f"Control task {self.name!r} is not registered with a coordinator") + return registration.context + def on_buttons(self, msg: Buttons) -> bool: """No-op default.""" return False diff --git a/dimos/control/tasks/g1_groot_wbc_task/test_g1_groot_wbc_task.py b/dimos/control/tasks/g1_groot_wbc_task/test_g1_groot_wbc_task.py index 95304b2a10..55659e4ced 100644 --- a/dimos/control/tasks/g1_groot_wbc_task/test_g1_groot_wbc_task.py +++ b/dimos/control/tasks/g1_groot_wbc_task/test_g1_groot_wbc_task.py @@ -244,14 +244,15 @@ def test_unarmed_task_holds_current_pose_without_running_policy( unarmed_task: G1GrootWBCTask, joints_29: list[str], patched_ort: list[str] ) -> None: unarmed_task.start() + positions = {n: 0.0 for n in joints_29} + for i, name in enumerate(joints_29[:15]): + positions[name] = 0.1 * (i + 1) snap = JointStateSnapshot( - joint_positions={n: 0.0 for n in joints_29}, + joint_positions=positions, joint_velocities={n: 0.0 for n in joints_29}, joint_efforts={n: 0.0 for n in joints_29}, timestamp=100.0, ) - for i, name in enumerate(joints_29[:15]): - snap.joint_positions[name] = 0.1 * (i + 1) out = None for _ in range(30): diff --git a/dimos/control/tasks/trajectory_task/trajectory_task.py b/dimos/control/tasks/trajectory_task/trajectory_task.py index c6d2a98e1b..c22e38ae14 100644 --- a/dimos/control/tasks/trajectory_task/trajectory_task.py +++ b/dimos/control/tasks/trajectory_task/trajectory_task.py @@ -22,7 +22,7 @@ from __future__ import annotations -from collections.abc import Mapping, Sequence +from collections.abc import Sequence from dataclasses import dataclass from enum import Enum, auto import math @@ -45,12 +45,13 @@ logger = setup_logger() +JOINT_TRAJECTORY_TASK_NAME = "joint_trajectory" + class TrajectoryExecutionStatus(Enum): """Semantic outcome of a trajectory execution request.""" ACCEPTED = auto() - NO_TRAJECTORY_TASK = auto() INVALID_TRAJECTORY = auto() START_STATE_UNAVAILABLE = auto() START_STATE_MISMATCH = auto() @@ -69,7 +70,6 @@ class TrajectoryCancellationStatus(Enum): CANCELLED = auto() ALREADY_STOPPED = auto() - NO_TRAJECTORY_TASK = auto() UNCERTAIN = auto() @@ -145,24 +145,22 @@ class JointTrajectoryTask(BaseControlTask): Example: >>> task = JointTrajectoryTask( - ... name="traj_left", - ... config=JointTrajectoryTaskConfig( + ... JointTrajectoryTaskConfig( ... joint_names=["left/joint1", "left/joint2"], ... priority=10, ... ), ... ) >>> coordinator.add_task(task) - >>> task.execute(my_trajectory, current_positions) + >>> task.execute(my_trajectory) """ - def __init__(self, name: str, config: JointTrajectoryTaskConfig) -> None: + def __init__(self, config: JointTrajectoryTaskConfig) -> None: """Initialize trajectory task. Args: - name: Unique task name config: Task configuration """ - self._name = name + self._name = JOINT_TRAJECTORY_TASK_NAME self._config = config self._joint_names = frozenset(config.joint_names) self._joint_names_list = list(config.joint_names) @@ -173,7 +171,9 @@ def __init__(self, name: str, config: JointTrajectoryTaskConfig) -> None: self._start_time: float = 0.0 self._pending_start: bool = False # Defer start time to first compute() - logger.info(f"JointTrajectoryTask {name} initialized for joints: {config.joint_names}") + logger.info( + f"JointTrajectoryTask {self._name} initialized for joints: {config.joint_names}" + ) def claim(self) -> ResourceClaim: """Declare resource requirements.""" @@ -300,13 +300,11 @@ def _validate_trajectory(self, trajectory: JointTrajectory) -> bool: def execute( self, trajectory: JointTrajectory, - current_positions: Mapping[str, float], ) -> TrajectoryExecutionResult: """Start executing a trajectory. Args: trajectory: Trajectory to execute - current_positions: Authoritative positions from the coordinator. Returns: Semantic execution acceptance result. @@ -331,6 +329,13 @@ def execute( "Trajectory structure or joints are invalid", ) + state = self.context.get_state() + if state is None: + return TrajectoryExecutionResult( + TrajectoryExecutionStatus.START_STATE_UNAVAILABLE, + "Coordinator state is unavailable before the first control tick", + ) + current_positions = state.joints.joint_positions first_positions = trajectory.points[0].positions for joint_name, planned_position in zip( trajectory.joint_names, first_positions, strict=True @@ -421,9 +426,12 @@ class JointTrajectoryTaskParams(BaseConfig): def create_task(cfg: Any, hardware: Any) -> JointTrajectoryTask: + if cfg.name != JOINT_TRAJECTORY_TASK_NAME: + raise ValueError( + f"trajectory task name must be {JOINT_TRAJECTORY_TASK_NAME!r}, got {cfg.name!r}" + ) params = JointTrajectoryTaskParams.model_validate(cfg.params) return JointTrajectoryTask( - cfg.name, JointTrajectoryTaskConfig( joint_names=cfg.joint_names, priority=cfg.priority, diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index c481965070..cf0c937234 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -43,7 +43,9 @@ JointStateSnapshot, ResourceClaim, ) +from dimos.control.tasks.registry import control_task_registry from dimos.control.tasks.trajectory_task.trajectory_task import ( + JOINT_TRAJECTORY_TASK_NAME, JointTrajectoryTask, JointTrajectoryTaskConfig, TrajectoryCancellationStatus, @@ -85,13 +87,32 @@ def connected_hardware(mock_adapter): @pytest.fixture -def trajectory_task(): +def trajectory_coordinator(make_coordinator) -> ControlCoordinator: + coordinator = make_coordinator() + coordinator._set_latest_state( + CoordinatorState( + joints=JointStateSnapshot( + joint_positions={ + "arm/joint1": 0.0, + "arm/joint2": 0.0, + "arm/joint3": 0.0, + } + ) + ) + ) + return coordinator + + +@pytest.fixture +def trajectory_task(trajectory_coordinator): """Create a JointTrajectoryTask for testing.""" config = JointTrajectoryTaskConfig( joint_names=["arm/joint1", "arm/joint2", "arm/joint3"], priority=10, ) - return JointTrajectoryTask(name="test_traj", config=config) + task = JointTrajectoryTask(config) + assert trajectory_coordinator.add_task(task, task_type="trajectory") + return task @pytest.fixture @@ -124,6 +145,18 @@ def trajectory_start_positions(trajectory: JointTrajectory) -> dict[str, float]: ) +def _set_trajectory_positions( + coordinator: ControlCoordinator, + positions: dict[str, float] | None, +) -> None: + if positions is None: + coordinator._clear_latest_state() + return + coordinator._set_latest_state( + CoordinatorState(joints=JointStateSnapshot(joint_positions=positions)) + ) + + @pytest.fixture def coordinator_state(): """Create a sample CoordinatorState.""" @@ -296,7 +329,13 @@ def start_coordinator(tasks): ] ) _, non_eef_twist_subscribe = start_coordinator( - [TaskConfig(name="traj", type="trajectory", joint_names=["arm/joint1"])] + [ + TaskConfig( + name=JOINT_TRAJECTORY_TASK_NAME, + type="trajectory", + joint_names=["arm/joint1"], + ) + ] ) eef_twist_subscribe.assert_called_once() @@ -430,66 +469,31 @@ def test_start_stop_with_adapter_without_lifecycle_methods(self): class TestControlCoordinatorTrajectoryExecution: - def test_rejects_second_trajectory_task(self, make_coordinator): + def test_coordinator_rejects_second_canonical_trajectory_task(self, make_coordinator): coordinator = make_coordinator() first = JointTrajectoryTask( - "first", JointTrajectoryTaskConfig(joint_names=["arm/joint1"]), ) second = JointTrajectoryTask( - "second", JointTrajectoryTaskConfig(joint_names=["arm/joint2"]), ) - assert coordinator.add_task(first, task_type="trajectory") - with pytest.raises(ValueError, match="exactly one JointTrajectoryTask"): - coordinator.add_task(second, task_type="trajectory") - - assert coordinator.list_tasks() == ["first"] - - def test_removing_trajectory_task_allows_replacement(self, make_coordinator): - coordinator = make_coordinator() - first = JointTrajectoryTask( - "first", - JointTrajectoryTaskConfig(joint_names=["arm/joint1"]), - ) - second = JointTrajectoryTask( - "second", - JointTrajectoryTaskConfig(joint_names=["arm/joint2"]), - ) - coordinator.add_task(first, task_type="trajectory") - assert coordinator.remove_task("first") - assert coordinator.add_task(second, task_type="trajectory") - assert coordinator.list_tasks() == ["second"] + assert coordinator.add_task(second, task_type="trajectory") is False - def test_execute_and_cancel_without_trajectory_task_are_semantic(self, make_coordinator): - coordinator = make_coordinator() + assert coordinator.list_tasks() == [JOINT_TRAJECTORY_TASK_NAME] + with pytest.raises(RuntimeError, match="not registered"): + second.context # noqa: B018 - execute_result = coordinator.execute_trajectory(JointTrajectory()) - cancel_result = coordinator.cancel_trajectory() - - assert execute_result.status is TrajectoryExecutionStatus.NO_TRAJECTORY_TASK - assert cancel_result.status is TrajectoryCancellationStatus.NO_TRAJECTORY_TASK - - def test_execute_rejects_trajectory_when_hardware_start_differs( - self, - make_coordinator, - connected_hardware, - mock_adapter, - trajectory_task, - simple_trajectory, - ): - coordinator = make_coordinator() - mock_adapter.read_joint_positions.return_value = [0.1, 0.0, 0.0, 0.0, 0.0, 0.0] - coordinator.add_hardware(connected_hardware.adapter, connected_hardware.component) - coordinator.add_task(trajectory_task, task_type="trajectory") + def test_trajectory_factory_rejects_noncanonical_task_name(self): + config = TaskConfig( + name="custom_trajectory", + type="trajectory", + joint_names=["arm/joint1"], + ) - result = coordinator.execute_trajectory(simple_trajectory) - - assert result.status is TrajectoryExecutionStatus.START_STATE_MISMATCH - assert "arm/joint1" in result.message - assert not trajectory_task.is_active() + with pytest.raises(ValueError, match=JOINT_TRAJECTORY_TASK_NAME): + control_task_registry.create("trajectory", config) class TestJointTrajectoryTask: @@ -509,7 +513,6 @@ def test_config_requires_finite_non_negative_start_tolerance(self, tolerance): ) def test_initial_state(self, trajectory_task): - assert trajectory_task.name == "test_traj" assert not trajectory_task.is_active() assert trajectory_task.get_state() == TrajectoryState.IDLE @@ -521,9 +524,7 @@ def test_claim(self, trajectory_task): assert "arm/joint3" in claim.joints def test_execute_trajectory(self, trajectory_task, simple_trajectory): - result = trajectory_task.execute( - simple_trajectory, trajectory_start_positions(simple_trajectory) - ) + result = trajectory_task.execute(simple_trajectory) assert result.status is TrajectoryExecutionStatus.ACCEPTED assert trajectory_task.is_active() assert trajectory_task.get_state() == TrajectoryState.EXECUTING @@ -537,41 +538,55 @@ def test_execute_partial_subset_and_claims_full_configuration(self, trajectory_t ], ) - assert ( - trajectory_task.execute(trajectory, trajectory_start_positions(trajectory)).status - is TrajectoryExecutionStatus.ACCEPTED - ) + assert trajectory_task.execute(trajectory).status is TrajectoryExecutionStatus.ACCEPTED assert trajectory_task.claim().joints == frozenset( {"arm/joint1", "arm/joint2", "arm/joint3"} ) - def test_execute_rejects_missing_start_position(self, trajectory_task, simple_trajectory): - result = trajectory_task.execute( - simple_trajectory, + def test_execute_rejects_missing_start_position( + self, trajectory_task, trajectory_coordinator, simple_trajectory + ): + _set_trajectory_positions( + trajectory_coordinator, {"arm/joint1": 0.0, "arm/joint2": 0.0}, ) + result = trajectory_task.execute(simple_trajectory) assert result.status is TrajectoryExecutionStatus.START_STATE_UNAVAILABLE assert "arm/joint3" in result.message assert trajectory_task.get_state() == TrajectoryState.IDLE + def test_execute_before_first_tick_reports_state_unavailable( + self, trajectory_task, trajectory_coordinator, simple_trajectory + ): + _set_trajectory_positions(trajectory_coordinator, None) + + result = trajectory_task.execute(simple_trajectory) + + assert result.status is TrajectoryExecutionStatus.START_STATE_UNAVAILABLE + assert trajectory_task.get_state() == TrajectoryState.IDLE + def test_execute_rejects_start_position_outside_tolerance( - self, trajectory_task, simple_trajectory + self, trajectory_task, trajectory_coordinator, simple_trajectory ): current_positions = trajectory_start_positions(simple_trajectory) current_positions["arm/joint2"] = 0.051 + _set_trajectory_positions(trajectory_coordinator, current_positions) - result = trajectory_task.execute(simple_trajectory, current_positions) + result = trajectory_task.execute(simple_trajectory) assert result.status is TrajectoryExecutionStatus.START_STATE_MISMATCH assert "arm/joint2" in result.message assert trajectory_task.get_state() == TrajectoryState.IDLE - def test_execute_accepts_start_position_at_tolerance(self, trajectory_task, simple_trajectory): + def test_execute_accepts_start_position_at_tolerance( + self, trajectory_task, trajectory_coordinator, simple_trajectory + ): current_positions = trajectory_start_positions(simple_trajectory) current_positions["arm/joint2"] = 0.05 + _set_trajectory_positions(trajectory_coordinator, current_positions) - result = trajectory_task.execute(simple_trajectory, current_positions) + result = trajectory_task.execute(simple_trajectory) assert result.status is TrajectoryExecutionStatus.ACCEPTED @@ -635,7 +650,7 @@ def test_execute_accepts_start_position_at_tolerance(self, trajectory_task, simp def test_invalid_partial_inputs_reject_before_state_changes(self, trajectory_task, trajectory): assert trajectory_task.get_state() == TrajectoryState.IDLE assert ( - trajectory_task.execute(trajectory, {}).status + trajectory_task.execute(trajectory).status is TrajectoryExecutionStatus.INVALID_TRAJECTORY ) assert trajectory_task.get_state() == TrajectoryState.IDLE @@ -652,10 +667,7 @@ def test_compute_emits_active_subset_only_and_clears_on_completion(self, traject TrajectoryPoint(positions=[1.0], velocities=[0.0], time_from_start=1.0), ], ) - assert ( - trajectory_task.execute(trajectory, trajectory_start_positions(trajectory)).status - is TrajectoryExecutionStatus.ACCEPTED - ) + assert trajectory_task.execute(trajectory).status is TrajectoryExecutionStatus.ACCEPTED trajectory_task.compute(CoordinatorState(joints=MagicMock(), t_now=10.0, dt=0.01)) output = trajectory_task.compute(CoordinatorState(joints=MagicMock(), t_now=10.5, dt=0.01)) assert output is not None @@ -671,7 +683,9 @@ def test_compute_emits_active_subset_only_and_clears_on_completion(self, traject is None ) - def test_replacement_reset_and_cancel_clear_active_subset(self, trajectory_task): + def test_replacement_reset_and_cancel_clear_active_subset( + self, trajectory_task, trajectory_coordinator + ): first = JointTrajectory( joint_names=["arm/joint1"], points=[ @@ -686,14 +700,12 @@ def test_replacement_reset_and_cancel_clear_active_subset(self, trajectory_task) TrajectoryPoint(positions=[3.0], velocities=[0.0], time_from_start=1.0), ], ) - assert ( - trajectory_task.execute(first, trajectory_start_positions(first)).status - is TrajectoryExecutionStatus.ACCEPTED - ) - assert ( - trajectory_task.execute(second, trajectory_start_positions(second)).status - is TrajectoryExecutionStatus.ACCEPTED + assert trajectory_task.execute(first).status is TrajectoryExecutionStatus.ACCEPTED + _set_trajectory_positions( + trajectory_coordinator, + trajectory_start_positions(second), ) + assert trajectory_task.execute(second).status is TrajectoryExecutionStatus.ACCEPTED trajectory_task.compute(CoordinatorState(joints=MagicMock(), t_now=1.0, dt=0.01)) output = trajectory_task.compute(CoordinatorState(joints=MagicMock(), t_now=1.5, dt=0.01)) assert output is not None @@ -710,7 +722,7 @@ def test_replacement_reset_and_cancel_clear_active_subset(self, trajectory_task) def test_compute_during_trajectory(self, trajectory_task, simple_trajectory, coordinator_state): t_start = time.perf_counter() - trajectory_task.execute(simple_trajectory, trajectory_start_positions(simple_trajectory)) + trajectory_task.execute(simple_trajectory) # First compute sets start time (deferred start) state0 = CoordinatorState( @@ -735,7 +747,7 @@ def test_compute_during_trajectory(self, trajectory_task, simple_trajectory, coo def test_trajectory_completes(self, trajectory_task, simple_trajectory, coordinator_state): t_start = time.perf_counter() - trajectory_task.execute(simple_trajectory, trajectory_start_positions(simple_trajectory)) + trajectory_task.execute(simple_trajectory) # First compute sets start time (deferred start) state0 = CoordinatorState( @@ -760,7 +772,7 @@ def test_trajectory_completes(self, trajectory_task, simple_trajectory, coordina assert trajectory_task.get_state() == TrajectoryState.COMPLETED def test_cancel_trajectory(self, trajectory_task, simple_trajectory): - trajectory_task.execute(simple_trajectory, trajectory_start_positions(simple_trajectory)) + trajectory_task.execute(simple_trajectory) assert trajectory_task.is_active() result = trajectory_task.cancel() @@ -775,7 +787,7 @@ def test_cancel_when_stopped_reports_already_stopped(self, trajectory_task): assert result.status is TrajectoryCancellationStatus.ALREADY_STOPPED def test_preemption(self, trajectory_task, simple_trajectory): - trajectory_task.execute(simple_trajectory, trajectory_start_positions(simple_trajectory)) + trajectory_task.execute(simple_trajectory) trajectory_task.on_preempted("safety_task", frozenset({"arm/joint1"})) assert trajectory_task.get_state() == TrajectoryState.ABORTED @@ -783,7 +795,7 @@ def test_preemption(self, trajectory_task, simple_trajectory): def test_progress(self, trajectory_task, simple_trajectory, coordinator_state): t_start = time.perf_counter() - trajectory_task.execute(simple_trajectory, trajectory_start_positions(simple_trajectory)) + trajectory_task.execute(simple_trajectory) # First compute sets start time (deferred start) state0 = CoordinatorState( @@ -973,7 +985,7 @@ def test_tick_loop_calls_compute(self, mock_adapter): class TestIntegration: - def test_full_trajectory_execution(self, mock_adapter): + def test_full_trajectory_execution(self, make_coordinator, mock_adapter): component = HardwareComponent( hardware_id="arm", hardware_type=HardwareType.MANIPULATOR, @@ -986,8 +998,17 @@ def test_full_trajectory_execution(self, mock_adapter): joint_names=[f"arm/joint{i + 1}" for i in range(6)], priority=10, ) - traj_task = JointTrajectoryTask(name="traj_arm", config=config) - tasks = {"traj_arm": traj_task} + traj_task = JointTrajectoryTask(config) + coordinator = make_coordinator() + coordinator._set_latest_state( + CoordinatorState( + joints=JointStateSnapshot( + joint_positions={f"arm/joint{i + 1}": 0.0 for i in range(6)} + ) + ) + ) + assert coordinator.add_task(traj_task, task_type="trajectory") + tasks = {JOINT_TRAJECTORY_TASK_NAME: traj_task} joint_to_hardware = {f"arm/joint{i + 1}": "arm" for i in range(6)} @@ -998,6 +1019,7 @@ def test_full_trajectory_execution(self, mock_adapter): tasks=tasks, task_lock=threading.Lock(), joint_to_hardware=joint_to_hardware, + observation_callback=coordinator._set_latest_state, ) trajectory = JointTrajectory( @@ -1017,7 +1039,7 @@ def test_full_trajectory_execution(self, mock_adapter): ) tick_loop.start() - traj_task.execute(trajectory, trajectory_start_positions(trajectory)) + traj_task.execute(trajectory) time.sleep(0.6) tick_loop.stop() diff --git a/dimos/control/test_coordinator_commands.py b/dimos/control/test_coordinator_commands.py index 1750fe16cc..dab0e21aa8 100644 --- a/dimos/control/test_coordinator_commands.py +++ b/dimos/control/test_coordinator_commands.py @@ -33,7 +33,6 @@ import pytest -import dimos.control.coordinator as coord_mod from dimos.control.coordinator import ControlCoordinator from dimos.control.task import ( BaseControlTask, @@ -149,20 +148,9 @@ def test_execute_cancel_get_state_round_trip(self, coordinator): assert coordinator.task_invoke("traj_arm", "get_state") == "ABORTED" assert task.executed is traj - def test_injects_t_now_when_none(self, coordinator): - task = CommandRecordingTask("traj_arm") - coordinator.add_task(task, task_type="trajectory") - - result = coordinator.task_invoke("traj_arm", "record_time", {"t_now": None}) - - assert isinstance(result, float) - assert task.t_now_seen == result - - def test_unknown_task_warns_and_returns_none(self, coordinator, mocker): - warn = mocker.patch.object(coord_mod.logger, "warning") - - assert coordinator.task_invoke("nope", "execute", {}) is None - assert warn.called + def test_unknown_task_raises(self, coordinator): + with pytest.raises(KeyError, match="nope"): + coordinator.task_invoke("nope", "execute", {}) def test_unknown_method_raises(self, coordinator): task = CommandRecordingTask("traj_arm") @@ -264,16 +252,13 @@ def test_defaulted_param_can_be_omitted(self, coordinator): assert task.armed is True assert task.arm_calls == [None] - def test_undeclared_existing_method_warns_but_dispatches(self, coordinator, mocker): + def test_undeclared_existing_method_is_absent(self, coordinator): task = CommandRecordingTask("traj_arm") coordinator.add_task(task, task_type="trajectory") - warn = mocker.patch.object(coord_mod.logger, "warning") - - result = coordinator.task_invoke("traj_arm", "record_time", {"t_now": None}) - assert isinstance(result, float) # legacy dispatch still happened - assert task.t_now_seen == result - assert any("undeclared task_invoke" in str(c.args[0]) for c in warn.call_args_list) + with pytest.raises(AttributeError, match="record_time"): + coordinator.task_invoke("traj_arm", "record_time", {"t_now": None}) + assert task.t_now_seen is None def test_typo_method_raises_and_names_declared_commands(self, coordinator): task = CommandRecordingTask("traj_arm") diff --git a/dimos/control/test_coordinator_routing.py b/dimos/control/test_coordinator_routing.py index 60733c468a..b9d8268649 100644 --- a/dimos/control/test_coordinator_routing.py +++ b/dimos/control/test_coordinator_routing.py @@ -338,7 +338,13 @@ def test_twist_subscribed_for_base_hardware_without_tasks(self, make_coordinator def test_twist_not_subscribed_without_base_or_velocity_capable_task(self, make_coordinator): coordinator, taps = make_coordinator( - tasks=[TaskConfig(name="traj", type="trajectory", joint_names=ARM_JOINTS)] + tasks=[ + TaskConfig( + name="joint_trajectory", + type="trajectory", + joint_names=ARM_JOINTS, + ) + ] ) coordinator.start() @@ -500,7 +506,13 @@ def pump() -> None: class TestSubscriptionLifecycle: def test_streams_without_consumers_are_not_subscribed(self, make_coordinator): coordinator, taps = make_coordinator( - tasks=[TaskConfig(name="traj", type="trajectory", joint_names=ARM_JOINTS)] + tasks=[ + TaskConfig( + name="joint_trajectory", + type="trajectory", + joint_names=ARM_JOINTS, + ) + ] ) coordinator.start() diff --git a/dimos/control/test_task_context.py b/dimos/control/test_task_context.py new file mode 100644 index 0000000000..5d510d32b7 --- /dev/null +++ b/dimos/control/test_task_context.py @@ -0,0 +1,333 @@ +# 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. + +"""Focused contracts for immutable control observations and task context.""" + +from collections.abc import Callable, Iterator +import gc +import threading +from typing import Any +from weakref import ref + +from attrs.exceptions import FrozenInstanceError +import pytest + +from dimos.control._control_test_helpers import RecordingTask +from dimos.control.coordinator import ControlCoordinator +from dimos.control.task import ( + ControlTask, + ControlTaskContext, + CoordinatorState, + JointCommandOutput, + JointStateSnapshot, + ResourceClaim, +) +from dimos.control.tick_loop import TickLoop + + +def _state(position: float = 1.0) -> CoordinatorState: + return CoordinatorState( + joints=JointStateSnapshot( + joint_positions={"arm/joint1": position}, + joint_velocities={"arm/joint1": 2.0}, + joint_efforts={"arm/joint1": 3.0}, + timestamp=4.0, + ), + imu={}, + t_now=5.0, + dt=0.01, + ) + + +@pytest.fixture +def make_coordinator() -> Iterator[Callable[[], ControlCoordinator]]: + coordinators: list[ControlCoordinator] = [] + + def make() -> ControlCoordinator: + coordinator = ControlCoordinator(publish_joint_state=False) + coordinators.append(coordinator) + return coordinator + + try: + yield make + finally: + for coordinator in coordinators: + coordinator.stop() + + +def test_observation_fields_and_mappings_are_immutable() -> None: + source_positions = {"arm/joint1": 1.0} + source_imu = {} + joints = JointStateSnapshot(joint_positions=source_positions) + state = CoordinatorState(joints=joints, imu=source_imu) + + source_positions["arm/joint1"] = 9.0 + source_imu["body"] = None + + assert joints.joint_positions["arm/joint1"] == 1.0 + assert state.imu == {} + with pytest.raises(TypeError): + joints.joint_positions["arm/joint1"] = 2.0 # type: ignore[index] + with pytest.raises(TypeError): + state.imu["body"] = None # type: ignore[index] + with pytest.raises(FrozenInstanceError): + state.dt = 1.0 + with pytest.raises(FrozenInstanceError): + joints.timestamp = 1.0 + + +def test_context_is_frozen_and_returns_callback_value_by_identity() -> None: + state = _state() + context = ControlTaskContext(get_state=lambda: state) + + assert context.get_state() is state + with pytest.raises(FrozenInstanceError): + context.get_state = lambda: None # type: ignore[method-assign] + + +class ObservationTask(RecordingTask): + def __init__( + self, + name: str, + *, + active: bool, + published: list[CoordinatorState], + computed: list[CoordinatorState], + ) -> None: + super().__init__(name) + self._active = active + self._published = published + self._computed = computed + + def is_active(self) -> bool: + assert len(self._published) == 1 + return self._active + + def compute(self, state: CoordinatorState) -> None: + self._computed.append(state) + return None + + +def test_tick_publishes_complete_observation_before_filtering_and_shares_identity() -> None: + published: list[CoordinatorState] = [] + seen_by_compute: list[CoordinatorState] = [] + inactive = ObservationTask( + "inactive", + active=False, + published=published, + computed=seen_by_compute, + ) + active = ObservationTask( + "active", + active=True, + published=published, + computed=seen_by_compute, + ) + loop = TickLoop( + tick_rate=100.0, + hardware={}, + hardware_lock=threading.Lock(), + tasks={"inactive": inactive, "active": active}, + task_lock=threading.Lock(), + joint_to_hardware={}, + observation_callback=published.append, + ) + + loop._tick() + + assert seen_by_compute == [published[0]] + assert published[0].joints.timestamp > 0.0 + + +def test_coordinator_state_is_none_then_atomically_replaced(make_coordinator) -> None: + coordinator = make_coordinator() + task = RecordingTask("task") + coordinator.add_task(task) + first = _state(1.0) + second = _state(2.0) + + assert task.context.get_state() is None + coordinator._set_latest_state(first) + assert task.context.get_state() is first + coordinator._set_latest_state(second) + assert task.context.get_state() is second + + +def test_state_publication_completes_while_task_lock_is_held(make_coordinator) -> None: + coordinator = make_coordinator() + task = RecordingTask("task") + coordinator.add_task(task) + entered = threading.Event() + release = threading.Event() + published = threading.Event() + + def hold_task_lock() -> None: + with coordinator._task_lock: + entered.set() + release.wait(timeout=1.0) + + def publish_state() -> None: + coordinator._set_latest_state(_state()) + published.set() + + holder = threading.Thread(target=hold_task_lock) + holder.start() + update = threading.Thread(target=publish_state) + try: + assert entered.wait(timeout=1.0) + update.start() + assert published.wait(timeout=1.0) + finally: + release.set() + holder.join(timeout=1.0) + if update.ident is not None: + update.join(timeout=1.0) + + assert not holder.is_alive() + assert not update.is_alive() + assert task.context.get_state() is not None + + +class StructuralTask: + """Protocol-compatible task intentionally not derived from BaseControlTask.""" + + name = "structural" + + def claim(self) -> ResourceClaim: + return ResourceClaim(joints=frozenset()) + + def is_active(self) -> bool: + return False + + def compute(self, state: CoordinatorState) -> JointCommandOutput | None: + return None + + def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: + pass + + def on_buttons(self, msg: Any) -> bool: + return False + + def on_cartesian_command(self, pose: Any, t_now: float) -> bool: + return False + + def on_ee_twist_command(self, twist: Any, t_now: float) -> bool: + return False + + def set_target_by_name(self, positions: dict[str, float], t_now: float) -> bool: + return False + + def set_velocities_by_name(self, velocities: dict[str, float], t_now: float) -> bool: + return False + + def reset_runtime_state(self, reactivate: bool | None = None) -> bool: + return False + + +def test_registration_grants_base_task_context(make_coordinator) -> None: + coordinator = make_coordinator() + base = RecordingTask("base") + + assert coordinator.add_task(base) + assert base.context is coordinator._task_context + + +def test_registration_accepts_structural_tasks(make_coordinator) -> None: + coordinator = make_coordinator() + structural = StructuralTask() + + assert coordinator.add_task(structural) + assert isinstance(structural, ControlTask) + assert coordinator.list_tasks() == ["structural"] + + +def test_registration_rejects_task_bound_to_another_coordinator(make_coordinator) -> None: + first = make_coordinator() + second = make_coordinator() + shared = RecordingTask("shared") + assert first.add_task(shared) + + with pytest.raises(RuntimeError, match="another coordinator"): + second.add_task(shared) + assert second.list_tasks() == [] + + +def test_failed_routing_registration_releases_context_access(make_coordinator) -> None: + coordinator = make_coordinator() + failing = RecordingTask("failing") + + with pytest.raises(ValueError, match="no input port"): + coordinator.add_task( + failing, + task_type="servo", + stream_bind={"joint_command": "missing_port"}, + ) + assert coordinator.list_tasks() == [] + with pytest.raises(RuntimeError, match="not registered"): + failing.context # noqa: B018 + + +def test_removal_revokes_context_and_allows_registration_with_another_coordinator( + make_coordinator, +) -> None: + first = make_coordinator() + second = make_coordinator() + task = RecordingTask("task") + first.add_task(task) + + assert first.remove_task(task.name) + with pytest.raises(RuntimeError, match="not registered"): + task.context # noqa: B018 + assert second.add_task(task) + assert task.context is second._task_context + + +def test_coordinator_deletion_revokes_context_access() -> None: + coordinator = ControlCoordinator(publish_joint_state=False) + task = RecordingTask("task") + coordinator.add_task(task) + coordinator_ref = ref(coordinator) + + coordinator.stop() + assert task.context is coordinator._task_context + del coordinator + gc.collect() + + assert coordinator_ref() is None + with pytest.raises(RuntimeError, match="not registered"): + task.context # noqa: B018 + + +def test_stop_clears_state_without_detaching_tasks(make_coordinator) -> None: + coordinator = make_coordinator() + task = RecordingTask("task") + coordinator.add_task(task) + original_context = task.context + + coordinator._set_latest_state(_state()) + coordinator.stop() + assert task.context is original_context + assert task.context.get_state() is None + + +def test_runtime_reset_clears_state_without_detaching_tasks(make_coordinator) -> None: + coordinator = make_coordinator() + task = RecordingTask("task") + coordinator.add_task(task) + original_context = task.context + + coordinator._set_latest_state(_state()) + assert coordinator.reset_runtime_state() == {} + assert task.context is original_context + assert task.context.get_state() is None diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index 3152bba393..2f62a4276d 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -95,6 +95,7 @@ def __init__( tasks: dict[TaskName, ControlTask], task_lock: threading.Lock, joint_to_hardware: dict[JointName, HardwareId], + observation_callback: Callable[[CoordinatorState], None] | None = None, publish_callback: Callable[[JointState], None] | None = None, publish_robot_callback: Callable[[HardwareId, JointState], None] | None = None, frame_id: str = "coordinator", @@ -106,6 +107,7 @@ def __init__( self._tasks = tasks self._task_lock = task_lock self._joint_to_hardware = joint_to_hardware + self._observation_callback = observation_callback self._publish_callback = publish_callback self._publish_robot_callback = publish_robot_callback self._frame_id = frame_id @@ -181,6 +183,9 @@ def _tick(self) -> None: imu_states = self._read_all_imu() state = CoordinatorState(joints=joint_states, imu=imu_states, t_now=t_now, dt=dt) + if self._observation_callback: + self._observation_callback(state) + commands = self._compute_all_tasks(state) joint_commands, preemptions = self._arbitrate(commands) diff --git a/dimos/e2e_tests/test_control_coordinator.py b/dimos/e2e_tests/test_control_coordinator.py index 40e4800b47..b81df0ecb0 100644 --- a/dimos/e2e_tests/test_control_coordinator.py +++ b/dimos/e2e_tests/test_control_coordinator.py @@ -23,6 +23,11 @@ import pytest from dimos.control.coordinator import ControlCoordinator +from dimos.control.tasks.trajectory_task.trajectory_task import ( + JOINT_TRAJECTORY_TASK_NAME, + TrajectoryCancellationStatus, + TrajectoryExecutionStatus, +) from dimos.core.rpc_client import RPCClient from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory @@ -30,6 +35,20 @@ from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState +def _wait_for_trajectory_state( + client: RPCClient, + expected: TrajectoryState, + *, + timeout: float = 5.0, +) -> bool: + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + if client.task_invoke(JOINT_TRAJECTORY_TASK_NAME, "get_state") == expected: + return True + time.sleep(0.1) + return False + + @pytest.mark.skipif_in_ci class TestControlCoordinatorE2E: """End-to-end tests for ControlCoordinator.""" @@ -60,7 +79,7 @@ def test_coordinator_starts_and_responds_to_rpc(self, lcm_spy, start_blueprint) # Test list_tasks RPC tasks = client.list_tasks() assert tasks is not None - assert "traj_arm" in tasks + assert JOINT_TRAJECTORY_TASK_NAME in tasks # Test list_hardware RPC hardware = client.list_hardware() @@ -105,22 +124,12 @@ def test_coordinator_executes_trajectory(self, lcm_spy, start_blueprint) -> None ) # Execute trajectory via task_invoke - result = client.task_invoke("traj_arm", "execute", {"trajectory": trajectory}) - assert result is True - - # Poll for completion - timeout = 5.0 - start_time = time.time() - completed = False - - while time.time() - start_time < timeout: - state = client.task_invoke("traj_arm", "get_state") - if state is not None and state == TrajectoryState.COMPLETED: - completed = True - break - time.sleep(0.1) - - assert completed, "Trajectory did not complete within timeout" + result = client.task_invoke( + JOINT_TRAJECTORY_TASK_NAME, "execute", {"trajectory": trajectory} + ) + assert result.status is TrajectoryExecutionStatus.ACCEPTED + + assert _wait_for_trajectory_state(client, TrajectoryState.COMPLETED) finally: client.stop_rpc_client() @@ -182,23 +191,27 @@ def test_coordinator_cancel_trajectory(self, lcm_spy, start_blueprint) -> None: ) # Start trajectory via task_invoke - result = client.task_invoke("traj_arm", "execute", {"trajectory": trajectory}) - assert result is True + result = client.task_invoke( + JOINT_TRAJECTORY_TASK_NAME, "execute", {"trajectory": trajectory} + ) + assert result.status is TrajectoryExecutionStatus.ACCEPTED - # Wait a bit then cancel - time.sleep(0.5) - cancel_result = client.task_invoke("traj_arm", "cancel") - assert cancel_result is True + assert ( + client.task_invoke(JOINT_TRAJECTORY_TASK_NAME, "get_state") + == TrajectoryState.EXECUTING + ) + cancel_result = client.task_invoke(JOINT_TRAJECTORY_TASK_NAME, "cancel") + assert cancel_result.status is TrajectoryCancellationStatus.CANCELLED # Check status is ABORTED - state = client.task_invoke("traj_arm", "get_state") + state = client.task_invoke(JOINT_TRAJECTORY_TASK_NAME, "get_state") assert state is not None assert state == TrajectoryState.ABORTED finally: client.stop_rpc_client() def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: - """Test dual-arm coordinator with independent trajectories.""" + """Test the canonical trajectory task across both arms.""" lcm_spy.save_topic("/coordinator_joint_state#sensor_msgs.JointState") # Start dual-arm mock coordinator @@ -212,44 +225,30 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: assert "left_arm/joint1" in joints assert "right_arm/joint1" in joints - tasks = client.list_tasks() - assert "traj_left" in tasks - assert "traj_right" in tasks - - # Create trajectories for both arms - left_trajectory = JointTrajectory( - joint_names=[f"left_arm/joint{i + 1}" for i in range(7)], - points=[ - TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 7), - TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 7), + combined = JointTrajectory( + joint_names=[ + *[f"left_arm/joint{i + 1}" for i in range(7)], + *[f"right_arm/joint{i + 1}" for i in range(6)], ], - ) - - right_trajectory = JointTrajectory( - joint_names=[f"right_arm/joint{i + 1}" for i in range(6)], points=[ - TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 6), - TrajectoryPoint(time_from_start=0.5, positions=[0.3] * 6), + TrajectoryPoint( + time_from_start=0.0, + positions=[0.0] * 13, + velocities=[0.0] * 13, + ), + TrajectoryPoint( + time_from_start=0.5, + positions=[*[0.2] * 7, *[0.3] * 6], + velocities=[0.0] * 13, + ), ], ) - - # Execute both via task_invoke - assert ( - client.task_invoke("traj_left", "execute", {"trajectory": left_trajectory}) is True - ) - assert ( - client.task_invoke("traj_right", "execute", {"trajectory": right_trajectory}) - is True + result = client.task_invoke( + JOINT_TRAJECTORY_TASK_NAME, + "execute", + {"trajectory": combined}, ) - - # Wait for completion - time.sleep(1.0) - - # Both should complete - left_state = client.task_invoke("traj_left", "get_state") - right_state = client.task_invoke("traj_right", "get_state") - - assert left_state == TrajectoryState.COMPLETED - assert right_state == TrajectoryState.COMPLETED + assert result.status is TrajectoryExecutionStatus.ACCEPTED + assert _wait_for_trajectory_state(client, TrajectoryState.COMPLETED) finally: client.stop_rpc_client() diff --git a/dimos/e2e_tests/test_manipulation_planning_groups.py b/dimos/e2e_tests/test_manipulation_planning_groups.py index 390db63fde..05de3a0c7e 100644 --- a/dimos/e2e_tests/test_manipulation_planning_groups.py +++ b/dimos/e2e_tests/test_manipulation_planning_groups.py @@ -34,7 +34,6 @@ from dimos.manipulation.manipulation_module import ManipulationModule from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.robot.manipulators.common.topics import DEFAULT_TRAJECTORY_TASK_NAME pytestmark = [pytest.mark.self_hosted_large] @@ -166,9 +165,6 @@ def test_single_arm_plans_and_executes_through_control_coordinator( left_info = _wait_for_robot_info(client, "left_arm") left_id = _planning_group_id(left_info) - tasks = coordinator_client.list_tasks() - assert tasks == [DEFAULT_TRAJECTORY_TASK_NAME] - _prepare_for_planning(client, ("left_arm",)) planned = client.plan_to_joint_targets({left_id: _offset_target(client, "left_arm", 0.02)}) @@ -197,9 +193,6 @@ def test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator( left_id = _planning_group_id(left_info) right_id = _planning_group_id(right_info) - tasks = coordinator_client.list_tasks() - assert tasks == [DEFAULT_TRAJECTORY_TASK_NAME] - _prepare_for_planning(client, ("left_arm", "right_arm")) planned = client.plan_to_joint_targets( diff --git a/dimos/manipulation/conftest.py b/dimos/manipulation/conftest.py index ded73ddf1a..2bac4a0b8c 100644 --- a/dimos/manipulation/conftest.py +++ b/dimos/manipulation/conftest.py @@ -39,12 +39,15 @@ def __call__(self, coordinator: ControlCoordinator | None = None) -> Manipulatio def _mock_control_coordinator() -> MagicMock: """Create a coordinator reference with safe default execution results.""" coordinator = MagicMock(spec=ControlCoordinator) - coordinator.execute_trajectory.return_value = TrajectoryExecutionResult( - TrajectoryExecutionStatus.ACCEPTED - ) - coordinator.cancel_trajectory.return_value = TrajectoryCancellationResult( - TrajectoryCancellationStatus.ALREADY_STOPPED - ) + + def task_invoke(_task_name: str, method: str, _kwargs: Any = None) -> object: + if method == "execute": + return TrajectoryExecutionResult(TrajectoryExecutionStatus.ACCEPTED) + if method == "cancel": + return TrajectoryCancellationResult(TrajectoryCancellationStatus.ALREADY_STOPPED) + raise AssertionError(f"unexpected task command {method}") + + coordinator.task_invoke.side_effect = task_invoke return coordinator diff --git a/dimos/manipulation/control/coordinator_client.py b/dimos/manipulation/control/coordinator_client.py index 93a22be8f0..6da605c6de 100644 --- a/dimos/manipulation/control/coordinator_client.py +++ b/dimos/manipulation/control/coordinator_client.py @@ -28,15 +28,14 @@ # Terminal 2: Run this client python -m dimos.manipulation.control.coordinator_client - python -m dimos.manipulation.control.coordinator_client --task traj_left - python -m dimos.manipulation.control.coordinator_client --task traj_right + python -m dimos.manipulation.control.coordinator_client --task joint_trajectory How it works: 1. Connects to ControlCoordinator via LCM RPC 2. Queries available hardware/tasks/joints 3. You add waypoints (joint positions) 4. Generates trajectory with trapezoidal velocity profile - 5. Sends trajectory to coordinator via execute_trajectory() RPC + 5. Invokes the canonical trajectory task through the generic task-command RPC 6. Coordinator's tick loop executes it at 100Hz """ @@ -44,11 +43,12 @@ import math import sys -from typing import TYPE_CHECKING, cast +from typing import TYPE_CHECKING, Any, cast from dimos.control.components import split_joint_name from dimos.control.coordinator import ControlCoordinator from dimos.control.tasks.trajectory_task.trajectory_task import ( + JOINT_TRAJECTORY_TASK_NAME, TrajectoryCancellationResult, TrajectoryCancellationStatus, TrajectoryExecutionResult, @@ -77,10 +77,10 @@ class CoordinatorClient: # Query state print(client.list_hardware()) # ['left_arm', 'right_arm'] - print(client.list_tasks()) # ['traj_left', 'traj_right'] + print(client.list_tasks()) # ['joint_trajectory'] # Setup for a task - client.select_task("traj_left") + client.select_task(JOINT_TRAJECTORY_TASK_NAME) # Get current position and create trajectory current = client.get_current_positions() @@ -88,7 +88,11 @@ class CoordinatorClient: trajectory = client.generate_trajectory([current, target]) # Execute - client.execute_trajectory(trajectory) + client.task_invoke( + JOINT_TRAJECTORY_TASK_NAME, + "execute", + {"trajectory": trajectory}, + ) """ def __init__(self) -> None: @@ -124,19 +128,9 @@ def get_joint_positions(self) -> dict[str, float]: """Get current joint positions for all joints.""" return self._rpc.get_joint_positions() or {} - def execute_trajectory(self, trajectory: JointTrajectory) -> TrajectoryExecutionResult: - """Execute through the coordinator's sole trajectory task.""" - return cast( - "TrajectoryExecutionResult", - self._rpc.execute_trajectory(trajectory), - ) - - def cancel_trajectory(self) -> TrajectoryCancellationResult: - """Cancel the coordinator's sole trajectory task.""" - return cast( - "TrajectoryCancellationResult", - self._rpc.cancel_trajectory(), - ) + def task_invoke(self, task_name: str, method: str, kwargs: dict[str, Any] | None = None) -> Any: + """Invoke a command exposed by a registered control task.""" + return self._rpc.task_invoke(task_name, method, kwargs) def select_task(self, task_name: str) -> bool: """ @@ -152,25 +146,9 @@ def select_task(self, task_name: str) -> bool: self._current_task = task_name - # Get joints for this task (infer from task name pattern) - # e.g., "traj_left" -> joints starting with "left_arm_" (hardware_id based naming) - # e.g., "traj_arm" -> joints starting with "arm_" + # The canonical trajectory task can span every configured manipulator. all_joints = self.list_joints() - - # Try to infer hardware_id from task name - if "_" in task_name: - suffix = task_name.split("_", 1)[1] # "traj_left" -> "left" - # Try both patterns: exact suffix (e.g., "arm/") and with "_arm" suffix (e.g., "left_arm/") - task_joints = [j for j in all_joints if j.startswith(suffix + "/")] - if not task_joints: - # Try with "_arm" suffix for dual-arm setups (left -> left_arm) - task_joints = [j for j in all_joints if j.startswith(suffix + "_arm/")] - else: - task_joints = all_joints - - if not task_joints: - # Fallback: use all joints - task_joints = all_joints + task_joints = all_joints self._task_joints[task_name] = task_joints @@ -428,7 +406,14 @@ def run(self) -> None: preview_trajectory(self._generated_trajectory, self._joints()) confirm = input("\nExecute? [y/N]: ").strip().lower() if confirm == "y": - result = self._client.execute_trajectory(self._generated_trajectory) + result = cast( + "TrajectoryExecutionResult", + self._client.task_invoke( + JOINT_TRAJECTORY_TASK_NAME, + "execute", + {"trajectory": self._generated_trajectory}, + ), + ) if result.status is TrajectoryExecutionStatus.ACCEPTED: print( "Trajectory accepted " @@ -445,7 +430,10 @@ def status(self) -> None: def cancel(self) -> None: """Cancel active trajectory.""" - result = self._client.cancel_trajectory() + result = cast( + "TrajectoryCancellationResult", + self._client.task_invoke(JOINT_TRAJECTORY_TASK_NAME, "cancel"), + ) if result.status is TrajectoryCancellationStatus.CANCELLED: print("Cancelled") else: @@ -615,18 +603,15 @@ def main() -> int: # Single arm (with coordinator-mock running) python -m dimos.manipulation.control.coordinator_client - # Dual arm - control left arm - python -m dimos.manipulation.control.coordinator_client --task traj_left - - # Dual arm - control right arm - python -m dimos.manipulation.control.coordinator_client --task traj_right + # Explicit canonical task selection + python -m dimos.manipulation.control.coordinator_client --task joint_trajectory """, ) parser.add_argument( "--task", type=str, - default="traj_arm", - help="Initial task to control (default: traj_arm)", + default=JOINT_TRAJECTORY_TASK_NAME, + help=f"Initial task to control (default: {JOINT_TRAJECTORY_TASK_NAME})", ) parser.add_argument( "--vel", diff --git a/dimos/manipulation/execution_manager.py b/dimos/manipulation/execution_manager.py index 4cab26178c..fd4dc533d3 100644 --- a/dimos/manipulation/execution_manager.py +++ b/dimos/manipulation/execution_manager.py @@ -20,11 +20,13 @@ from enum import Enum, auto import threading from types import MappingProxyType +from typing import cast import attrs from dimos.control.coordinator import ControlCoordinator from dimos.control.tasks.trajectory_task.trajectory_task import ( + JOINT_TRAJECTORY_TASK_NAME, TrajectoryCancellationResult, TrajectoryCancellationStatus, TrajectoryExecutionResult, @@ -190,7 +192,14 @@ def execute(self, plan: GeneratedPlan) -> ExecutionDispatchResult: ) try: - result = self._coordinator.execute_trajectory(trajectory) + result = cast( + "TrajectoryExecutionResult", + self._coordinator.task_invoke( + JOINT_TRAJECTORY_TASK_NAME, + "execute", + {"trajectory": trajectory}, + ), + ) except Exception as exc: logger.exception("Coordinator execute RPC failed") return ExecutionDispatchResult( @@ -214,7 +223,13 @@ def cancel(self) -> TrajectoryCancellationResult: """Cancel coordinator trajectory execution.""" with self._operation_lock: try: - return self._coordinator.cancel_trajectory() + return cast( + "TrajectoryCancellationResult", + self._coordinator.task_invoke( + JOINT_TRAJECTORY_TASK_NAME, + "cancel", + ), + ) except Exception as exc: logger.exception("Coordinator cancel RPC failed") return TrajectoryCancellationResult( diff --git a/dimos/manipulation/planning/README.md b/dimos/manipulation/planning/README.md index 2e31f6266c..a3752dd374 100644 --- a/dimos/manipulation/planning/README.md +++ b/dimos/manipulation/planning/README.md @@ -186,5 +186,5 @@ planning/ pytest dimos/manipulation/test_manipulation_unit.py -v # Integration tests (requires Drake) -pytest dimos/e2e_tests/test_manipulation_module.py -v +pytest dimos/manipulation/test_manipulation_drake_integration.py -v ``` diff --git a/dimos/manipulation/test_execution_manager.py b/dimos/manipulation/test_execution_manager.py index be12f9053e..49887c4c3d 100644 --- a/dimos/manipulation/test_execution_manager.py +++ b/dimos/manipulation/test_execution_manager.py @@ -21,6 +21,7 @@ from dimos.control.coordinator import ControlCoordinator from dimos.control.tasks.trajectory_task.trajectory_task import ( + JOINT_TRAJECTORY_TASK_NAME, TrajectoryCancellationResult, TrajectoryCancellationStatus, TrajectoryExecutionResult, @@ -86,12 +87,16 @@ def _plan( def _coordinator() -> MagicMock: coordinator = MagicMock(spec=ControlCoordinator) - coordinator.execute_trajectory.return_value = TrajectoryExecutionResult( - TrajectoryExecutionStatus.ACCEPTED - ) - coordinator.cancel_trajectory.return_value = TrajectoryCancellationResult( - TrajectoryCancellationStatus.ALREADY_STOPPED - ) + + def task_invoke(task_name: str, method: str, kwargs: dict[str, object] | None = None) -> object: + assert task_name == JOINT_TRAJECTORY_TASK_NAME + if method == "execute": + return TrajectoryExecutionResult(TrajectoryExecutionStatus.ACCEPTED) + if method == "cancel": + return TrajectoryCancellationResult(TrajectoryCancellationStatus.ALREADY_STOPPED) + raise AssertionError(f"unexpected command {method}") + + coordinator.task_invoke.side_effect = task_invoke return coordinator @@ -161,8 +166,12 @@ def test_execute_maps_all_robots_into_one_trajectory() -> None: result = manager.execute(plan) assert result.outcome is ExecutionOutcome.ACCEPTED - coordinator.execute_trajectory.assert_called_once() - trajectory = coordinator.execute_trajectory.call_args.args[0] + trajectory = coordinator.task_invoke.call_args.args[2]["trajectory"] + coordinator.task_invoke.assert_called_once_with( + JOINT_TRAJECTORY_TASK_NAME, + "execute", + {"trajectory": trajectory}, + ) assert trajectory.joint_names == ["left_hw/j1", "right_hw/j1"] assert trajectory.points == plan.trajectory.points assert trajectory.timestamp == plan.trajectory.timestamp @@ -179,7 +188,12 @@ def test_execute_preserves_single_robot_subset() -> None: result = manager.execute(_plan(("left/j2",))) assert result.accepted - trajectory = coordinator.execute_trajectory.call_args.args[0] + trajectory = coordinator.task_invoke.call_args.args[2]["trajectory"] + coordinator.task_invoke.assert_called_once_with( + JOINT_TRAJECTORY_TASK_NAME, + "execute", + {"trajectory": trajectory}, + ) assert trajectory.joint_names == ["j2"] assert trajectory.points[0].positions == [0.0] @@ -204,7 +218,7 @@ def test_execute_rejects_unmappable_plan_before_rpc( assert result.outcome is ExecutionOutcome.REJECTED assert message in result.message - coordinator.execute_trajectory.assert_not_called() + coordinator.task_invoke.assert_not_called() def test_execute_rejects_cross_robot_mapping_collision() -> None: @@ -227,13 +241,12 @@ def test_execute_rejects_cross_robot_mapping_collision() -> None: assert result.outcome is ExecutionOutcome.REJECTED assert "duplicate coordinator joints" in result.message - coordinator.execute_trajectory.assert_not_called() + coordinator.task_invoke.assert_not_called() @pytest.mark.parametrize( "status", [ - TrajectoryExecutionStatus.NO_TRAJECTORY_TASK, TrajectoryExecutionStatus.INVALID_TRAJECTORY, TrajectoryExecutionStatus.START_STATE_UNAVAILABLE, TrajectoryExecutionStatus.START_STATE_MISMATCH, @@ -242,7 +255,8 @@ def test_execute_rejects_cross_robot_mapping_collision() -> None: def test_execute_preserves_coordinator_rejection(status: TrajectoryExecutionStatus) -> None: coordinator = _coordinator() coordinator_result = TrajectoryExecutionResult(status, "specific rejection") - coordinator.execute_trajectory.return_value = coordinator_result + coordinator.task_invoke.side_effect = None + coordinator.task_invoke.return_value = coordinator_result manager = _manager(coordinator=coordinator) result = manager.execute(_plan()) @@ -254,12 +268,18 @@ def test_execute_preserves_coordinator_rejection(status: TrajectoryExecutionStat def test_execute_rpc_failure_is_uncertain() -> None: coordinator = _coordinator() - coordinator.execute_trajectory.side_effect = TimeoutError("timed out") + coordinator.task_invoke.side_effect = TimeoutError("timed out") result = _manager(coordinator=coordinator).execute(_plan()) assert result.outcome is ExecutionOutcome.UNCERTAIN assert "timed out" in result.message + trajectory = coordinator.task_invoke.call_args.args[2]["trajectory"] + coordinator.task_invoke.assert_called_once_with( + JOINT_TRAJECTORY_TASK_NAME, + "execute", + {"trajectory": trajectory}, + ) @pytest.mark.parametrize( @@ -275,11 +295,6 @@ def test_execute_rpc_failure_is_uncertain() -> None: True, False, ), - ( - TrajectoryCancellationStatus.NO_TRAJECTORY_TASK, - True, - False, - ), ], ) def test_cancel_preserves_coordinator_semantics( @@ -289,18 +304,23 @@ def test_cancel_preserves_coordinator_semantics( ) -> None: coordinator = _coordinator() coordinator_result = TrajectoryCancellationResult(status, "cancel result") - coordinator.cancel_trajectory.return_value = coordinator_result + coordinator.task_invoke.side_effect = None + coordinator.task_invoke.return_value = coordinator_result result = _manager(coordinator=coordinator).cancel() assert result is coordinator_result assert result.safe is safe assert result.cancelled is cancelled + coordinator.task_invoke.assert_called_once_with( + JOINT_TRAJECTORY_TASK_NAME, + "cancel", + ) def test_cancel_rpc_failure_is_uncertain() -> None: coordinator = _coordinator() - coordinator.cancel_trajectory.side_effect = TimeoutError("timed out") + coordinator.task_invoke.side_effect = TimeoutError("timed out") result = _manager(coordinator=coordinator).cancel() @@ -308,48 +328,62 @@ def test_cancel_rpc_failure_is_uncertain() -> None: assert not result.safe assert not result.cancelled assert "timed out" in result.message + coordinator.task_invoke.assert_called_once_with( + JOINT_TRAJECTORY_TASK_NAME, + "cancel", + ) def test_cancel_waits_for_in_flight_execute_then_cancels() -> None: coordinator = _coordinator() execute_started = Event() release_execute = Event() - - def execute_trajectory(_trajectory: JointTrajectory) -> TrajectoryExecutionResult: - execute_started.set() - if not release_execute.wait(timeout=1.0): - raise TimeoutError("test did not release execute RPC") - return TrajectoryExecutionResult(TrajectoryExecutionStatus.ACCEPTED) - - coordinator.execute_trajectory.side_effect = execute_trajectory + cancel_requested = Event() + cancel_invoked = Event() + + def task_invoke( + _task_name: str, method: str, _kwargs: dict[str, object] | None = None + ) -> object: + if method == "execute": + execute_started.set() + if not release_execute.wait(timeout=1.0): + raise TimeoutError("test did not release execute RPC") + return TrajectoryExecutionResult(TrajectoryExecutionStatus.ACCEPTED) + cancel_invoked.set() + return TrajectoryCancellationResult(TrajectoryCancellationStatus.ALREADY_STOPPED) + + coordinator.task_invoke.side_effect = task_invoke manager = _manager(coordinator=coordinator) execute_results: list[ExecutionDispatchResult] = [] cancel_results: list[TrajectoryCancellationResult] = [] execute_thread = Thread(target=lambda: execute_results.append(manager.execute(_plan()))) - cancel_thread = Thread(target=lambda: cancel_results.append(manager.cancel())) + + def cancel() -> None: + cancel_requested.set() + cancel_results.append(manager.cancel()) + + cancel_thread = Thread(target=cancel) execute_thread.start() - execute_was_started = execute_started.wait(timeout=1.0) - cancel_was_started = False - cancel_called_before_release = False try: - if execute_was_started: - cancel_thread.start() - cancel_was_started = True - cancel_called_before_release = coordinator.cancel_trajectory.called + assert execute_started.wait(timeout=1.0) + cancel_thread.start() + assert cancel_requested.wait(timeout=1.0) + assert not cancel_invoked.wait(timeout=0.1) finally: release_execute.set() execute_thread.join(timeout=1.0) - if cancel_was_started: + if cancel_thread.ident is not None: cancel_thread.join(timeout=1.0) - assert execute_was_started assert not execute_thread.is_alive() - assert cancel_was_started assert not cancel_thread.is_alive() - assert not cancel_called_before_release + assert cancel_invoked.is_set() assert len(execute_results) == 1 assert execute_results[0].outcome is ExecutionOutcome.ACCEPTED assert len(cancel_results) == 1 assert cancel_results[0].status is TrajectoryCancellationStatus.ALREADY_STOPPED - coordinator.cancel_trajectory.assert_called_once_with() + assert coordinator.task_invoke.call_args_list[-1].args == ( + JOINT_TRAJECTORY_TASK_NAME, + "cancel", + ) diff --git a/dimos/manipulation/test_manipulation_module.py b/dimos/manipulation/test_manipulation_drake_integration.py similarity index 75% rename from dimos/manipulation/test_manipulation_module.py rename to dimos/manipulation/test_manipulation_drake_integration.py index d313d5ad2a..e295f5a81b 100644 --- a/dimos/manipulation/test_manipulation_module.py +++ b/dimos/manipulation/test_manipulation_drake_integration.py @@ -28,6 +28,7 @@ from dimos.control.coordinator import ControlCoordinator from dimos.control.tasks.trajectory_task.trajectory_task import ( + JOINT_TRAJECTORY_TASK_NAME, TrajectoryCancellationResult, TrajectoryCancellationStatus, TrajectoryExecutionResult, @@ -124,12 +125,15 @@ def joint_state_zeros(): def module(xarm7_config): """Create a started ManipulationModule with ports disabled.""" coordinator = MagicMock(spec=ControlCoordinator) - coordinator.execute_trajectory.return_value = TrajectoryExecutionResult( - TrajectoryExecutionStatus.ACCEPTED - ) - coordinator.cancel_trajectory.return_value = TrajectoryCancellationResult( - TrajectoryCancellationStatus.ALREADY_STOPPED - ) + + def task_invoke(_task_name: str, method: str, _kwargs: object = None) -> object: + if method == "execute": + return TrajectoryExecutionResult(TrajectoryExecutionStatus.ACCEPTED) + if method == "cancel": + return TrajectoryCancellationResult(TrajectoryCancellationStatus.ALREADY_STOPPED) + raise AssertionError(f"unexpected task command {method}") + + coordinator.task_invoke.side_effect = task_invoke mod = ManipulationModule( robots=[xarm7_config], planning_timeout=10.0, @@ -248,28 +252,8 @@ def test_ee_pose(self, module, joint_state_zeros): assert hasattr(pose, "y") assert hasattr(pose, "z") - def test_trajectory_name_translation(self, module, joint_state_zeros): - """Test that trajectory joint names are translated for coordinator.""" - module._on_joint_state(joint_state_zeros) - - success = module.plan_to_joints(JointState(position=[0.05] * 7)) - assert success is True - - assert module._last_plan is not None - robot_config = module._robots["test_arm"][1] - assert module.execute() is True - trajectory = module._control_coordinator.execute_trajectory.call_args.args[0] - - assert trajectory.joint_names == list(robot_config.joint_name_mapping.keys()) - - -@pytest.mark.skipif(not _drake_available(), reason="Drake not installed") -@pytest.mark.skipif(not _xarm_urdf_available(), reason="XArm URDF not available") -class TestCoordinatorIntegration: - """Test coordinator integration with mocked RPC client.""" - - def test_execute_with_mock_coordinator(self, module, joint_state_zeros): - """Test execute sends trajectory to coordinator.""" + def test_plan_and_dispatch_to_coordinator(self, module, joint_state_zeros): + """Test that a real Drake plan is translated and dispatched.""" module._on_joint_state(joint_state_zeros) success = module.plan_to_joints(JointState(position=[0.05] * 7)) @@ -280,48 +264,14 @@ def test_execute_with_mock_coordinator(self, module, joint_state_zeros): assert result is True assert module._state == ManipulationState.COMPLETED - # Verify coordinator was called - module._control_coordinator.execute_trajectory.assert_called_once() - trajectory = module._control_coordinator.execute_trajectory.call_args.args[0] + trajectory = module._control_coordinator.task_invoke.call_args.args[2]["trajectory"] + module._control_coordinator.task_invoke.assert_called_once_with( + JOINT_TRAJECTORY_TASK_NAME, + "execute", + {"trajectory": trajectory}, + ) assert len(trajectory.points) > 1 # Joint names should be translated robot_config = module._robots["test_arm"][1] assert trajectory.joint_names == list(robot_config.joint_name_mapping.keys()) - - def test_execute_rejected_by_coordinator(self, module, joint_state_zeros): - """Test handling of coordinator rejection.""" - module._on_joint_state(joint_state_zeros) - - module.plan_to_joints(JointState(position=[0.05] * 7)) - - module._control_coordinator.execute_trajectory.return_value = TrajectoryExecutionResult( - TrajectoryExecutionStatus.INVALID_TRAJECTORY - ) - - result = module.execute() - - assert result is False - assert module._state == ManipulationState.COMPLETED - assert "rejected" in module._error_message.lower() - - def test_state_transitions_during_execution(self, module, joint_state_zeros): - """Test state transitions during plan and execute.""" - assert module._state == ManipulationState.IDLE - - module._on_joint_state(joint_state_zeros) - - # Plan - should go through PLANNING -> COMPLETED - module.plan_to_joints(JointState(position=[0.05] * 7)) - assert module._state == ManipulationState.COMPLETED - - # Reset works from COMPLETED - module.reset() - assert module._state == ManipulationState.IDLE - - # Plan again - module.plan_to_joints(JointState(position=[0.05] * 7)) - - # Execute - should go to EXECUTING then COMPLETED - module.execute() - assert module._state == ManipulationState.COMPLETED diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index efa429f2c8..e7ef9dacf7 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -24,6 +24,7 @@ from dimos.control.coordinator import ControlCoordinator from dimos.control.tasks.trajectory_task.trajectory_task import ( + JOINT_TRAJECTORY_TASK_NAME, TrajectoryCancellationResult, TrajectoryCancellationStatus, TrajectoryExecutionResult, @@ -60,8 +61,15 @@ def _control_coordinator( cancel_status: TrajectoryCancellationStatus = (TrajectoryCancellationStatus.ALREADY_STOPPED), ) -> MagicMock: coordinator = MagicMock(spec=ControlCoordinator) - coordinator.execute_trajectory.return_value = TrajectoryExecutionResult(execute_status) - coordinator.cancel_trajectory.return_value = TrajectoryCancellationResult(cancel_status) + + def task_invoke(_task_name: str, method: str, _kwargs: object = None) -> object: + if method == "execute": + return TrajectoryExecutionResult(execute_status) + if method == "cancel": + return TrajectoryCancellationResult(cancel_status) + raise AssertionError(f"unexpected task command {method}") + + coordinator.task_invoke.side_effect = task_invoke return coordinator @@ -319,7 +327,10 @@ def test_cancel_completed_execution_cancels_coordinator_task(self, module_factor assert module._state == ManipulationState.COMPLETED assert module.cancel() is True - module._control_coordinator.cancel_trajectory.assert_called_once_with() + assert module._control_coordinator.task_invoke.call_args_list[-1].args == ( + JOINT_TRAJECTORY_TASK_NAME, + "cancel", + ) assert module._state == ManipulationState.IDLE def test_reset_not_during_execution(self, module_factory): @@ -881,8 +892,12 @@ def test_execute_plan_dispatches_selected_subsets_once_with_shared_clock_and_map assert module.execute_plan() is True - mock_coordinator.execute_trajectory.assert_called_once() - payload = mock_coordinator.execute_trajectory.call_args.args[0] + payload = mock_coordinator.task_invoke.call_args.args[2]["trajectory"] + mock_coordinator.task_invoke.assert_called_once_with( + JOINT_TRAJECTORY_TASK_NAME, + "execute", + {"trajectory": payload}, + ) assert payload.joint_names == ["left_coord_j1", "k0"] assert [point.time_from_start for point in payload.points] == [0.0, 2.5] assert [point.positions for point in payload.points] == [[0.0, 1.0], [0.5, 1.5]] @@ -1017,6 +1032,76 @@ def test_execute_requires_trajectory(self, robot_config, module_factory): assert module.execute() is False assert module._state == ManipulationState.IDLE + def test_execute_plan_can_dispatch_cached_plan_repeatedly(self, module_factory): + coordinator = _control_coordinator() + module = module_factory(coordinator) + _install_generated_plan(module, _one_joint_config(), MagicMock(), [0.0], [1.0]) + + assert module.execute_plan() + assert module.execute_plan() + assert [call.args[:2] for call in coordinator.task_invoke.call_args_list] == [ + (JOINT_TRAJECTORY_TASK_NAME, "execute"), + (JOINT_TRAJECTORY_TASK_NAME, "execute"), + ] + + def test_direct_plan_does_not_replace_cached_plan(self, module_factory): + coordinator = _control_coordinator() + module = module_factory(coordinator) + config = _one_joint_config() + _install_generated_plan(module, config, MagicMock(), [0.0], [1.0]) + cached = module._last_plan + assert cached is not None + direct = GeneratedPlan( + group_ids=(f"{config.name}/manipulator",), + trajectory=_generated_plan_trajectory([f"{config.name}/j0"], [0.0], [2.0]), + path=[ + JointState(name=[f"{config.name}/j0"], position=[0.0]), + JointState(name=[f"{config.name}/j0"], position=[2.0]), + ], + status=PlanningStatus.SUCCESS, + ) + + assert module.execute_plan(plan=direct) + + assert module._last_plan is cached + dispatched = coordinator.task_invoke.call_args.args[2]["trajectory"] + assert dispatched.points[-1].positions == [2.0] + + def test_known_coordinator_rejection_restores_previous_state(self, module_factory): + coordinator = _control_coordinator( + execute_status=TrajectoryExecutionStatus.START_STATE_MISMATCH + ) + module = module_factory(coordinator) + _install_generated_plan(module, _one_joint_config(), MagicMock(), [0.0], [1.0]) + module._state = ManipulationState.COMPLETED + + assert not module.execute_plan() + + assert module._state is ManipulationState.COMPLETED + assert module._last_plan is not None + + def test_uncertain_execute_projects_to_fault(self, module_factory): + coordinator = _control_coordinator() + coordinator.task_invoke.side_effect = TimeoutError("timed out") + module = module_factory(coordinator) + _install_generated_plan(module, _one_joint_config(), MagicMock(), [0.0], [1.0]) + + assert not module.execute_plan() + + assert module._state is ManipulationState.FAULT + assert "timed out" in module.get_error() + + def test_uncertain_cancel_projects_to_fault(self, module_factory): + coordinator = _control_coordinator() + coordinator.task_invoke.side_effect = TimeoutError("timed out") + module = module_factory(coordinator) + module._state = ManipulationState.EXECUTING + + assert not module.cancel() + + assert module._state is ManipulationState.FAULT + assert "timed out" in module.get_error() + class TestRobotModelConfigMapping: """Test RobotModelConfig joint name mapping helpers.""" diff --git a/dimos/manipulation/test_plan_execution.py b/dimos/manipulation/test_plan_execution.py deleted file mode 100644 index 5a14c224bc..0000000000 --- a/dimos/manipulation/test_plan_execution.py +++ /dev/null @@ -1,161 +0,0 @@ -# Copyright 2025-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. - -"""Tests for ManipulationModule plan-execution result projection.""" - -from pathlib import Path -from unittest.mock import MagicMock - -from dimos.control.coordinator import ControlCoordinator -from dimos.control.tasks.trajectory_task.trajectory_task import ( - TrajectoryCancellationResult, - TrajectoryCancellationStatus, - TrajectoryExecutionResult, - TrajectoryExecutionStatus, -) -from dimos.manipulation.manipulation_module import ManipulationModule, ManipulationState -from dimos.manipulation.planning.groups.models import PlanningGroupDefinition -from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.manipulation.planning.spec.enums import PlanningStatus -from dimos.manipulation.planning.spec.models import GeneratedPlan -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint - - -def _plan(final_position: float = 1.0) -> GeneratedPlan: - names = ["arm/j0"] - trajectory = JointTrajectory( - joint_names=names, - points=[ - TrajectoryPoint( - positions=[0.0], - velocities=[0.0], - time_from_start=0.0, - ), - TrajectoryPoint( - positions=[final_position], - velocities=[0.0], - time_from_start=1.0, - ), - ], - ) - return GeneratedPlan( - group_ids=("arm/manipulator",), - trajectory=trajectory, - path=[ - JointState(name=names, position=[0.0]), - JointState(name=names, position=[final_position]), - ], - status=PlanningStatus.SUCCESS, - ) - - -def _module_with_coordinator( - coordinator: MagicMock, - module_factory, -) -> ManipulationModule: - module = module_factory(coordinator) - config = RobotModelConfig( - name="arm", - model_path=Path("/path/to/robot.urdf"), - joint_names=["j0"], - base_link="base", - planning_groups=[ - PlanningGroupDefinition( - name="manipulator", - joint_names=("j0",), - base_link="base", - tip_link="tool", - ) - ], - ) - module._robots = {"arm": ("arm_id", config, MagicMock())} - module._initialize_execution() - return module - - -def _coordinator( - *, - execute_status: TrajectoryExecutionStatus = TrajectoryExecutionStatus.ACCEPTED, - cancel_status: TrajectoryCancellationStatus = (TrajectoryCancellationStatus.ALREADY_STOPPED), -) -> MagicMock: - coordinator = MagicMock(spec=ControlCoordinator) - coordinator.execute_trajectory.return_value = TrajectoryExecutionResult(execute_status) - coordinator.cancel_trajectory.return_value = TrajectoryCancellationResult(cancel_status) - return coordinator - - -def test_execute_plan_can_dispatch_cached_plan_repeatedly( - module_factory, -) -> None: - coordinator = _coordinator() - module = _module_with_coordinator(coordinator, module_factory) - module._last_plan = _plan() - - assert module.execute_plan() - assert module.execute_plan() - assert coordinator.execute_trajectory.call_count == 2 - - -def test_direct_plan_does_not_replace_cached_plan(module_factory) -> None: - coordinator = _coordinator() - module = _module_with_coordinator(coordinator, module_factory) - cached = _plan(1.0) - direct = _plan(2.0) - module._last_plan = cached - - assert module.execute_plan(plan=direct) - - assert module._last_plan is cached - dispatched = coordinator.execute_trajectory.call_args.args[0] - assert dispatched.points[-1].positions == [2.0] - - -def test_known_coordinator_rejection_restores_previous_state( - module_factory, -) -> None: - coordinator = _coordinator(execute_status=TrajectoryExecutionStatus.START_STATE_MISMATCH) - module = _module_with_coordinator(coordinator, module_factory) - module._last_plan = _plan() - module._state = ManipulationState.COMPLETED - - assert not module.execute_plan() - - assert module._state is ManipulationState.COMPLETED - assert module._last_plan is not None - - -def test_uncertain_execute_projects_to_fault(module_factory) -> None: - coordinator = _coordinator() - coordinator.execute_trajectory.side_effect = TimeoutError("timed out") - module = _module_with_coordinator(coordinator, module_factory) - module._last_plan = _plan() - - assert not module.execute_plan() - - assert module._state is ManipulationState.FAULT - assert "timed out" in module.get_error() - - -def test_uncertain_cancel_projects_to_fault(module_factory) -> None: - coordinator = _coordinator() - coordinator.cancel_trajectory.side_effect = TimeoutError("timed out") - module = _module_with_coordinator(coordinator, module_factory) - module._state = ManipulationState.EXECUTING - - assert not module.cancel() - - assert module._state is ManipulationState.FAULT - assert "timed out" in module.get_error() diff --git a/dimos/robot/manipulators/common/blueprints.py b/dimos/robot/manipulators/common/blueprints.py index dedb3a6ae8..0c210a1db7 100644 --- a/dimos/robot/manipulators/common/blueprints.py +++ b/dimos/robot/manipulators/common/blueprints.py @@ -22,33 +22,28 @@ from dimos.control.components import HardwareComponent from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.control.tasks.trajectory_task.trajectory_task import ( + JOINT_TRAJECTORY_TASK_NAME, +) from dimos.core.coordination.blueprints import Blueprint from dimos.manipulation.manipulation_module import ManipulationModule from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.robot.manipulators.common.topics import ( CARTESIAN_IK_TASK_NAME, COORDINATOR_FRAME_ID, - DEFAULT_TRAJECTORY_TASK_NAME, EEF_TWIST_TASK_NAME, - trajectory_task_name, ) def trajectory_task( hardware: HardwareComponent, *additional_hardware: HardwareComponent, - name: str | None = None, priority: int = 10, start_position_tolerance: float = 0.05, ) -> TaskConfig: hardware_components = (hardware, *additional_hardware) return TaskConfig( - name=name - or ( - trajectory_task_name(hardware.hardware_id) - if not additional_hardware - else DEFAULT_TRAJECTORY_TASK_NAME - ), + name=JOINT_TRAJECTORY_TASK_NAME, type="trajectory", joint_names=[ joint_name for component in hardware_components for joint_name in component.joints @@ -154,9 +149,3 @@ def planner( if visualization is not None: module_kwargs["visualization"] = visualization return ManipulationModule.blueprint(**module_kwargs) - - -def default_trajectory_task_name(hardware_id: str) -> str: - if hardware_id == "arm": - return DEFAULT_TRAJECTORY_TASK_NAME - return trajectory_task_name(hardware_id) diff --git a/dimos/robot/manipulators/common/mixed.py b/dimos/robot/manipulators/common/mixed.py index 591f5fbfa0..aab69a9f0d 100644 --- a/dimos/robot/manipulators/common/mixed.py +++ b/dimos/robot/manipulators/common/mixed.py @@ -38,7 +38,7 @@ hardware=[_xarm6_dual, _piper_dual], tasks=[ TaskConfig( - name="traj_arm", + name="joint_trajectory", type="trajectory", joint_names=[*_xarm6_dual.joints, *_piper_dual.joints], priority=10, diff --git a/dimos/robot/manipulators/common/mock.py b/dimos/robot/manipulators/common/mock.py index 46ee860074..1f0d997bf3 100644 --- a/dimos/robot/manipulators/common/mock.py +++ b/dimos/robot/manipulators/common/mock.py @@ -32,7 +32,7 @@ hardware=[_mock_hw], tasks=[ TaskConfig( - name="traj_arm", + name="joint_trajectory", type="trajectory", joint_names=_mock_hw.joints, priority=10, @@ -65,7 +65,7 @@ class _DualMockCoordinator(ControlCoordinator): hardware=[_mock_left, _mock_right], tasks=[ TaskConfig( - name="traj_arm", + name="joint_trajectory", type="trajectory", joint_names=[*_mock_left.joints, *_mock_right.joints], priority=10, diff --git a/dimos/robot/manipulators/common/topics.py b/dimos/robot/manipulators/common/topics.py index c9bae167bb..cb81687015 100644 --- a/dimos/robot/manipulators/common/topics.py +++ b/dimos/robot/manipulators/common/topics.py @@ -24,8 +24,3 @@ COORDINATOR_FRAME_ID: FrameId = "coordinator" CARTESIAN_IK_TASK_NAME: TaskName = "cartesian_ik_arm" EEF_TWIST_TASK_NAME: TaskName = "eef_twist_arm" -DEFAULT_TRAJECTORY_TASK_NAME: TaskName = "traj_arm" - - -def trajectory_task_name(hardware_id: str) -> TaskName: - return f"traj_{hardware_id}" diff --git a/dimos/robot/manipulators/openarm/blueprints/basic.py b/dimos/robot/manipulators/openarm/blueprints/basic.py index 012d3ceb97..863c187314 100644 --- a/dimos/robot/manipulators/openarm/blueprints/basic.py +++ b/dimos/robot/manipulators/openarm/blueprints/basic.py @@ -27,8 +27,8 @@ ) -def openarm_task(hw: HardwareComponent, name: str | None = None) -> TaskConfig: - return trajectory_task(hw, name=name) +def openarm_task(hw: HardwareComponent) -> TaskConfig: + return trajectory_task(hw) mock_left = openarm_hardware(side="left") diff --git a/dimos/robot/manipulators/piper/blueprints/basic.py b/dimos/robot/manipulators/piper/blueprints/basic.py index 415c08914d..4c2c142f06 100644 --- a/dimos/robot/manipulators/piper/blueprints/basic.py +++ b/dimos/robot/manipulators/piper/blueprints/basic.py @@ -28,7 +28,7 @@ hardware=[_piper_hw], tasks=[ TaskConfig( - name="traj_piper", + name="joint_trajectory", type="trajectory", joint_names=_piper_hw.joints, priority=10, diff --git a/dimos/robot/manipulators/xarm/blueprints/basic.py b/dimos/robot/manipulators/xarm/blueprints/basic.py index 31bf7bb022..25aaec282c 100644 --- a/dimos/robot/manipulators/xarm/blueprints/basic.py +++ b/dimos/robot/manipulators/xarm/blueprints/basic.py @@ -84,7 +84,7 @@ hardware=[_xarm7_left, _xarm6_right], tasks=[ TaskConfig( - name="traj_arm", + name="joint_trajectory", type="trajectory", joint_names=[*_xarm7_left.joints, *_xarm6_right.joints], priority=10, diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index 4f9df72a86..69fc0463ad 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -448,7 +448,7 @@ coordinator_yourarm = ControlCoordinator.blueprint( ], tasks=[ TaskConfig( - name="traj_arm", # Task name (used by ManipulationModule RPC) + name="joint_trajectory", # Canonical trajectory task name type="trajectory", # Trajectory execution task joint_names=[f"arm_joint{i+1}" for i in range(6)], priority=10, # Higher priority wins arbitration @@ -468,6 +468,12 @@ coordinator_yourarm = ControlCoordinator.blueprint( | `address` | Connection info passed to adapter's `__init__` as `address` kwarg. | | `joints` | List of joint names. `make_joints("arm", 6)` creates `["arm_joint1", ..., "arm_joint6"]`. | | `auto_enable` | If `True`, servos are enabled automatically when the coordinator starts. | + +Manipulation dispatches through +`task_invoke("joint_trajectory", "execute", {"trajectory": trajectory})`. +The coordinator intentionally has no trajectory-specific RPC; trajectory +support is optional and belongs to the registered task. Do not derive the task +name from the hardware ID or override it in a blueprint. | `task.name` | Name used by the ManipulationModule to invoke trajectory execution via RPC. | | `task.type` | Task type: `"trajectory"`, `"servo"`, `"velocity"`, or `"cartesian_ik"`. | | `task.priority` | Priority for per-joint arbitration. Higher number wins. | diff --git a/docs/capabilities/manipulation/planning_groups.md b/docs/capabilities/manipulation/planning_groups.md index 67b611474f..55956c354c 100644 --- a/docs/capabilities/manipulation/planning_groups.md +++ b/docs/capabilities/manipulation/planning_groups.md @@ -148,9 +148,10 @@ Preview and execution consume the stored trajectory; they do not lazily parameterize the geometric path. Preview forwards the raw globally named trajectory through the visualization boundary, where renderers project it to their robot-local visuals while preserving stored timestamps. Execution -translates selected joint names at the coordinator boundary and invokes the -coordinator's sole trajectory task once without filling omitted joints in the -RPC trajectory. The task remains planning-group agnostic, claims its full +translates selected joint names at the coordinator boundary and invokes +`execute` on the canonical `joint_trajectory` task through the coordinator's +generic task-command RPC, without filling omitted joints in the trajectory. +The task remains planning-group agnostic, claims its full configured joint set, and holds omitted joints while executing the active planned subset. A newly accepted trajectory replaces the task's current trajectory.