diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index d8648c4b6..308d42ea3 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -467,6 +467,10 @@ topics: - PlanningContext - ExecutionSession - EffectVerificationRequest + - EffectVerificationResult + - eligible_mask + - deactivate_rows + - effect verification deadline - ExecutionRunner - ObservationProvider - CommandSink diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 4c56e299c..de796f9fb 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -378,7 +378,7 @@ runner = ExecutionRunner( command_sink, clock=execution_clock, ) -result = runner.step(effect_success=None) +result = runner.step(effect_result=None) ``` `ExecutionSession` owns deterministic planning progress and recovery state. It @@ -404,15 +404,43 @@ active targets so the caller can still hold them. The session monitors: - action-attempt timeout; - planner and semantic-effect failure. -It replans from the latest observation within per-environment budgets. The -budgets and eligibility masks are row-local, while the action waypoint cursor -is batch-synchronized: one allowed replan regenerates the active cohort and -restarts its action trajectory without charging unaffected rows. Unknown -or exhausted failures are reported as structured `ExecutionEvent` objects. A -non-empty `StateDelta` is not committed until the caller supplies an external -`effect_success` mask. While verification is outstanding, -`ExecutionTick.pending_effect` retains a typed `EffectVerificationRequest` on -every tick; `EFFECT_VERIFICATION_REQUIRED` is only the one-time audit event. +It replans from the latest observation within per-environment budgets. Pass an +owned boolean `eligible_mask` to `engine.start()` when a previous semantic call +has already deactivated rows. Eligibility can only shrink; use +`runner.deactivate_rows(mask, reason=...)` while a runner owns scheduling so its +cached effect request stays correlated. The budgets, verified task state, and +eligibility masks are row-local, while the action waypoint cursor and call +barrier are batch-synchronized. One allowed replan regenerates the still-pending +cohort without charging unaffected rows. Exhausted rows hold and never become +eligible again. + +A non-empty `StateDelta` is not committed until the caller supplies a +correlated `EffectVerificationResult`. Its disjoint `success_mask` and +`failure_mask` must be subsets of the current request mask; requested rows in +neither mask remain unresolved. Partial successes commit immediately while +unresolved rows keep the barrier pending. `EffectVerificationRequest` carries a +monotonic `verification_id`, stable `requested_at`/`deadline` values in the +robot-observation timestamp domain, and an owned effect snapshot. Mask shrinkage +creates a new ID without extending the deadline; whole-action retry creates a +new attempt. Results for an old ID are rejected. `RecoveryPolicy.action_timeout` +covers the trajectory and terminal effect wait together, and only timestamps +strictly greater than the deadline time out. While verification is outstanding, +`ExecutionTick.pending_effect` retains the request on every tick; +`EFFECT_VERIFICATION_REQUIRED` is only the one-time audit event. + +```python +request = tick.pending_effect +effect_result = EffectVerificationResult( + verification_id=request.verification_id, + success_mask=observed_success, + failure_mask=observed_failure, +) +result = runner.step(effect_result=effect_result) +``` + +Cause events (`ACTION_PLANNING_FAILED`, `EFFECT_VERIFICATION_FAILED`, and +`EFFECT_VERIFICATION_TIMEOUT`) are distinct from the `ACTION_RETRY` recovery +event. `SESSION_COMPLETED` and `SESSION_FAILED` are distinct terminal events. Recovery replans reuse the current immutable `ResolvedActionRequest`, including its owned goal snapshot. Mutable goal values are copied, while simulator-backed diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 0ae3d06b3..56afd9a57 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -427,9 +427,11 @@ an older custom action by renaming its implementation to `_plan()`. | `AtomicAction.plan(request, context)` | `AtomicActionEngine` | Binds the current collision scene into a copied policy, then delegates to `_plan()` | | `AtomicAction._plan(request, context)` | Atomic-action implementer | Consumes the prepared immutable `ResolvedActionRequest` and returns an `ActionPlan` | | `engine.plan_action(action, invocation, context)` | Extension or isolated test | Temporarily binds and plans an unregistered action instance; built-in parameter variants should use invocation `skill_options` instead | +| `engine.start(invocations, context, eligible_mask=...)` | Runtime orchestrator | Starts a session whose owned row cohort can only shrink across action barriers and recovery | | `session.revise_current(invocation)` | Manually ticked runtime orchestrator | Replaces the active logical call with a newer same-destination revision and replans from the latest observed context | | `runner.revise_current(invocation)` | Runner-driven runtime orchestrator or Action Agent | Snapshots a revision, preserves the current frame deadline, then replans from a fresh due-time observation | -| `runner.step(effect_success=...)` | Non-blocking controller integration | Observes and routes a `RuntimeCommandFrame` only when it is due | +| `runner.deactivate_rows(mask, reason=...)` | Runner-driven runtime orchestrator | Permanently removes rows and refreshes the runner's cached effect request; prefer it over direct session mutation | +| `runner.step(effect_result=...)` | Non-blocking controller integration | Observes and routes a `RuntimeCommandFrame` only when it is due | | `runner.run_until_blocked(...)` | Simple blocking application or tutorial | Advances the injected clock until terminal or external effect verification is required | | `runner.cancel(reason)` | Explicit safe stop | Requests controller cancellation followed by an observed-position hold | @@ -607,6 +609,16 @@ unknown or incompatible transport cannot cause partial dispatch. Cancellation, observation/session exceptions, and negative acknowledgements enter a best-effort cancel-then-hold path for every armed runtime target. +Pass an owned `eligible_mask` to `engine.start()` when only a subset of rows may +enter the invocation sequence. This cohort is sticky: eligibility can only +shrink across action barriers and replans. Later failures outside the atomic +runtime should call `runner.deactivate_rows(mask, reason=...)`; the operation is +idempotent, the next command neutralizes changed rows, and removing the final +eligible row fails and terminates the session. When effect verification is +pending, deactivation narrows the request and assigns a new +`verification_id`. Do not mutate `session` directly while its runner owns +scheduling, because the runner must refresh its cached effect boundary. + The engine authorizes every emitted command against the immutable target and physical claims in the resolved binding. A command cannot address an unbound destination, substitute target metadata, or overlap another endpoint's joints @@ -757,20 +769,39 @@ environment row. Pick, place, handover, and coordinated skills also return an uncommitted `StateDelta` describing the attachment state expected after execution. -At the terminal waypoint, an `ExecutionSession` requests an external -per-environment verification mask before committing a non-empty effect: +At the terminal waypoint, an `ExecutionSession` requests an external, +correlated per-environment result before committing a non-empty effect: ```python +from embodichain.lab.sim.atomic_actions import EffectVerificationResult + tick = session.tick(latest_context) if tick.pending_effect is not None: - effect_success = verify_grasp_or_release() - tick = session.tick(latest_context, effect_success=effect_success) + request = tick.pending_effect + success_mask, failure_mask = verify_grasp_or_release(request.env_mask) + effect_result = EffectVerificationResult( + verification_id=request.verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + ) + tick = session.tick(latest_context, effect_result=effect_result) ``` This prevents a collision-free or well-tracked command plan from being misreported as a successful grasp, release, or handover. The typed `EffectVerificationRequest` persists on subsequent ticks while waiting; -`EFFECT_VERIFICATION_REQUIRED` remains a one-time observability event. +`EFFECT_VERIFICATION_REQUIRED` remains a one-time observability event. Success +and failure masks are disjoint subsets of the request mask; omitted request rows +remain unresolved. Request IDs change after mask shrinkage or whole-action +retry, so a delayed result cannot commit a newer attempt. + +`request.deadline` is expressed in the robot-observation timestamp domain. +`RecoveryPolicy.action_timeout` covers both trajectory execution and the +terminal effect wait; a retry invalidates the old request ID. With +`ExecutionRunner.step()`, a call made before the next due cycle does not consume +its `effect_result`: schedule another call using `wait_duration`, re-read the +current request, and submit a result for that current ID. Partial resolution and +row deactivation can also replace the request before the delayed result arrives. ## Action Agent integration diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 6f7de6cb2..181472035 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -332,7 +332,12 @@ must be resolved from the latest scene snapshot: ) task = TaskState.empty(robot.get_qpos().shape[0], robot.device) initial_context = adapter.observe(task) - session = engine.start((invocation,), initial_context) + initial_eligible = determine_ready_rows(initial_context) + session = engine.start( + (invocation,), + initial_context, + eligible_mask=initial_eligible, + ) router = EndpointCommandRouter((adapter,)) runner = ExecutionRunner(session, adapter, router, clock=adapter) result = runner.run_until_blocked() @@ -359,6 +364,25 @@ For an application that already owns its event loop, call the non-blocking with ``is_waiting`` set has not consumed a new observation or effect result; use its ``wait_duration`` to schedule the next call. +``eligible_mask`` is an owned initial cohort, not a one-tick filter. Eligibility +can only shrink for the lifetime of the session and remains inactive across +action barriers and replans. If an application later loses a row, deactivate it +through the runner that owns scheduling: + +.. code-block:: python + + changed = runner.deactivate_rows( + lost_tracking_mask, + reason="object tracking was lost", + ) + +The operation is idempotent and the next command actively neutralizes changed +rows. Deactivating rows while an effect is pending narrows the request and +changes its ``verification_id``. Deactivating the last eligible row fails and +terminates the session. Do not call ``session.deactivate_rows()`` directly while +an ``ExecutionRunner`` owns the session because the runner must refresh its +cached effect boundary. + The complete simulation example starts with a visible cube directly in front of the robot, then applies a short horizontal force pulse so physics and friction slide it sideways during one ``PickUp`` invocation whose @@ -451,24 +475,58 @@ Task-state effects Pick, place, handover, and coordinated skills declare attachment changes as a :class:`~embodichain.lab.sim.atomic_actions.StateDelta`. Planning does not commit -those changes. During closed-loop execution, a non-empty effect requires an -external per-environment verification mask: +those changes. During closed-loop execution, a non-empty effect requires a +correlated per-environment verification result: .. code-block:: python + from embodichain.lab.sim.atomic_actions import EffectVerificationResult + def verify_effect(context, tick): - return verify_grasp_or_release(context) + request = tick.pending_effect + assert request is not None + success_mask, failure_mask = verify_grasp_or_release(context, request.env_mask) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + ) result = runner.run_until_blocked(effect_verifier=verify_effect) This prevents a successful trajectory plan from being mistaken for a successful physical grasp or release. If verification is asynchronous, omit the callback; ``run_until_blocked`` returns at the verification boundary and the application -can later resume with ``runner.step(effect_success=verified)`` when the next -cycle is due, or call ``run_until_blocked(effect_verifier=...)`` again. The -runner remembers the pending boundary even though the session emits its event -only once. The durable state is ``tick.pending_effect`` (an -``EffectVerificationRequest``), not the presence of that one-time event. +can later resume from the *current* pending request: + +.. code-block:: python + + request = runner.session.pending_effect + assert request is not None + success_mask, failure_mask = await_effect_observation(request.env_mask) + verified = EffectVerificationResult( + verification_id=request.verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + ) + resumed = runner.step(effect_result=verified) + if resumed.is_waiting: + schedule_after(resumed.wait_duration) + # This call did not consume ``verified``. Re-read the current request + # and submit a result for that ID again at the due cycle. + +Alternatively, call ``run_until_blocked(effect_verifier=...)`` again. Success +and failure masks must be disjoint subsets of the request mask; rows in neither +mask remain unresolved. A result must reuse the current request's +``verification_id``. Deactivation, partial resolution, or retry can replace the +request, so re-read it before delayed submission and re-verify if its ID or mask +changed. ``request.deadline`` uses the robot-observation timestamp domain; +``RecoveryPolicy.action_timeout`` covers both trajectory execution and the +terminal effect wait. A result submitted after timeout cannot satisfy the new +retry attempt because its old ID is invalid. The runner remembers the pending +boundary even though the session emits its event only once. The durable state is +``tick.pending_effect`` (an ``EffectVerificationRequest``), not the presence of +that one-time event. Adding an action ---------------- diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 607470da6..6eec6bdac 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -58,6 +58,7 @@ ) from .execution import ( EffectVerificationRequest, + EffectVerificationResult, ExecutionEvent, ExecutionEventKind, ExecutionSession, @@ -202,6 +203,7 @@ "EndpointCommandTransport", "EntityState", "EffectVerificationRequest", + "EffectVerificationResult", "EffectVerifier", "ExecutionClock", "ExecutionFeedbackMode", diff --git a/embodichain/lab/sim/atomic_actions/effects.py b/embodichain/lab/sim/atomic_actions/effects.py index f9c1f537b..90b80bfa5 100644 --- a/embodichain/lab/sim/atomic_actions/effects.py +++ b/embodichain/lab/sim/atomic_actions/effects.py @@ -18,12 +18,16 @@ from __future__ import annotations -from dataclasses import dataclass, field +from collections.abc import Mapping +from copy import deepcopy +from dataclasses import dataclass, field, fields, is_dataclass from types import MappingProxyType -from typing import Mapping +from typing import TYPE_CHECKING import torch +from embodichain.lab.sim.common import BatchEntity + from .state import ( CoordinatedHeldObjectState, HeldObjectState, @@ -33,6 +37,86 @@ _normalize_mask, ) +if TYPE_CHECKING: + from .core import ObjectSemantics + + +def _effect_snapshot_memo(value: object) -> dict[int, object]: + """Preserve live entities and private runtime caches during effect copies.""" + memo: dict[int, object] = {} + visited: set[int] = set() + + def visit(nested: object) -> None: + nested_id = id(nested) + if nested_id in visited: + return + visited.add(nested_id) + if isinstance(nested, BatchEntity): + memo[nested_id] = nested + return + if is_dataclass(nested) and not isinstance(nested, type): + for data_field in fields(nested): + child = getattr(nested, data_field.name) + if data_field.name == "_generator" and child is not None: + memo[id(child)] = None + elif not data_field.init and child is not None: + memo[id(child)] = child + else: + visit(child) + return + if isinstance(nested, Mapping): + for key, child in nested.items(): + visit(key) + visit(child) + return + if isinstance(nested, (list, tuple, set, frozenset)): + for child in nested: + visit(child) + + visit(value) + return memo + + +def _snapshot_semantics(value: ObjectSemantics) -> ObjectSemantics: + """Copy semantic data while retaining live simulation-entity identity.""" + try: + copied = deepcopy(value, _effect_snapshot_memo(value)) + except Exception as exc: + raise TypeError( + "ObjectSemantics effect metadata must be copyable without cloning " + "live simulation entities." + ) from exc + if type(copied) is not type(value) or copied is value: + raise TypeError( + "ObjectSemantics effect snapshots must produce a distinct value " + "of the same exact type." + ) + return copied + + +def _snapshot_held(value: HeldObjectState) -> HeldObjectState: + """Return an independently owned held-object effect value.""" + return HeldObjectState( + semantics=_snapshot_semantics(value.semantics), + object_to_eef=value.object_to_eef.clone(), + grasp_xpos=value.grasp_xpos.clone(), + env_mask=None if value.env_mask is None else value.env_mask.clone(), + ) + + +def _snapshot_coordinated( + value: CoordinatedHeldObjectState, +) -> CoordinatedHeldObjectState: + """Return an independently owned coordinated held-object effect value.""" + return CoordinatedHeldObjectState( + semantics=_snapshot_semantics(value.semantics), + left_object_to_eef=value.left_object_to_eef.clone(), + right_object_to_eef=value.right_object_to_eef.clone(), + left_grasp_xpos=value.left_grasp_xpos.clone(), + right_grasp_xpos=value.right_grasp_xpos.clone(), + env_mask=None if value.env_mask is None else value.env_mask.clone(), + ) + def _with_held_mask( value: HeldObjectState, @@ -217,6 +301,26 @@ def is_empty(self) -> bool: """Whether this delta declares no symbolic state changes.""" return not self.held_object_updates and not self.coordinated_held_object_updates + def snapshot(self) -> StateDelta: + """Return an independently owned symbolic-effect snapshot. + + Live simulation entities retain identity, while semantic metadata, + affordance data, and every attachment tensor are copied. + + Returns: + Independently owned state delta. + """ + return StateDelta( + held_object_updates={ + resource: None if value is None else _snapshot_held(value) + for resource, value in self.held_object_updates.items() + }, + coordinated_held_object_updates={ + resources: (None if value is None else _snapshot_coordinated(value)) + for resources, value in self.coordinated_held_object_updates.items() + }, + ) + def apply( self, state: TaskState, diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index 0baab8ab7..2c7d446df 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -546,6 +546,8 @@ def start( self, invocations: Iterable[ActionInvocation], context: PlanningContext | None = None, + *, + eligible_mask: torch.Tensor | None = None, ) -> ExecutionSession: """Start closed-loop execution for a grounded invocation sequence. @@ -553,6 +555,8 @@ def start( invocations: Grounded action requests in execution order. context: Initial measured state and scene snapshot. The engine captures one when omitted. + eligible_mask: Optional rows allowed to enter this session. Inactive + rows remain inactive across every invocation in the sequence. Returns: Stateful execution session advanced by ``session.tick(...)``. @@ -560,7 +564,12 @@ def start( from .execution import ExecutionSession initial = self.initial_context() if context is None else context - return ExecutionSession(self, tuple(invocations), initial) + return ExecutionSession( + self, + tuple(invocations), + initial, + eligible_mask=eligible_mask, + ) def _validate_context(self, context: PlanningContext) -> None: """Validate an externally supplied planning context.""" diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index b425b9934..d44f24505 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -20,6 +20,7 @@ from dataclasses import dataclass from enum import Enum +import math from typing import TYPE_CHECKING import torch @@ -60,13 +61,18 @@ class ExecutionEventKind(str, Enum): TRACKING_ERROR = "tracking_error" DYNAMIC_GOAL_CHANGED = "dynamic_goal_changed" COLLISION_WORLD_CHANGED = "collision_world_changed" + ACTION_PLANNING_FAILED = "action_planning_failed" ACTION_TIMEOUT = "action_timeout" TRAJECTORY_COMPLETED = "trajectory_completed" EFFECT_VERIFICATION_REQUIRED = "effect_verification_required" + EFFECT_VERIFICATION_FAILED = "effect_verification_failed" + EFFECT_VERIFICATION_TIMEOUT = "effect_verification_timeout" ACTION_RETRY = "action_retry" ACTION_COMPLETED = "action_completed" RECOVERY_EXHAUSTED = "recovery_exhausted" + ROWS_DEACTIVATED = "rows_deactivated" SESSION_COMPLETED = "session_completed" + SESSION_FAILED = "session_failed" @dataclass(frozen=True, slots=True, eq=False) @@ -89,6 +95,8 @@ def __post_init__(self) -> None: raise ValueError("invocation_index must be non-negative.") if self.invocation_revision < 0: raise ValueError("invocation_revision must be non-negative.") + if not isinstance(self.env_mask, torch.Tensor): + raise TypeError("env_mask must be a torch.Tensor.") if self.env_mask.dtype != torch.bool or self.env_mask.dim() != 1: raise ValueError("ExecutionEvent.env_mask must be a 1D bool tensor.") object.__setattr__(self, "env_mask", self.env_mask.clone()) @@ -96,17 +104,27 @@ def __post_init__(self) -> None: @dataclass(frozen=True, slots=True, eq=False) class EffectVerificationRequest: - """Typed boundary describing a semantic effect awaiting verification.""" + """Typed boundary describing a semantic effect awaiting verification. + ``requested_at`` and ``deadline`` use the same timestamp domain as + :class:`RobotObservation`. Request-mask shrinkage retains both values; + only a whole-action retry starts a new attempt deadline. + """ + + verification_id: int skill_id: str invocation_id: str | None invocation_revision: int invocation_index: int terminal_segment: str | None + requested_at: float + deadline: float env_mask: torch.Tensor expected_effects: StateDelta def __post_init__(self) -> None: + if type(self.verification_id) is not int or self.verification_id < 0: + raise ValueError("verification_id must be a non-negative integer.") if not isinstance(self.skill_id, str) or not self.skill_id: raise ValueError("skill_id must be a non-empty string.") if self.invocation_id is not None and ( @@ -121,13 +139,71 @@ def __post_init__(self) -> None: not isinstance(self.terminal_segment, str) or not self.terminal_segment ): raise ValueError("terminal_segment must be a non-empty string or None.") + if not math.isfinite(self.requested_at) or self.requested_at < 0.0: + raise ValueError("requested_at must be finite and non-negative.") + if not math.isfinite(self.deadline) or self.deadline < self.requested_at: + raise ValueError( + "deadline must be finite and no earlier than requested_at." + ) + if not isinstance(self.env_mask, torch.Tensor): + raise TypeError("env_mask must be a torch.Tensor.") if self.env_mask.dtype != torch.bool or self.env_mask.dim() != 1: raise ValueError("env_mask must be a 1D bool tensor.") + if not self.env_mask.any(): + raise ValueError("env_mask must contain at least one requested row.") if not isinstance(self.expected_effects, StateDelta): raise TypeError("expected_effects must be a StateDelta.") if self.expected_effects.is_empty: raise ValueError("Effect verification requires a non-empty StateDelta.") object.__setattr__(self, "env_mask", self.env_mask.clone()) + object.__setattr__(self, "expected_effects", self.expected_effects.snapshot()) + + def snapshot(self) -> EffectVerificationRequest: + """Return a request snapshot with an independently owned row mask.""" + return EffectVerificationRequest( + verification_id=self.verification_id, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + invocation_index=self.invocation_index, + terminal_segment=self.terminal_segment, + requested_at=self.requested_at, + deadline=self.deadline, + env_mask=self.env_mask, + expected_effects=self.expected_effects, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class EffectVerificationResult: + """Correlated per-environment update for one effect boundary. + + Rows absent from both masks remain unresolved. This lets one shared batch + barrier commit verified rows while other rows continue observing the same + physical effect. + """ + + verification_id: int + success_mask: torch.Tensor + failure_mask: torch.Tensor + + def __post_init__(self) -> None: + if type(self.verification_id) is not int or self.verification_id < 0: + raise ValueError("verification_id must be a non-negative integer.") + for name in ("success_mask", "failure_mask"): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + if self.success_mask.shape != self.failure_mask.shape: + raise ValueError("success_mask and failure_mask must have equal shapes.") + if self.success_mask.device != self.failure_mask.device: + raise ValueError("success_mask and failure_mask must use the same device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("success_mask and failure_mask must not overlap.") + object.__setattr__(self, "success_mask", self.success_mask.clone()) + object.__setattr__(self, "failure_mask", self.failure_mask.clone()) @dataclass(frozen=True, slots=True, eq=False) @@ -162,6 +238,16 @@ def __post_init__(self) -> None: raise TypeError("hold_targets must contain RuntimeEndpointTarget values.") if self.command is not None and self.hold_targets: raise ValueError("A tick cannot send commands and request a hold together.") + if self.pending_effect is not None: + if not isinstance(self.pending_effect, EffectVerificationRequest): + raise TypeError( + "pending_effect must be an EffectVerificationRequest or None." + ) + object.__setattr__( + self, + "pending_effect", + self.pending_effect.snapshot(), + ) hold_targets: list[RuntimeEndpointTarget] = [] for target in self.hold_targets: snapshot = target.snapshot() @@ -182,11 +268,14 @@ class ExecutionSession: The session never steps a simulator itself. Each :meth:`tick` consumes the latest observation and scene snapshot and emits at most one synchronized endpoint-command frame. Expected symbolic effects are committed only after the caller - supplies ``effect_success`` for a non-empty :class:`StateDelta`. + supplies a correlated :class:`EffectVerificationResult` for a non-empty + :class:`StateDelta`. Environment eligibility and recovery budgets are tracked per row. The waypoint cursor is batch-synchronized: a recoverable row replans the active cohort from the latest observation and restarts the action trajectory. + Calls that mutate the session must be serialized by its owner; the session + does not provide thread synchronization. """ def __init__( @@ -194,6 +283,8 @@ def __init__( engine: AtomicActionEngine, invocations: tuple[ActionInvocation, ...], context: PlanningContext, + *, + eligible_mask: torch.Tensor | None = None, ) -> None: if not invocations: raise ValueError("ExecutionSession requires at least one invocation.") @@ -218,16 +309,34 @@ def __init__( self._last_command_mask = torch.zeros( context.batch_size, dtype=torch.bool, device=context.robot.qpos.device ) - self._eligible = torch.ones_like(self._last_command_mask) + self._eligible = ( + torch.ones_like(self._last_command_mask) + if eligible_mask is None + else self._normalize_mask(eligible_mask, "eligible_mask") + ) self._pending = self._eligible.clone() self._action_retries = torch.zeros( context.batch_size, dtype=torch.long, device=context.robot.qpos.device ) self._replans = torch.zeros_like(self._action_retries) self._pending_effect: EffectVerificationRequest | None = None - self._status = ExecutionStatus.RUNNING + self._effect_failures = torch.zeros_like(self._eligible) + self._effect_requested_at: float | None = None + self._next_effect_verification_id = 0 + self._status = ( + ExecutionStatus.RUNNING if self._eligible.any() else ExecutionStatus.FAILED + ) self._queued_events: list[ExecutionEvent] = [] - self._plan_current(context, ExecutionEventKind.ACTION_PLANNED) + if self._status is ExecutionStatus.RUNNING: + self._plan_current(context, ExecutionEventKind.ACTION_PLANNED) + else: + self._queued_events.append( + self._event( + ExecutionEventKind.SESSION_FAILED, + self._eligible, + "No environment was initially eligible for execution.", + ) + ) @property def status(self) -> ExecutionStatus: @@ -254,6 +363,68 @@ def effect_verification_pending(self) -> bool: """Whether the current physical effect still requires verification.""" return self._pending_effect is not None + @property + def pending_effect(self) -> EffectVerificationRequest | None: + """Owned snapshot of the current effect boundary, when present.""" + return None if self._pending_effect is None else self._pending_effect.snapshot() + + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> torch.Tensor: + """Permanently remove selected rows from this invocation sequence. + + Deactivation is sticky across action barriers and recovery replans. + The next emitted command frame marks those rows inactive so the command + sink can apply target-specific safe hold behavior. + + Args: + env_mask: Rows requested for deactivation. + reason: Human-readable event message. + + Returns: + Owned mask of rows that changed from eligible to inactive. + + Raises: + RuntimeError: If the session is already terminal. + ValueError: If ``reason`` is empty or the mask shape is invalid. + """ + if self._status is not ExecutionStatus.RUNNING: + raise RuntimeError("Only a running execution session can deactivate rows.") + if type(reason) is not str or not reason: + raise ValueError("reason must be a non-empty string.") + requested = self._normalize_mask(env_mask, "env_mask") + changed = requested & self._eligible + if not changed.any(): + return changed + self._eligible &= ~changed + self._pending &= ~changed + self._effect_failures &= ~changed + self._last_command_mask &= ~changed + self._queued_events.append( + self._event(ExecutionEventKind.ROWS_DEACTIVATED, changed, reason) + ) + if self._pending_effect is not None: + assert self._plan is not None + previous_effect = self._pending_effect + remaining_effect = ( + previous_effect.env_mask & self._pending & self._plan.plan_success + ) + if torch.equal(remaining_effect, previous_effect.env_mask): + self._pending_effect = previous_effect + elif remaining_effect.any(): + self._pending_effect = self._effect_verification_request( + remaining_effect + ) + else: + self._pending_effect = None + terminal_event = self._update_terminal_status() + if terminal_event is not None: + self._queued_events.append(terminal_event) + return changed.clone() + def revise_current( self, invocation: ActionInvocation, @@ -302,10 +473,10 @@ def _prepare_revision( raise TypeError("invocation must be an ActionInvocation.") if self._status is not ExecutionStatus.RUNNING: raise RuntimeError("Only a running execution session can be revised.") - if self._pending_effect is not None: + if self._pending_effect is not None or self._effect_failures.any(): raise RuntimeError( - "Cannot revise while a physical effect is awaiting verification; " - "verify it or cancel and start a new invocation." + "Cannot revise while physical-effect resolution is pending; " + "resolve it or cancel and start a new invocation." ) self._validate_revision_identity( skill_id=invocation.skill_id, @@ -324,10 +495,10 @@ def _install_prepared_revision( raise TypeError("replacement must be a ResolvedActionRequest.") if self._status is not ExecutionStatus.RUNNING: raise RuntimeError("Only a running execution session can be revised.") - if self._pending_effect is not None: + if self._pending_effect is not None or self._effect_failures.any(): raise RuntimeError( - "Cannot revise while a physical effect is awaiting verification; " - "verify it or cancel and start a new invocation." + "Cannot revise while physical-effect resolution is pending; " + "resolve it or cancel and start a new invocation." ) self._validate_revision_identity( skill_id=replacement.skill_id, @@ -408,32 +579,41 @@ def tick( self, context: PlanningContext, *, - effect_success: torch.Tensor | None = None, + effect_result: EffectVerificationResult | None = None, ) -> ExecutionTick: """Advance execution by one observation/command cycle. Args: context: Latest measured robot and versioned scene state. Its task state is replaced by the session's verified task state. - effect_success: Optional per-environment semantic-effect verification - for an action waiting at its terminal waypoint. + effect_result: Optional correlated semantic-effect result for an + action waiting at its terminal waypoint. Returns: Status, optional command, events, and current verified task state. """ self._context = self._validated_context(context) events = self._drain_events() + if effect_result is not None: + if type(effect_result) is not EffectVerificationResult: + raise TypeError( + "effect_result must be exactly EffectVerificationResult or None." + ) + if self._pending_effect is None: + raise ValueError("No semantic effect is awaiting verification.") + if effect_result.verification_id != self._pending_effect.verification_id: + raise ValueError( + "effect_result verification_id does not match the pending " + "effect boundary." + ) if self._status is not ExecutionStatus.RUNNING: return self._tick_result(command=None, events=events) assert self._plan is not None - if self._pending_effect is not None: - execution_mask = ( - self._pending_effect.env_mask & self._pending & self._plan.plan_success - ) + if not self._pending.any(): command, hold_targets, completion_events = self._finish_action( - execution_mask, - effect_success, + self._pending, + None, ) events.extend(completion_events) return self._tick_result( @@ -442,6 +622,104 @@ def tick( events=events, ) + if self._pending_effect is not None: + execution_mask = ( + self._pending_effect.env_mask & self._pending & self._plan.plan_success + ) + if self._action_timed_out(self._plan, execution_mask): + timed_out = execution_mask.clone() + known_failures = self._effect_failures.clone() + planning_failed = self._pending & ~self._plan.plan_success + retry_mask = timed_out | known_failures | planning_failed + self._pending_effect = None + self._effect_failures.zero_() + if known_failures.any(): + events.append( + self._event( + ExecutionEventKind.EFFECT_VERIFICATION_FAILED, + known_failures, + "Expected semantic effects were not observed.", + ) + ) + if planning_failed.any(): + events.append( + self._event( + ExecutionEventKind.ACTION_PLANNING_FAILED, + planning_failed, + "Planning failed for pending environments.", + ) + ) + events.extend( + self._attempt_action_retry( + retry_mask, + ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT, + "Effect verification exceeded the action attempt timeout.", + reason_mask=timed_out, + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + assert self._plan is not None + effect_result = None + else: + command, hold_targets, completion_events = self._finish_action( + execution_mask, + effect_result, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + + if self._effect_failures.any(): + failed_effect = self._effect_failures.clone() + planning_failed = self._pending & ~self._plan.plan_success + retry_mask = failed_effect | planning_failed + self._effect_failures.zero_() + if planning_failed.any(): + events.append( + self._event( + ExecutionEventKind.ACTION_PLANNING_FAILED, + planning_failed, + "Planning failed for pending environments.", + ) + ) + events.extend( + self._attempt_action_retry( + retry_mask, + ExecutionEventKind.EFFECT_VERIFICATION_FAILED, + "Expected semantic effects were not observed.", + reason_mask=failed_effect, + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) + assert self._plan is not None + plan = self._plan execution_mask = self._pending & plan.plan_success recovery_events = self._recover_if_needed(plan, execution_mask) @@ -459,6 +737,17 @@ def tick( assert self._plan is not None plan = self._plan execution_mask = self._pending & self._plan.plan_success + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) commands = plan.commands if self._waypoint_index < commands.frame_count: @@ -483,6 +772,17 @@ def tick( assert self._plan is not None plan = self._plan execution_mask = self._pending & self._plan.plan_success + if not self._pending.any(): + command, hold_targets, completion_events = self._finish_action( + self._pending, + None, + ) + events.extend(completion_events) + return self._tick_result( + command=command, + hold_targets=hold_targets, + events=events, + ) if plan.commands.frame_count > 0: command = self._command_at(plan, 0, execution_mask) self._waypoint_index = 1 @@ -496,7 +796,7 @@ def tick( ) command, hold_targets, completion_events = self._finish_action( execution_mask, - effect_success, + effect_result, ) events.extend(completion_events) return self._tick_result( @@ -515,7 +815,7 @@ def tick( command, hold_targets, completion_events = self._finish_action( execution_mask, - effect_success, + effect_result, ) events.extend(completion_events) return self._tick_result( @@ -594,6 +894,8 @@ def _install_plan( self._last_joint_ids = () self._last_command_mask.zero_() self._pending_effect = None + self._effect_failures.zero_() + self._effect_requested_at = None planned_mask = self._pending & plan.plan_success self._queued_events.append( self._event(event_kind, planned_mask, "Planned from the latest context.") @@ -669,10 +971,7 @@ def _recover_if_needed( events: list[ExecutionEvent] = [] if not execution_mask.any(): return events - if ( - self._context.robot.timestamp - self._action_started_at - > plan.recovery_policy.action_timeout - ): + if self._action_timed_out(plan, execution_mask): return self._attempt_action_retry( execution_mask, ExecutionEventKind.ACTION_TIMEOUT, @@ -718,6 +1017,18 @@ def _recover_if_needed( ) return events + def _action_timed_out( + self, + plan: ActionPlan, + execution_mask: torch.Tensor, + ) -> bool: + """Return whether an active action attempt exceeded its deadline.""" + return bool( + execution_mask.any() + and self._context.robot.timestamp - self._action_started_at + > plan.recovery_policy.action_timeout + ) + def _attempt_replan( self, trigger_mask: torch.Tensor, @@ -748,7 +1059,9 @@ def _attempt_replan( self._replans[allowed] += 1 self._plan_current(self._context, ExecutionEventKind.REPLANNED) events.extend(self._drain_events()) - self._update_terminal_status() + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) return events def _attempt_action_retry( @@ -756,11 +1069,16 @@ def _attempt_action_retry( trigger_mask: torch.Tensor, reason: ExecutionEventKind, message: str, + *, + reason_mask: torch.Tensor | None = None, ) -> list[ExecutionEvent]: """Retry the current action or permanently fail exhausted rows.""" assert self._plan is not None policy = self._plan.recovery_policy - events = [self._event(reason, trigger_mask, message)] + cause_mask = trigger_mask if reason_mask is None else reason_mask + events = [self._event(reason, cause_mask, message)] + self._pending_effect = None + self._effect_failures &= ~trigger_mask allowed = trigger_mask & (self._action_retries < policy.max_action_retries) exhausted = trigger_mask & ~allowed if exhausted.any(): @@ -785,13 +1103,15 @@ def _attempt_action_retry( ) self._plan_current(self._context, ExecutionEventKind.REPLANNED) events.extend(self._drain_events()) - self._update_terminal_status() + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) return events def _finish_action( self, execution_mask: torch.Tensor, - effect_success: torch.Tensor | None, + effect_result: EffectVerificationResult | None, ) -> tuple[ RuntimeCommandFrame | None, tuple[RuntimeEndpointTarget, ...], @@ -807,22 +1127,39 @@ def _finish_action( ) orphaned_targets = bool(active_targets) and not plan_targets events: list[ExecutionEvent] = [] + if not self._pending.any(): + hold_targets, barrier_events = self._advance_action_barrier( + active_targets, + orphaned_targets=orphaned_targets, + ) + return None, hold_targets, barrier_events + planning_failed = self._pending & ~self._plan.plan_success if not execution_mask.any() and planning_failed.any(): events.extend( self._attempt_action_retry( planning_failed, - ExecutionEventKind.ACTION_RETRY, + ExecutionEventKind.ACTION_PLANNING_FAILED, "Planning failed for every pending environment.", ) ) if self._status is not ExecutionStatus.RUNNING: return None, active_targets, events + if not self._pending.any(): + hold_targets, barrier_events = self._advance_action_barrier( + active_targets, + orphaned_targets=orphaned_targets, + ) + events.extend(barrier_events) + return None, hold_targets, events return None, active_targets, events + failed_effect = torch.zeros_like(execution_mask) + unresolved = torch.zeros_like(execution_mask) + made_progress = False if self._plan.expected_effects.is_empty: verified = execution_mask - elif effect_success is None: + elif effect_result is None: if self._pending_effect is None: self._pending_effect = self._effect_verification_request(execution_mask) events.append( @@ -834,9 +1171,27 @@ def _finish_action( ) return None, active_targets, events else: - verified_input = self._normalize_mask(effect_success, "effect_success") - verified = execution_mask & verified_input - self._pending_effect = None + success_input = self._normalize_mask( + effect_result.success_mask, + "effect_result.success_mask", + ) + failure_input = self._normalize_mask( + effect_result.failure_mask, + "effect_result.failure_mask", + ) + reported = success_input | failure_input + if (reported & ~execution_mask).any(): + raise ValueError( + "Effect verification masks must be subsets of the pending " + "effect request env_mask." + ) + verified = execution_mask & success_input + failed_effect = execution_mask & failure_input + unresolved = execution_mask & ~reported + made_progress = bool(reported.any().item()) + self._effect_failures |= failed_effect + if not unresolved.any(): + self._pending_effect = None if verified.any(): self._task_state = self._plan.expected_effects.apply( @@ -849,22 +1204,67 @@ def _finish_action( env_ids=self._context.env_ids, ) self._pending &= ~verified - failed_effect = execution_mask & ~verified - retry_mask = failed_effect | planning_failed + if unresolved.any(): + if made_progress: + self._pending_effect = self._effect_verification_request(unresolved) + return None, active_targets, events + retry_mask = self._effect_failures | planning_failed if retry_mask.any(): + effect_failure_mask = self._effect_failures.clone() + self._effect_failures.zero_() + reason = ( + ExecutionEventKind.EFFECT_VERIFICATION_FAILED + if effect_failure_mask.any() + else ExecutionEventKind.ACTION_PLANNING_FAILED + ) + reason_mask = ( + effect_failure_mask if effect_failure_mask.any() else retry_mask + ) + if effect_failure_mask.any() and planning_failed.any(): + events.append( + self._event( + ExecutionEventKind.ACTION_PLANNING_FAILED, + planning_failed, + "Planning failed for pending environments.", + ) + ) events.extend( self._attempt_action_retry( retry_mask, - ExecutionEventKind.ACTION_RETRY, + reason, "Planning or expected-effect verification failed.", + reason_mask=reason_mask, ) ) if self._status is not ExecutionStatus.RUNNING: return None, active_targets, events - return None, active_targets, events + if self._pending.any(): + return None, active_targets, events if self._pending.any(): return None, active_targets, events + hold_targets, barrier_events = self._advance_action_barrier( + active_targets, + orphaned_targets=orphaned_targets, + ) + events.extend(barrier_events) + return None, hold_targets, events + + def _advance_action_barrier( + self, + active_targets: tuple[RuntimeEndpointTarget, ...], + *, + orphaned_targets: bool, + ) -> tuple[tuple[RuntimeEndpointTarget, ...], list[ExecutionEvent]]: + """Complete an empty action cohort and install the next invocation.""" + if self._status is not ExecutionStatus.RUNNING or self._plan is None: + raise RuntimeError("Only a running planned action can cross its barrier.") + if self._pending.any(): + raise RuntimeError("The action barrier cannot advance with pending rows.") + self._pending_effect = None + self._effect_failures.zero_() + self._effect_requested_at = None + events: list[ExecutionEvent] = [] events.append( self._event( ExecutionEventKind.ACTION_COMPLETED, @@ -879,22 +1279,28 @@ def _finish_action( if self._eligible.any() else ExecutionStatus.FAILED ) + terminal_kind = ( + ExecutionEventKind.SESSION_COMPLETED + if self._status is ExecutionStatus.COMPLETED + else ExecutionEventKind.SESSION_FAILED + ) events.append( self._event( - ExecutionEventKind.SESSION_COMPLETED, + terminal_kind, self._eligible, "Invocation sequence completed.", ) ) - return None, (active_targets if orphaned_targets else ()), events + return (active_targets if orphaned_targets else ()), events self._pending = self._eligible.clone() self._pending_effect = None + self._effect_failures.zero_() self._action_retries.zero_() self._replans.zero_() self._plan_current(self._context, ExecutionEventKind.ACTION_PLANNED) events.extend(self._drain_events()) - return None, active_targets, events + return active_targets, events def _command_at( self, @@ -1037,6 +1443,8 @@ def _batched_entity_pose(self, state: EntityState) -> torch.Tensor: def _normalize_mask(self, value: torch.Tensor, name: str) -> torch.Tensor: """Validate and copy a per-environment boolean mask.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") if value.dtype != torch.bool or value.shape != (self._context.batch_size,): raise ValueError( f"{name} must be bool with shape ({self._context.batch_size},)." @@ -1050,7 +1458,12 @@ def _effect_verification_request( """Describe the current action's pending semantic-effect boundary.""" assert self._plan is not None request = self._requests[self._invocation_index] + verification_id = self._next_effect_verification_id + self._next_effect_verification_id += 1 + if self._effect_requested_at is None: + self._effect_requested_at = self._context.robot.timestamp return EffectVerificationRequest( + verification_id=verification_id, skill_id=request.skill_id, invocation_id=request.invocation_id, invocation_revision=request.revision, @@ -1058,6 +1471,10 @@ def _effect_verification_request( terminal_segment=( self._plan.segments[-1].name if self._plan.segments else None ), + requested_at=self._effect_requested_at, + deadline=( + self._action_started_at + self._plan.recovery_policy.action_timeout + ), env_mask=env_mask, expected_effects=self._plan.expected_effects, ) @@ -1101,10 +1518,19 @@ def _drain_events(self) -> list[ExecutionEvent]: self._queued_events = [] return events - def _update_terminal_status(self) -> None: - """Mark the session failed when no environment can continue.""" - if not self._eligible.any(): + def _update_terminal_status(self) -> ExecutionEvent | None: + """Mark and report failure when no environment can continue.""" + if not self._eligible.any() and self._status is ExecutionStatus.RUNNING: self._status = ExecutionStatus.FAILED + self._pending_effect = None + self._effect_failures.zero_() + self._effect_requested_at = None + return self._event( + ExecutionEventKind.SESSION_FAILED, + self._eligible, + "No environment remains eligible for execution.", + ) + return None def _tick_result( self, @@ -1127,6 +1553,7 @@ def _tick_result( __all__ = [ "EffectVerificationRequest", + "EffectVerificationResult", "ExecutionEvent", "ExecutionEventKind", "ExecutionSession", diff --git a/embodichain/lab/sim/atomic_actions/policies.py b/embodichain/lab/sim/atomic_actions/policies.py index 8f82b49a6..5ef36d4c6 100644 --- a/embodichain/lab/sim/atomic_actions/policies.py +++ b/embodichain/lab/sim/atomic_actions/policies.py @@ -162,7 +162,7 @@ class RecoveryPolicy: """Dynamic-goal rotation threshold in radians (five degrees by default).""" action_timeout: float = 30.0 - """Maximum execution time for one action attempt in seconds.""" + """Maximum time for one action attempt, including terminal effect verification.""" def __post_init__(self) -> None: if self.max_replans < 0: diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 063bd1fa5..65043e374 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -19,7 +19,7 @@ from __future__ import annotations from collections.abc import Callable -from dataclasses import dataclass +from dataclasses import dataclass, replace from enum import Enum import math import time @@ -31,6 +31,7 @@ from .bindings import RuntimeEndpointTarget from .execution import ( + EffectVerificationResult, ExecutionSession, ExecutionStatus, ExecutionTick, @@ -291,7 +292,10 @@ def is_waiting(self) -> bool: ) -EffectVerifier = Callable[[PlanningContext, ExecutionTick], torch.Tensor | None] +EffectVerifier = Callable[ + [PlanningContext, ExecutionTick], + EffectVerificationResult | None, +] """Callback that verifies a pending semantic effect for each environment.""" RunnerStepCallback = Callable[[RunnerStep], None] @@ -307,6 +311,8 @@ class ExecutionRunner: :meth:`run_until_blocked` supplies the blocking loop for tutorials and simple applications. Controller rejection, timeout, observation failure, and session exceptions all trigger a best-effort cancel-then-hold sequence. + Runner methods are designed for serialized event-loop use and are not + thread-safe. Args: session: Stateful atomic-action execution session. @@ -354,8 +360,9 @@ def __init__( def session(self) -> ExecutionSession: """Execution session advanced by this runner. - Call :meth:`revise_current` on the runner, rather than mutating the - session directly, while this runner owns scheduling. + Call :meth:`revise_current` or :meth:`deactivate_rows` on the runner, + rather than mutating the session directly, while this runner owns + scheduling. """ return self._session @@ -410,16 +417,59 @@ def revise_current(self, invocation: ActionInvocation) -> None: ) self._pending_revision = prepared + def deactivate_rows( + self, + env_mask: torch.Tensor, + *, + reason: str, + ) -> torch.Tensor: + """Permanently deactivate environment rows owned by this runner. + + The runner refreshes its cached effect boundary so a verifier cannot + submit a result correlated with a request that deactivation replaced. + In-flight controller work is neutralized for those rows by the next + due command frame according to the :class:`CommandSink` contract. + + Args: + env_mask: Rows requested for deactivation. + reason: Human-readable event message. + + Returns: + Owned mask of rows that changed from eligible to inactive. + + Raises: + RuntimeError: If the runner is already terminal. + TypeError: If ``env_mask`` is not a tensor. + ValueError: If the mask or reason is invalid. + """ + if self._status is not RunnerStatus.RUNNING: + raise RuntimeError("Only a running execution runner can deactivate rows.") + changed = self._session.deactivate_rows(env_mask, reason=reason) + if self._session.status is not ExecutionStatus.RUNNING: + self._pending_revision = None + pending_effect = self._session.pending_effect + if pending_effect is None: + self._clear_effect_boundary() + elif self._effect_tick is not None: + self._effect_tick = replace( + self._effect_tick, + status=self._session.status, + eligible_mask=self._session.eligible_mask, + task_state=self._session.task_state, + pending_effect=pending_effect, + ) + return changed + def step( self, *, - effect_success: torch.Tensor | None = None, + effect_result: EffectVerificationResult | None = None, ) -> RunnerStep: """Perform one due observation/session/controller update without sleeping. Args: - effect_success: Optional per-environment verification mask. If this - call occurs before the next cycle is due, it is not consumed and + effect_result: Optional correlated effect result. If this call + occurs before the next cycle is due, it is not consumed and must be supplied again on a later call. Returns: @@ -456,7 +506,9 @@ def step( context, ) self._pending_revision = None - tick = self._session.tick(context, effect_success=effect_success) + tick = self._session.tick(context, effect_result=effect_result) + context = self._session.latest_context + self._last_context = context except Exception as exc: return self._fail( f"Execution session failed: {type(exc).__name__}: {exc}", @@ -520,6 +572,8 @@ def step( dispatches=dispatches, ) self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time + elif tick.pending_effect is not None: + self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time else: self._next_step_at = self._clock_now() @@ -549,7 +603,7 @@ def step( self._next_step_at = self._clock_now() elif tick.status is ExecutionStatus.FAILED: return self._fail( - "Execution session exhausted its recovery budget.", + "Execution session failed; inspect its terminal events for the cause.", context=context, tick=tick, dispatches=dispatches, @@ -616,7 +670,7 @@ def run_until_blocked( """ if max_steps <= 0: raise ValueError("max_steps must be greater than zero.") - effect_success: torch.Tensor | None = None + effect_result: EffectVerificationResult | None = None now = self._clock_now() last_result = self._result( timestamp=now, @@ -624,30 +678,12 @@ def run_until_blocked( context=self._effect_context, tick=self._effect_tick, ) - if self.effect_verification_pending: - if ( - effect_verifier is None - or self._effect_context is None - or self._effect_tick is None - ): - return last_result - try: - effect_success = effect_verifier( - self._effect_context, - self._effect_tick, - ) - except Exception as exc: - return self._fail( - f"Effect verifier failed: {type(exc).__name__}: {exc}", - context=self._effect_context, - tick=self._effect_tick, - ) - if effect_success is None: - return last_result + if self.effect_verification_pending and effect_verifier is None: + return last_result for _ in range(max_steps): - result = self.step(effect_success=effect_success) + result = self.step(effect_result=effect_result) if result.tick is not None: - effect_success = None + effect_result = None if on_step is not None: try: on_step(result) @@ -668,7 +704,7 @@ def run_until_blocked( if effect_verifier is None or result.context is None: return result try: - effect_success = effect_verifier(result.context, result.tick) + effect_result = effect_verifier(result.context, result.tick) except Exception as exc: return self._fail( f"Effect verifier failed: {type(exc).__name__}: {exc}", @@ -676,7 +712,7 @@ def run_until_blocked( tick=result.tick, dispatches=list(result.dispatches), ) - if effect_success is None: + if effect_result is None: return result if result.wait_duration > 0.0: try: diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 7243013fd..141813b98 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -40,7 +40,10 @@ EndpointCommand, EntityState, ExecutionEventKind, + ExecutionSession, ExecutionStatus, + ExecutionTick, + EffectVerificationResult, GraspGoal, HeldObjectState, JointPositionPayload, @@ -150,6 +153,23 @@ def _plan( return replace(plan, plan_success=torch.zeros_like(plan.plan_success)) +class MixedEffectAction(EffectAction): + """Effect action whose final environment row always fails planning.""" + + skill_id: ClassVar[str] = "mixed_effect" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + plan_success = torch.ones_like(plan.plan_success) + plan_success[-1] = False + return replace(plan, plan_success=plan_success) + + class NonuniformTimingAction(DynamicAction): """Test action with explicit nonuniform waypoint arrival intervals.""" @@ -450,6 +470,44 @@ def _destination_invocation( ) +def _effect_session( + *, + batch_size: int = 1, + max_action_retries: int = 2, + action_timeout: float = 30.0, + eligible_mask: torch.Tensor | None = None, + action: EffectAction | None = None, +) -> tuple[ExecutionSession, ExecutionTick]: + """Advance a test effect action to its verification boundary.""" + engine, _ = _engine(batch_size=batch_size) + selected_action = EffectAction() if action is None else action + engine.register(selected_action) + base = _invocation( + engine, + max_action_retries=max_action_retries, + action_timeout=action_timeout, + ) + invocation = ActionInvocation( + skill_id=selected_action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + qpos = tuple(0.0 for _ in range(batch_size)) + target = tuple(0.2 for _ in range(batch_size)) + session = engine.start( + (invocation,), + _context(0.0, qpos, target, 0), + eligible_mask=eligible_mask, + ) + session.tick(_context(0.0, qpos, target, 0)) + session.tick(_context(0.1, qpos, target, 0)) + waiting = session.tick(_context(0.2, target, target, 0)) + assert waiting.pending_effect is not None + return session, waiting + + def _joint_positions(command: RuntimeCommandFrame | None) -> torch.Tensor: """Return the only joint-position payload emitted by the test action.""" assert command is not None @@ -474,6 +532,112 @@ def test_session_completes_incremental_command_sequence() -> None: assert final.eligible_mask.tolist() == [True] +def test_initial_eligibility_is_sticky_across_invocation_barriers() -> None: + engine, _ = _engine(batch_size=2) + invocation = _invocation(engine) + supplied_mask = torch.tensor([True, False]) + session = engine.start( + (invocation, invocation), + _context(0.0, (0.0, 0.0), (0.2, 0.2), 0), + eligible_mask=supplied_mask, + ) + supplied_mask.fill_(True) + + first = session.tick(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) + session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.2), 0)) + barrier = session.tick(_context(0.2, (0.2, 7.0), (0.2, 0.2), 0)) + second_action = session.tick(_context(0.3, (0.2, 7.0), (0.2, 0.2), 0)) + + assert first.command is not None + assert first.command.active_mask.tolist() == [True, False] + assert barrier.status is ExecutionStatus.RUNNING + assert second_action.command is not None + assert second_action.command.active_mask.tolist() == [True, False] + assert second_action.eligible_mask.tolist() == [True, False] + + +def test_empty_initial_eligibility_fails_without_planning() -> None: + engine, action = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.2), 0) + + session = engine.start( + (_invocation(engine),), + initial, + eligible_mask=torch.tensor([False, False]), + ) + terminal = session.tick(initial) + + assert action.plan_count == 0 + assert terminal.status is ExecutionStatus.FAILED + assert terminal.eligible_mask.tolist() == [False, False] + assert any( + event.kind is ExecutionEventKind.SESSION_FAILED for event in terminal.events + ) + + +def test_initial_eligibility_is_owned_and_validated() -> None: + engine, _ = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.2), 0) + + with pytest.raises(TypeError, match="eligible_mask must be a torch.Tensor"): + engine.start((_invocation(engine),), initial, eligible_mask=[True, False]) + with pytest.raises(ValueError, match="bool with shape"): + engine.start( + (_invocation(engine),), + initial, + eligible_mask=torch.tensor([1, 0]), + ) + with pytest.raises(ValueError, match="bool with shape"): + engine.start( + (_invocation(engine),), + initial, + eligible_mask=torch.tensor([True]), + ) + + supplied = torch.tensor([True, False]) + session = engine.start( + (_invocation(engine),), + initial, + eligible_mask=supplied, + ) + supplied.fill_(False) + observed = session.eligible_mask + observed.fill_(False) + + assert session.eligible_mask.tolist() == [True, False] + + +def test_deactivate_rows_is_sticky_and_masks_the_next_command() -> None: + engine, _ = _engine(batch_size=2) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.2), 0) + session = engine.start((_invocation(engine),), initial) + session.tick(initial) + + changed = session.deactivate_rows( + torch.tensor([False, True]), + reason="environment terminated", + ) + unchanged = session.deactivate_rows( + torch.tensor([False, True]), + reason="duplicate termination", + ) + tick = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.2), 0)) + + assert changed.tolist() == [False, True] + assert unchanged.tolist() == [False, False] + assert tick.command is not None + assert tick.command.active_mask.tolist() == [True, False] + assert tick.eligible_mask.tolist() == [True, False] + deactivated = [ + event + for event in tick.events + if event.kind is ExecutionEventKind.ROWS_DEACTIVATED + ] + assert len(deactivated) == 1 + assert deactivated[0].env_mask.tolist() == [False, True] + assert deactivated[0].message == "environment terminated" + + def test_session_commands_schedule_arrivals_and_final_settling() -> None: engine, _ = _engine() engine.register(NonuniformTimingAction()) @@ -1154,7 +1318,11 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None still_waiting = session.tick(_context(0.25, 0.2, 0.2, 0)) completed = session.tick( _context(0.3, 0.2, 0.2, 0), - effect_success=torch.tensor([True]), + effect_result=EffectVerificationResult( + waiting.pending_effect.verification_id, + torch.tensor([True]), + torch.tensor([False]), + ), ) assert waiting.status is ExecutionStatus.RUNNING @@ -1182,6 +1350,347 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None assert completed.task_state.get_held_object("arm") is not None +def test_initially_ineligible_rows_never_receive_effects() -> None: + session, waiting = _effect_session( + batch_size=2, + eligible_mask=torch.tensor([True, False]), + ) + request = waiting.pending_effect + assert request is not None + assert request.env_mask.tolist() == [True, False] + + completed = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + held = completed.task_state.get_held_object("arm") + assert completed.status is ExecutionStatus.COMPLETED + assert completed.eligible_mask.tolist() == [True, False] + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + + +def test_partial_effect_success_commits_resolved_rows_and_shrinks_request() -> None: + session, waiting = _effect_session(batch_size=2) + first_request = waiting.pending_effect + assert first_request is not None + + no_progress = session.tick( + _context(0.205, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([False, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + assert no_progress.pending_effect is not None + assert no_progress.pending_effect.verification_id == first_request.verification_id + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + held = partial.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + assert partial.pending_effect is not None + assert partial.pending_effect.env_mask.tolist() == [False, True] + assert partial.pending_effect.verification_id != first_request.verification_id + assert partial.pending_effect.requested_at == first_request.requested_at + assert partial.pending_effect.deadline == first_request.deadline + assert not any( + event.kind is ExecutionEventKind.ACTION_RETRY for event in partial.events + ) + + with pytest.raises(ValueError, match="verification_id"): + session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + first_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + current_request = partial.pending_effect + completed = session.tick( + _context(0.23, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + current_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + completed_held = completed.task_state.get_held_object("arm") + assert completed.status is ExecutionStatus.COMPLETED + assert completed_held is not None and completed_held.env_mask is not None + assert completed_held.env_mask.tolist() == [True, True] + + +def test_effect_result_masks_are_owned_disjoint_and_request_scoped() -> None: + success = torch.tensor([True, False]) + failure = torch.tensor([False, True]) + result = EffectVerificationResult(0, success, failure) + success.fill_(False) + failure.fill_(False) + assert result.success_mask.tolist() == [True, False] + assert result.failure_mask.tolist() == [False, True] + + with pytest.raises(ValueError, match="must not overlap"): + EffectVerificationResult( + 0, + torch.tensor([True, False]), + torch.tensor([True, False]), + ) + + session, waiting = _effect_session(batch_size=2) + request = waiting.pending_effect + assert request is not None + request.env_mask.fill_(False) + published_effect = request.expected_effects.held_object_updates["arm"] + assert published_effect is not None + published_effect.object_to_eef.fill_(9.0) + published_effect.grasp_xpos.fill_(8.0) + published_effect.semantics.affordance.set_custom_config("mutated", True) + preserved = session.pending_effect + assert preserved is not None + assert preserved.env_mask.tolist() == [True, True] + preserved_effect = preserved.expected_effects.held_object_updates["arm"] + assert preserved_effect is not None + assert torch.equal(preserved_effect.object_to_eef, torch.eye(4)) + assert torch.equal(preserved_effect.grasp_xpos, torch.eye(4)) + assert preserved_effect.semantics.affordance.get_custom_config("mutated") is None + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + preserved.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + current = partial.pending_effect + assert current is not None + held = partial.task_state.get_held_object("arm") + assert held is not None + assert torch.equal(held.object_to_eef[0], torch.eye(4)) + + with pytest.raises(ValueError, match="subsets"): + session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + current.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + +def test_state_delta_snapshot_owns_effect_data_and_preserves_live_entity() -> None: + entity = UncopyableEntity() + semantics = ObjectSemantics( + affordance=Affordance(custom_config={"threshold": [1.0]}), + geometry={"size": torch.ones(3)}, + properties={"mass": torch.tensor(1.0)}, + label="snapshot-object", + entity=entity, + ) + held = HeldObjectState( + semantics=semantics, + object_to_eef=torch.eye(4), + grasp_xpos=torch.eye(4), + ) + delta = StateDelta(held_object_updates={"arm": held}) + + snapshot = delta.snapshot() + copied = snapshot.held_object_updates["arm"] + assert copied is not None + assert copied is not held + assert copied.semantics is not semantics + assert copied.semantics.entity is entity + assert copied.semantics.affordance is not semantics.affordance + assert copied.object_to_eef.data_ptr() != held.object_to_eef.data_ptr() + assert copied.grasp_xpos.data_ptr() != held.grasp_xpos.data_ptr() + + copied.object_to_eef.fill_(7.0) + copied.semantics.affordance.custom_config["threshold"].append(2.0) + copied.semantics.geometry["size"].zero_() + assert torch.equal(held.object_to_eef, torch.eye(4)) + assert semantics.affordance.custom_config["threshold"] == [1.0] + assert torch.equal(semantics.geometry["size"], torch.ones(3)) + + +def test_partial_effect_failure_waits_for_unresolved_rows_then_retries_failure() -> ( + None +): + session, waiting = _effect_session(batch_size=2, max_action_retries=1) + request = waiting.pending_effect + assert request is not None + + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([False, False]), + failure_mask=torch.tensor([True, False]), + ), + ) + + assert partial.pending_effect is not None + assert partial.pending_effect.env_mask.tolist() == [False, True] + assert not any( + event.kind is ExecutionEventKind.ACTION_RETRY for event in partial.events + ) + + unresolved_request = partial.pending_effect + resolved = session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + unresolved_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + held = resolved.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [False, True] + failed_event = next( + event + for event in resolved.events + if event.kind is ExecutionEventKind.EFFECT_VERIFICATION_FAILED + ) + retry_event = next( + event + for event in resolved.events + if event.kind is ExecutionEventKind.ACTION_RETRY + ) + assert failed_event.env_mask.tolist() == [True, False] + assert retry_event.env_mask.tolist() == [True, False] + + retry_command = session.tick(_context(0.23, (0.2, 0.2), (0.2, 0.2), 0)) + assert retry_command.command is not None + assert retry_command.command.active_mask.tolist() == [True, False] + + +def test_effect_failure_exhaustion_advances_completed_rows_without_empty_request() -> ( + None +): + session, waiting = _effect_session(batch_size=2, max_action_retries=0) + request = waiting.pending_effect + assert request is not None + + terminal = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, True]), + ), + ) + + held = terminal.task_state.get_held_object("arm") + assert terminal.status is ExecutionStatus.COMPLETED + assert terminal.eligible_mask.tolist() == [True, False] + assert terminal.pending_effect is None + assert terminal.command is None + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + assert any( + event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + and event.env_mask.tolist() == [False, True] + for event in terminal.events + ) + assert any( + event.kind is ExecutionEventKind.SESSION_COMPLETED for event in terminal.events + ) + + +def test_deactivating_last_unresolved_effect_row_advances_barrier() -> None: + session, waiting = _effect_session(batch_size=2) + request = waiting.pending_effect + assert request is not None + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + assert partial.pending_effect is not None + + session.deactivate_rows( + torch.tensor([False, True]), + reason="effect observation terminated", + ) + terminal = session.tick(_context(0.22, (0.2, 0.2), (0.2, 0.2), 0)) + + assert terminal.status is ExecutionStatus.COMPLETED + assert terminal.eligible_mask.tolist() == [True, False] + assert terminal.pending_effect is None + assert not any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED + for event in terminal.events + ) + + +def test_deactivating_all_effect_rows_is_terminal_and_clears_request() -> None: + session, _ = _effect_session(batch_size=2) + + changed = session.deactivate_rows( + torch.tensor([True, True]), + reason="all environments terminated", + ) + terminal = session.tick(_context(0.21, (0.2, 0.2), (0.2, 0.2), 0)) + + assert changed.tolist() == [True, True] + assert terminal.status is ExecutionStatus.FAILED + assert terminal.pending_effect is None + assert any( + event.kind is ExecutionEventKind.SESSION_FAILED for event in terminal.events + ) + assert not any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED + for event in terminal.events + ) + + +def test_effect_request_deadline_is_stable_and_accepts_result_at_boundary() -> None: + session, waiting = _effect_session(action_timeout=0.25) + request = waiting.pending_effect + assert request is not None + assert request.requested_at == pytest.approx(0.2) + assert request.deadline == pytest.approx(0.25) + + polled = session.tick(_context(0.24, 0.2, 0.2, 0)) + assert polled.pending_effect is not None + assert polled.pending_effect.verification_id == request.verification_id + assert polled.pending_effect.requested_at == request.requested_at + assert polled.pending_effect.deadline == request.deadline + + completed = session.tick( + _context(0.25, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ), + ) + assert completed.status is ExecutionStatus.COMPLETED + + def test_session_revision_cannot_abandon_pending_effect_verification() -> None: engine, _ = _engine() engine.register(EffectAction()) @@ -1199,13 +1708,17 @@ def test_session_revision_cannot_abandon_pending_effect_verification() -> None: waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) assert waiting.pending_effect is not None - with pytest.raises(RuntimeError, match="awaiting verification"): + with pytest.raises(RuntimeError, match="physical-effect resolution"): session.revise_current(replace(invocation, revision=1)) assert session.effect_verification_pending is True completed = session.tick( _context(0.3, 0.2, 0.2, 0), - effect_success=torch.tensor([True]), + effect_result=EffectVerificationResult( + waiting.pending_effect.verification_id, + torch.tensor([True]), + torch.tensor([False]), + ), ) assert completed.status is ExecutionStatus.COMPLETED assert completed.task_state.get_held_object("arm") is not None @@ -1226,9 +1739,15 @@ def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: session.tick(_context(0.0, 0.0, 0.2, 0)) session.tick(_context(0.1, 0.0, 0.2, 0)) + waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert waiting.pending_effect is not None failed = session.tick( - _context(0.2, 0.2, 0.2, 0), - effect_success=torch.tensor([False]), + _context(0.3, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + waiting.pending_effect.verification_id, + torch.tensor([False]), + torch.tensor([True]), + ), ) assert failed.status is ExecutionStatus.FAILED @@ -1238,6 +1757,199 @@ def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: ) +def test_pending_effect_timeout_exhausts_without_committing_late_result() -> None: + engine, _ = _engine() + engine.register(EffectAction()) + base = _invocation( + engine, + max_action_retries=0, + action_timeout=0.25, + ) + invocation = ActionInvocation( + skill_id="effect", + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.1, 0.0, 0.2, 0)) + waiting = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert waiting.pending_effect is not None + + timed_out = session.tick( + _context(0.3, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + waiting.pending_effect.verification_id, + torch.tensor([True]), + torch.tensor([False]), + ), + ) + + kinds = {event.kind for event in timed_out.events} + assert ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT in kinds + assert ExecutionEventKind.RECOVERY_EXHAUSTED in kinds + assert timed_out.status is ExecutionStatus.FAILED + assert timed_out.pending_effect is None + assert timed_out.task_state.get_held_object("arm") is None + + +def test_effect_timeout_exhaustion_advances_rows_already_verified() -> None: + session, waiting = _effect_session( + batch_size=2, + max_action_retries=0, + action_timeout=0.25, + ) + request = waiting.pending_effect + assert request is not None + partial = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + assert partial.pending_effect is not None + + terminal = session.tick(_context(0.3, (0.2, 0.2), (0.2, 0.2), 0)) + + held = terminal.task_state.get_held_object("arm") + assert terminal.status is ExecutionStatus.COMPLETED + assert terminal.eligible_mask.tolist() == [True, False] + assert terminal.pending_effect is None + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + timeout_event = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT + ) + assert timeout_event.env_mask.tolist() == [False, True] + + +def test_effect_timeout_charges_concurrent_planning_failures() -> None: + session, _ = _effect_session( + batch_size=2, + max_action_retries=1, + action_timeout=0.25, + action=MixedEffectAction(), + ) + + first_retry = session.tick(_context(0.3, (0.2, 0.2), (0.2, 0.2), 0)) + retry_event = next( + event + for event in first_retry.events + if event.kind is ExecutionEventKind.ACTION_RETRY + ) + planning_event = next( + event + for event in first_retry.events + if event.kind is ExecutionEventKind.ACTION_PLANNING_FAILED + ) + assert retry_event.env_mask.tolist() == [True, True] + assert planning_event.env_mask.tolist() == [False, True] + + session.tick(_context(0.4, (0.2, 0.2), (0.2, 0.2), 0)) + second_wait = session.tick(_context(0.5, (0.2, 0.2), (0.2, 0.2), 0)) + assert second_wait.pending_effect is not None + terminal = session.tick(_context(0.6, (0.2, 0.2), (0.2, 0.2), 0)) + + assert terminal.status is ExecutionStatus.FAILED + assert terminal.eligible_mask.tolist() == [False, False] + exhausted = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + ) + assert exhausted.env_mask.tolist() == [True, True] + + +def test_deferred_effect_failure_charges_concurrent_planning_failures() -> None: + session, waiting = _effect_session( + batch_size=3, + max_action_retries=0, + action=MixedEffectAction(), + ) + request = waiting.pending_effect + assert request is not None + partial = session.tick( + _context(0.21, (0.2, 0.2, 0.2), (0.2, 0.2, 0.2), 0), + effect_result=EffectVerificationResult( + request.verification_id, + success_mask=torch.tensor([False, False, False]), + failure_mask=torch.tensor([True, False, False]), + ), + ) + assert partial.pending_effect is not None + assert partial.pending_effect.env_mask.tolist() == [False, True, False] + + session.deactivate_rows( + torch.tensor([False, True, False]), + reason="unresolved effect row terminated", + ) + terminal = session.tick(_context(0.22, (0.2, 0.2, 0.2), (0.2, 0.2, 0.2), 0)) + + assert terminal.status is ExecutionStatus.FAILED + assert terminal.eligible_mask.tolist() == [False, False, False] + planning_event = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.ACTION_PLANNING_FAILED + ) + exhausted = next( + event + for event in terminal.events + if event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + ) + assert planning_event.env_mask.tolist() == [False, False, True] + assert exhausted.env_mask.tolist() == [True, False, True] + + +def test_effect_retry_invalidates_previous_verification_id() -> None: + engine, _ = _engine() + engine.register(EffectAction()) + base = _invocation( + engine, + max_action_retries=1, + action_timeout=0.25, + ) + invocation = ActionInvocation( + skill_id="effect", + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.1, 0.0, 0.2, 0)) + first_wait = session.tick(_context(0.2, 0.2, 0.2, 0)) + assert first_wait.pending_effect is not None + old_id = first_wait.pending_effect.verification_id + old_deadline = first_wait.pending_effect.deadline + + retry = session.tick(_context(0.3, 0.2, 0.2, 0)) + assert retry.command is not None + assert any(event.kind is ExecutionEventKind.ACTION_RETRY for event in retry.events) + session.tick(_context(0.4, 0.2, 0.2, 0)) + second_wait = session.tick(_context(0.5, 0.2, 0.2, 0)) + assert second_wait.pending_effect is not None + assert second_wait.pending_effect.verification_id != old_id + assert second_wait.pending_effect.deadline > old_deadline + + with pytest.raises(ValueError, match="verification_id"): + session.tick( + _context(0.55, 0.2, 0.2, 0), + effect_result=EffectVerificationResult( + old_id, + torch.tensor([True]), + torch.tensor([False]), + ), + ) + + def test_failed_effect_plan_retries_without_requesting_effect_verification() -> None: engine, _ = _engine() engine.register(FailedEffectAction()) diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index 7fe66b73c..ed9432387 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -37,9 +37,11 @@ CommandAckStatus, CommandOperation, EndEffectorPoseGoal, + EffectVerificationResult, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, + ExecutionTick, HeldObjectState, JOINT_POSITION_CAPABILITY, JointPositionPayload, @@ -265,6 +267,8 @@ def _make_runner( with_effect: bool = False, batch_size: int = BATCH_SIZE, control_joint_ids: tuple[int, ...] | None = None, + max_action_retries: int = 2, + action_timeout: float = 10.0, ) -> tuple[ ExecutionRunner, FakeClock, @@ -301,8 +305,9 @@ def _make_runner( motion_policy=MotionPolicy(sample_count=3, control_dt=FIRST_INTERVAL), recovery_policy=RecoveryPolicy( max_replans=2, + max_action_retries=max_action_retries, tracking_error_threshold=0.05, - action_timeout=10.0, + action_timeout=action_timeout, ), ) session = engine.start((invocation,), initial_context) @@ -316,6 +321,28 @@ def _make_runner( return runner, clock, provider, sink, action +def _successful_effect_result( + context: PlanningContext, + tick: ExecutionTick, +) -> EffectVerificationResult: + """Correlate a successful result with the pending effect boundary.""" + pending_effect = tick.pending_effect + assert pending_effect is not None + return EffectVerificationResult( + verification_id=pending_effect.verification_id, + success_mask=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + failure_mask=torch.zeros( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + ) + + def test_joint_feedback_ignores_motion_outside_bound_endpoint() -> None: runner, clock, provider, sink, action = _make_runner(control_joint_ids=(0,)) @@ -554,15 +581,12 @@ def test_runner_revision_rejects_pending_effect_verification() -> None: revision=1, ) - with pytest.raises(RuntimeError, match="awaiting verification"): + with pytest.raises(RuntimeError, match="physical-effect resolution"): runner.revise_current(revised) assert runner.effect_verification_pending is True completed = runner.run_until_blocked( - effect_verifier=lambda context, tick: torch.ones( - context.batch_size, - dtype=torch.bool, - ) + effect_verifier=_successful_effect_result, ) assert completed.status is RunnerStatus.COMPLETED @@ -620,9 +644,7 @@ def test_blocking_runner_verifies_effect_before_committing_task_state() -> None: runner, _, _, _, _ = _make_runner(with_effect=True) completed = runner.run_until_blocked( - effect_verifier=lambda context, tick: torch.ones( - context.batch_size, dtype=torch.bool - ) + effect_verifier=_successful_effect_result, ) assert completed.status is RunnerStatus.COMPLETED @@ -641,12 +663,182 @@ def test_blocking_runner_resumes_a_stored_effect_verification_boundary() -> None assert runner.effect_verification_pending is True completed = runner.run_until_blocked( - effect_verifier=lambda context, tick: torch.ones( - context.batch_size, dtype=torch.bool - ) + effect_verifier=_successful_effect_result, ) assert runner.effect_verification_pending is False assert completed.status is RunnerStatus.COMPLETED assert completed.tick is not None assert completed.tick.task_state.get_held_object("arm") is not None + + +def test_resumed_effect_verifier_uses_a_fresh_observation() -> None: + runner, clock, _, _, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.context is not None + blocked_at = blocked.context.robot.timestamp + clock.advance(0.5) + resumed_at = clock.now() + observed_at: list[float] = [] + + def record_fresh_context( + context: PlanningContext, + tick: ExecutionTick, + ) -> EffectVerificationResult: + observed_at.append(context.robot.timestamp) + return _successful_effect_result(context, tick) + + completed = runner.run_until_blocked(effect_verifier=record_fresh_context) + + assert completed.status is RunnerStatus.COMPLETED + assert observed_at and observed_at[0] >= resumed_at + assert observed_at[0] > blocked_at + + +def test_partial_effect_verifier_receives_the_committed_task_state() -> None: + runner, _, _, _, _ = _make_runner(with_effect=True, batch_size=2) + observations: list[list[bool] | None] = [] + + def verify_in_two_updates( + context: PlanningContext, + tick: ExecutionTick, + ) -> EffectVerificationResult: + pending_effect = tick.pending_effect + assert pending_effect is not None + held = context.task.get_held_object("arm") + observations.append( + None if held is None or held.env_mask is None else held.env_mask.tolist() + ) + if pending_effect.env_mask.tolist() == [True, True]: + return EffectVerificationResult( + verification_id=pending_effect.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ) + assert pending_effect.env_mask.tolist() == [False, True] + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True, False] + assert torch.equal(context.task.held_objects["arm"].env_mask, held.env_mask) + return EffectVerificationResult( + verification_id=pending_effect.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ) + + completed = runner.run_until_blocked(effect_verifier=verify_in_two_updates) + + assert completed.status is RunnerStatus.COMPLETED + assert observations == [None, [True, False]] + assert completed.context is not None and completed.tick is not None + assert completed.context.task is completed.tick.task_state + + +def test_runner_effect_timeout_replans_and_invalidates_cached_request() -> None: + runner, clock, _, _, action = _make_runner( + with_effect=True, + max_action_retries=1, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + plan_count = action.plan_count + clock.advance(request.deadline - clock.now() + 0.01) + + retry = runner.step() + + assert retry.status is RunnerStatus.RUNNING + assert retry.tick is not None and retry.tick.command is not None + assert runner.effect_verification_pending is False + assert action.plan_count == plan_count + 1 + kinds = {event.kind for event in retry.tick.events} + assert ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT in kinds + assert ExecutionEventKind.ACTION_RETRY in kinds + assert ExecutionEventKind.REPLANNED in kinds + + +def test_runner_effect_timeout_exhaustion_cancels_and_holds() -> None: + runner, clock, _, sink, _ = _make_runner( + with_effect=True, + max_action_retries=0, + action_timeout=2.0, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + request = blocked.tick.pending_effect + clock.advance(request.deadline - clock.now() + 0.01) + + failed = runner.step() + + assert failed.status is RunnerStatus.FAILED + assert runner.effect_verification_pending is False + assert failed.tick is not None and failed.tick.pending_effect is None + assert failed.tick.task_state.get_held_object("arm") is None + assert [item.operation for item in failed.dispatches[-2:]] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + + +def test_runner_deactivation_refreshes_cached_effect_request() -> None: + runner, _, _, _, _ = _make_runner(with_effect=True, batch_size=2) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + old_id = blocked.tick.pending_effect.verification_id + + changed = runner.deactivate_rows( + torch.tensor([False, True]), + reason="environment terminated", + ) + refreshed = runner.run_until_blocked() + + assert changed.tolist() == [False, True] + assert refreshed.tick is not None and refreshed.tick.pending_effect is not None + assert refreshed.tick.pending_effect.env_mask.tolist() == [True, False] + assert refreshed.tick.pending_effect.verification_id != old_id + + def verify_remaining( + context: PlanningContext, + tick: ExecutionTick, + ) -> EffectVerificationResult: + pending_effect = tick.pending_effect + assert pending_effect is not None + return EffectVerificationResult( + verification_id=pending_effect.verification_id, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ) + + completed = runner.run_until_blocked(effect_verifier=verify_remaining) + + assert completed.status is RunnerStatus.COMPLETED + assert completed.tick is not None + assert completed.tick.eligible_mask.tolist() == [True, False] + + +def test_blocking_runner_fails_safely_for_a_mismatched_effect_result() -> None: + runner, _, _, sink, _ = _make_runner(with_effect=True) + + def mismatched_effect_result( + context: PlanningContext, + tick: ExecutionTick, + ) -> EffectVerificationResult: + pending_effect = tick.pending_effect + assert pending_effect is not None + return EffectVerificationResult( + verification_id=pending_effect.verification_id + 1, + success_mask=torch.ones(context.batch_size, dtype=torch.bool), + failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), + ) + + failed = runner.run_until_blocked(effect_verifier=mismatched_effect_result) + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches[-2:]] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert failed.message is not None + assert "verification_id does not match" in failed.message