From e8623ffe5ec1d794382c3884540b1d6982514587 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 03:33:12 +0800 Subject: [PATCH 1/3] refactor(atomic-actions): preserve per-environment runtime lifecycle --- agent_context/MAP.yaml | 4 + .../topics/atomic-actions/atomic-actions.md | 48 +- .../overview/sim/atomic_actions/index.md | 43 +- docs/source/tutorial/atomic_actions.rst | 76 +- .../lab/sim/atomic_actions/__init__.py | 2 + embodichain/lab/sim/atomic_actions/effects.py | 108 ++- embodichain/lab/sim/atomic_actions/engine.py | 11 +- .../lab/sim/atomic_actions/execution.py | 517 +++++++++++-- .../lab/sim/atomic_actions/policies.py | 2 +- embodichain/lab/sim/atomic_actions/runner.py | 104 ++- .../sim/atomic_actions/test_engine_per_env.py | 722 +++++++++++++++++- tests/sim/atomic_actions/test_runner.py | 216 +++++- 12 files changed, 1728 insertions(+), 125 deletions(-) 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 From 1317f99a8db208b925c5a4fd0d371a2c495d8200 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 03:50:13 +0800 Subject: [PATCH 2/3] refactor(atomic-actions): verify effects on due observations --- .../topics/atomic-actions/atomic-actions.md | 16 +- .../overview/sim/atomic_actions/index.md | 2 +- docs/source/tutorial/atomic_actions.rst | 9 +- .../lab/sim/atomic_actions/execution.py | 11 +- embodichain/lab/sim/atomic_actions/runner.py | 66 +++--- .../atomic_action/moving_target_recovery.py | 14 +- .../sim/atomic_actions/test_engine_per_env.py | 27 +++ tests/sim/atomic_actions/test_runner.py | 199 ++++++++++++++++-- 8 files changed, 285 insertions(+), 59 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index de796f9fb..dfcd1dbb1 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -420,14 +420,24 @@ correlated `EffectVerificationResult`. Its disjoint `success_mask` and 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` +robot-observation timestamp domain, a session-local `attempt_generation`, and +an owned effect snapshot. Mask shrinkage creates a new ID without extending the +deadline or changing the generation; installing a replacement plan increments +the generation. 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. +For synchronous verification, pass `effect_verifier(context, request)` to +`runner.step()` or `run_until_blocked()`. The runner calls it after the fresh +due-cycle observation and supplies its result to `session.tick()` in that same +cycle. It does not call the verifier when the observation timestamp is already +past the request deadline. A verifier must return an exact +`EffectVerificationResult`; all-false masks mean unresolved. External +asynchronous integrations instead pass `effect_result` explicitly on a due +`step()` call. + ```python request = tick.pending_effect effect_result = EffectVerificationResult( diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 56afd9a57..e033a7937 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -431,7 +431,7 @@ an older custom action by renaming its implementation to `_plan()`. | `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.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.step(effect_result=..., effect_verifier=...)` | Non-blocking controller integration | Observes only when due; accepts either an asynchronous correlated result or a synchronous verifier, never both | | `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 | diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 181472035..b451fe9ce 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -482,9 +482,7 @@ correlated per-environment verification result: from embodichain.lab.sim.atomic_actions import EffectVerificationResult - def verify_effect(context, tick): - request = tick.pending_effect - assert request is not None + def verify_effect(context, request): success_mask, failure_mask = verify_grasp_or_release(context, request.env_mask) return EffectVerificationResult( verification_id=request.verification_id, @@ -495,7 +493,10 @@ correlated per-environment verification result: 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; +physical grasp or release. The runner invokes this synchronous callback after a +fresh due-cycle observation and feeds its result to the session in that same +cycle. Returning all-false masks keeps the remaining rows unresolved. If +verification is asynchronous, omit the callback; ``run_until_blocked`` returns at the verification boundary and the application can later resume from the *current* pending request: diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index d44f24505..ba21c669c 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -108,7 +108,9 @@ class EffectVerificationRequest: ``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. + only a newly installed plan starts a new attempt deadline. + ``attempt_generation`` is session-local and remains stable when partial + resolution or row deactivation replaces only the request ID. """ verification_id: int @@ -116,6 +118,7 @@ class EffectVerificationRequest: invocation_id: str | None invocation_revision: int invocation_index: int + attempt_generation: int terminal_segment: str | None requested_at: float deadline: float @@ -135,6 +138,8 @@ def __post_init__(self) -> None: raise ValueError("invocation_revision must be non-negative.") if self.invocation_index < 0: raise ValueError("invocation_index must be non-negative.") + if type(self.attempt_generation) is not int or self.attempt_generation < 0: + raise ValueError("attempt_generation must be a non-negative integer.") if self.terminal_segment is not None and ( not isinstance(self.terminal_segment, str) or not self.terminal_segment ): @@ -166,6 +171,7 @@ def snapshot(self) -> EffectVerificationRequest: invocation_id=self.invocation_id, invocation_revision=self.invocation_revision, invocation_index=self.invocation_index, + attempt_generation=self.attempt_generation, terminal_segment=self.terminal_segment, requested_at=self.requested_at, deadline=self.deadline, @@ -304,6 +310,7 @@ def __init__( ] = {} self._planned_scene = context.scene self._action_started_at = context.robot.timestamp + self._attempt_generation = -1 self._last_joint_command: torch.Tensor | None = None self._last_joint_ids: tuple[int, ...] = () self._last_command_mask = torch.zeros( @@ -887,6 +894,7 @@ def _install_plan( ): self._active_targets = replacement_targets self._plan = plan + self._attempt_generation += 1 self._waypoint_index = 0 self._planned_scene = context.scene self._action_started_at = context.robot.timestamp @@ -1468,6 +1476,7 @@ def _effect_verification_request( invocation_id=request.invocation_id, invocation_revision=request.revision, invocation_index=self._invocation_index, + attempt_generation=self._attempt_generation, terminal_segment=( self._plan.segments[-1].name if self._plan.segments else None ), diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 65043e374..8dac661a6 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -31,6 +31,7 @@ from .bindings import RuntimeEndpointTarget from .execution import ( + EffectVerificationRequest, EffectVerificationResult, ExecutionSession, ExecutionStatus, @@ -293,10 +294,10 @@ def is_waiting(self) -> bool: EffectVerifier = Callable[ - [PlanningContext, ExecutionTick], - EffectVerificationResult | None, + [PlanningContext, EffectVerificationRequest], + EffectVerificationResult, ] -"""Callback that verifies a pending semantic effect for each environment.""" +"""Synchronous verifier called on a fresh due-cycle observation.""" RunnerStepCallback = Callable[[RunnerStep], None] """Optional observer called after every blocking runner-loop iteration.""" @@ -464,6 +465,7 @@ def step( self, *, effect_result: EffectVerificationResult | None = None, + effect_verifier: EffectVerifier | None = None, ) -> RunnerStep: """Perform one due observation/session/controller update without sleeping. @@ -471,11 +473,22 @@ def step( 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. + effect_verifier: Optional synchronous verifier for the current + pending request. It runs after a fresh due-cycle observation + and before the session consumes the result. It is not called + after the request deadline. Mutually exclusive with + ``effect_result``. Returns: Runner status, optional session tick, controller acknowledgements, and time remaining before another update is due. """ + if effect_result is not None and effect_verifier is not None: + raise ValueError( + "effect_result and effect_verifier are mutually exclusive." + ) + if effect_verifier is not None and not callable(effect_verifier): + raise TypeError("effect_verifier must be callable or None.") now = self._clock_now() if self._status is not RunnerStatus.RUNNING: return self._result(timestamp=now) @@ -499,6 +512,25 @@ def step( ) self._last_context = context + pending_effect = self._session.pending_effect + if ( + effect_verifier is not None + and pending_effect is not None + and context.robot.timestamp <= pending_effect.deadline + ): + try: + effect_result = effect_verifier(context, pending_effect) + if type(effect_result) is not EffectVerificationResult: + raise TypeError( + "EffectVerifier must return exactly " + "EffectVerificationResult." + ) + except Exception as exc: + return self._fail( + f"Effect verifier failed: {type(exc).__name__}: {exc}", + context=context, + ) + try: if self._pending_revision is not None: self._session._install_prepared_revision( @@ -659,9 +691,10 @@ def run_until_blocked( """Run with clock-driven waiting until terminal or effect verification blocks. Args: - effect_verifier: Optional callback used after an - ``effect_verification_required`` event. Without one, the method - returns the running step so the caller can verify externally. + effect_verifier: Optional synchronous callback used on fresh + due-cycle observations while effect verification is pending. + Without one, the method returns the running boundary so the + caller can verify externally. on_step: Optional callback for tracing or tutorial visualization. max_steps: Hard bound on loop iterations. @@ -670,7 +703,6 @@ def run_until_blocked( """ if max_steps <= 0: raise ValueError("max_steps must be greater than zero.") - effect_result: EffectVerificationResult | None = None now = self._clock_now() last_result = self._result( timestamp=now, @@ -681,9 +713,7 @@ def run_until_blocked( if self.effect_verification_pending and effect_verifier is None: return last_result for _ in range(max_steps): - result = self.step(effect_result=effect_result) - if result.tick is not None: - effect_result = None + result = self.step(effect_verifier=effect_verifier) if on_step is not None: try: on_step(result) @@ -700,20 +730,8 @@ def run_until_blocked( verification_required = ( result.tick is not None and result.tick.pending_effect is not None ) - if verification_required: - if effect_verifier is None or result.context is None: - return result - try: - effect_result = effect_verifier(result.context, result.tick) - except Exception as exc: - return self._fail( - f"Effect verifier failed: {type(exc).__name__}: {exc}", - context=result.context, - tick=result.tick, - dispatches=list(result.dispatches), - ) - if effect_result is None: - return result + if verification_required and effect_verifier is None: + return result if result.wait_duration > 0.0: try: self._clock.sleep(result.wait_duration) diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index db9da7b13..4c76f5968 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -36,10 +36,11 @@ AtomicActionEngine, ControlPartCommandProfile, EntityState, + EffectVerificationRequest, + EffectVerificationResult, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, - ExecutionTick, GraspGoal, MotionPolicy, ObjectSemantics, @@ -406,8 +407,8 @@ def on_step(step: RunnerStep) -> None: def verify_pickup_effect( _context: PlanningContext, - _: ExecutionTick, - ) -> torch.Tensor: + request: EffectVerificationRequest, + ) -> EffectVerificationResult: """Verify that the cube rose with, and remains near, the end effector.""" cube_position = target.get_local_pose(to_matrix=True)[:, :3, 3] eef_position = robot.compute_fk( @@ -426,7 +427,12 @@ def verify_pickup_effect( f"cube-to-EEF={held_distance.detach().cpu().tolist()} m, " f"success={success.detach().cpu().tolist()}." ) - return success + verified_success = request.env_mask & success + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=verified_success, + failure_mask=request.env_mask & ~success, + ) recording_started = start_auto_play_recording( sim, diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 141813b98..eb2dcf6cb 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -1406,6 +1406,7 @@ def test_partial_effect_success_commits_resolved_rows_and_shrinks_request() -> N 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.attempt_generation == first_request.attempt_generation assert partial.pending_effect.requested_at == first_request.requested_at assert partial.pending_effect.deadline == first_request.deadline assert not any( @@ -1929,6 +1930,7 @@ def test_effect_retry_invalidates_previous_verification_id() -> None: assert first_wait.pending_effect is not None old_id = first_wait.pending_effect.verification_id old_deadline = first_wait.pending_effect.deadline + old_generation = first_wait.pending_effect.attempt_generation retry = session.tick(_context(0.3, 0.2, 0.2, 0)) assert retry.command is not None @@ -1937,6 +1939,7 @@ def test_effect_retry_invalidates_previous_verification_id() -> None: 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.attempt_generation == old_generation + 1 assert second_wait.pending_effect.deadline > old_deadline with pytest.raises(ValueError, match="verification_id"): @@ -1950,6 +1953,30 @@ def test_effect_retry_invalidates_previous_verification_id() -> None: ) +def test_effect_request_generation_advances_after_tracking_replan() -> None: + engine, _ = _engine() + effect = EffectAction() + engine.register(effect) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=effect.skill_id, + 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)) + + replanned = session.tick(_context(0.1, 1.0, 0.2, 0)) + session.tick(_context(0.2, 1.0, 0.2, 0)) + waiting = session.tick(_context(0.3, 0.2, 0.2, 0)) + + assert any(event.kind is ExecutionEventKind.REPLANNED for event in replanned.events) + assert waiting.pending_effect is not None + assert waiting.pending_effect.attempt_generation == 1 + + 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 ed9432387..58dda7380 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -37,11 +37,11 @@ CommandAckStatus, CommandOperation, EndEffectorPoseGoal, + EffectVerificationRequest, EffectVerificationResult, ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, - ExecutionTick, HeldObjectState, JOINT_POSITION_CAPABILITY, JointPositionPayload, @@ -323,13 +323,11 @@ def _make_runner( def _successful_effect_result( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> 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, + verification_id=request.verification_id, success_mask=torch.ones( context.batch_size, dtype=torch.bool, @@ -683,10 +681,10 @@ def test_resumed_effect_verifier_uses_a_fresh_observation() -> None: def record_fresh_context( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: observed_at.append(context.robot.timestamp) - return _successful_effect_result(context, tick) + return _successful_effect_result(context, request) completed = runner.run_until_blocked(effect_verifier=record_fresh_context) @@ -695,32 +693,191 @@ def record_fresh_context( assert observed_at[0] > blocked_at +def test_due_effect_verifier_consumes_fresh_observation_in_the_same_step() -> None: + runner, clock, _, _, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.context is not None + assert blocked.tick is not None and blocked.tick.pending_effect is not None + blocked_at = blocked.context.robot.timestamp + clock.advance(MINIMUM_CYCLE_TIME) + observed_at: list[float] = [] + + def verify_fresh_observation( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_at.append(context.robot.timestamp) + return _successful_effect_result(context, request) + + completed = runner.step(effect_verifier=verify_fresh_observation) + + assert completed.status is RunnerStatus.COMPLETED + assert completed.tick is not None and completed.tick.pending_effect is None + assert completed.tick.task_state.get_held_object("arm") is not None + assert completed.context is not None + assert observed_at == [completed.context.robot.timestamp] + assert observed_at[0] > blocked_at + + +def test_effect_verifier_runs_and_succeeds_at_the_request_deadline() -> None: + runner, clock, _, _, _ = _make_runner( + with_effect=True, + 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()) + observed_at: list[float] = [] + + def verify_at_deadline( + context: PlanningContext, + current_request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_at.append(context.robot.timestamp) + return _successful_effect_result(context, current_request) + + completed = runner.step(effect_verifier=verify_at_deadline) + + assert completed.status is RunnerStatus.COMPLETED + assert observed_at == pytest.approx([request.deadline]) + + +def test_effect_verifier_is_not_called_after_deadline_and_session_retries() -> 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() + MINIMUM_CYCLE_TIME) + verifier = Mock() + + retry = runner.step(effect_verifier=verifier) + + verifier.assert_not_called() + assert retry.status is RunnerStatus.RUNNING + assert retry.tick is not None and retry.tick.command is not None + assert action.plan_count == plan_count + 1 + assert { + ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT, + ExecutionEventKind.ACTION_RETRY, + ExecutionEventKind.REPLANNED, + }.issubset({event.kind for event in retry.tick.events}) + + +def test_effect_result_and_effect_verifier_are_mutually_exclusive() -> None: + runner, _, _, sink, action = _make_runner(with_effect=True) + result = EffectVerificationResult( + verification_id=0, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ) + + with pytest.raises(ValueError, match="mutually exclusive"): + runner.step( + effect_result=result, + effect_verifier=_successful_effect_result, + ) + + assert action.plan_count == 1 + assert sink.sent == [] + + +@pytest.mark.parametrize( + "invalid_result", + [None, True], + ids=["none", "wrong-type"], +) +def test_effect_verifier_invalid_result_fails_with_cancel_then_hold( + invalid_result: object | None, +) -> None: + runner, clock, _, sink, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + clock.advance(MINIMUM_CYCLE_TIME) + + def invalid_verifier( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> object | None: + del context, request + return invalid_result + + failed = runner.step(effect_verifier=invalid_verifier) + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert failed.message is not None + assert "must return exactly EffectVerificationResult" in failed.message + + +def test_all_false_effect_updates_keep_polling_the_same_request() -> None: + runner, clock, _, sink, _ = _make_runner(with_effect=True) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + initial_request = blocked.tick.pending_effect + observed_requests: list[tuple[int, int]] = [] + + def report_no_progress( + context: PlanningContext, + request: EffectVerificationRequest, + ) -> EffectVerificationResult: + observed_requests.append((request.verification_id, request.attempt_generation)) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=torch.zeros(context.batch_size, dtype=torch.bool), + failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), + ) + + clock.advance(MINIMUM_CYCLE_TIME) + first_poll = runner.step(effect_verifier=report_no_progress) + clock.advance(MINIMUM_CYCLE_TIME) + second_poll = runner.step(effect_verifier=report_no_progress) + + assert first_poll.status is RunnerStatus.RUNNING + assert second_poll.status is RunnerStatus.RUNNING + assert first_poll.tick is not None and first_poll.tick.pending_effect is not None + assert second_poll.tick is not None and second_poll.tick.pending_effect is not None + assert observed_requests == [ + (initial_request.verification_id, initial_request.attempt_generation), + (initial_request.verification_id, initial_request.attempt_generation), + ] + assert sink.cancel_count == 0 + assert second_poll.tick.task_state.get_held_object("arm") is None + + 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, + request: EffectVerificationRequest, ) -> 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]: + if request.env_mask.tolist() == [True, True]: return EffectVerificationResult( - verification_id=pending_effect.verification_id, + verification_id=request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), ) - assert pending_effect.env_mask.tolist() == [False, True] + assert request.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, + verification_id=request.verification_id, success_mask=torch.tensor([False, True]), failure_mask=torch.tensor([False, False]), ) @@ -786,6 +943,7 @@ def test_runner_deactivation_refreshes_cached_effect_request() -> None: 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 + old_generation = blocked.tick.pending_effect.attempt_generation changed = runner.deactivate_rows( torch.tensor([False, True]), @@ -797,15 +955,14 @@ def test_runner_deactivation_refreshes_cached_effect_request() -> None: 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 + assert refreshed.tick.pending_effect.attempt_generation == old_generation def verify_remaining( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: - pending_effect = tick.pending_effect - assert pending_effect is not None return EffectVerificationResult( - verification_id=pending_effect.verification_id, + verification_id=request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), ) @@ -822,12 +979,10 @@ def test_blocking_runner_fails_safely_for_a_mismatched_effect_result() -> None: def mismatched_effect_result( context: PlanningContext, - tick: ExecutionTick, + request: EffectVerificationRequest, ) -> EffectVerificationResult: - pending_effect = tick.pending_effect - assert pending_effect is not None return EffectVerificationResult( - verification_id=pending_effect.verification_id + 1, + verification_id=request.verification_id + 1, success_mask=torch.ones(context.batch_size, dtype=torch.bool), failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), ) From 58bce6d987d7a975286336f8955fd70677cc56d2 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 13:17:03 +0800 Subject: [PATCH 3/3] feat(atomic-actions): complete verified action runtime --- .../lab/sim/atomic_actions/__init__.py | 27 +- .../lab/sim/atomic_actions/affordance.py | 303 ++++++++ .../lab/sim/atomic_actions/bindings.py | 13 + embodichain/lab/sim/atomic_actions/core.py | 46 ++ embodichain/lab/sim/atomic_actions/effects.py | 122 ++- embodichain/lab/sim/atomic_actions/engine.py | 12 +- .../lab/sim/atomic_actions/execution.py | 338 ++++++++- embodichain/lab/sim/atomic_actions/goals.py | 122 +++ .../lab/sim/atomic_actions/invocation.py | 13 + embodichain/lab/sim/atomic_actions/plans.py | 145 +++- .../sim/atomic_actions/primitives/__init__.py | 9 + .../sim/atomic_actions/primitives/_helpers.py | 38 +- .../primitives/coordinated_pickment.py | 28 +- .../primitives/coordinated_placement.py | 49 +- .../atomic_actions/primitives/hand_over.py | 36 +- .../primitives/move_held_object.py | 17 +- .../primitives/operate_articulation.py | 468 ++++++++++++ .../sim/atomic_actions/primitives/pick_up.py | 16 +- .../sim/atomic_actions/primitives/place.py | 36 +- embodichain/lab/sim/atomic_actions/runtime.py | 65 ++ embodichain/lab/sim/atomic_actions/state.py | 312 +++++++- embodichain/lab/sim/objects/articulation.py | 9 + tests/sim/atomic_actions/test_actions.py | 699 ++++++++++++++++-- .../test_articulation_effects.py | 120 +++ tests/sim/atomic_actions/test_control.py | 58 ++ tests/sim/atomic_actions/test_core.py | 175 ++++- tests/sim/atomic_actions/test_engine.py | 5 +- .../sim/atomic_actions/test_engine_per_env.py | 495 ++++++++++++- tests/sim/objects/test_articulation.py | 14 +- tests/sim/objects/test_robot.py | 20 +- 30 files changed, 3654 insertions(+), 156 deletions(-) create mode 100644 embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py create mode 100644 tests/sim/atomic_actions/test_articulation_effects.py diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 6eec6bdac..fd1e0429c 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -31,6 +31,8 @@ from .affordance import ( Affordance, AntipodalAffordance, + ArticulationOperationAffordance, + ArticulationOperationTarget, AssembleAffordance, InteractionPoints, ) @@ -61,15 +63,23 @@ EffectVerificationResult, ExecutionEvent, ExecutionEventKind, + ExecutionPlanAttempt, ExecutionSession, ExecutionStatus, ExecutionTick, ) -from .goals import ActionGoal, ObjectActionGoal, PoseGoalValue, SceneEntityPose +from .goals import ( + ActionGoal, + ObjectActionGoal, + PoseGoalValue, + SceneArticulationOperationGeometry, + SceneEntityPose, +) from .invocation import ActionInvocation, ActionOptions, ResolvedActionRequest from .plans import ( ActionPlan, CompiledTrajectory, + EffectVerificationRequirement, ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, @@ -119,6 +129,9 @@ MoveHeldObjectOptions, MoveJoints, MoveJointsOptions, + OperateArticulation, + OperateArticulationGoal, + OperateArticulationOptions, PickUp, PickUpOptions, Place, @@ -152,9 +165,11 @@ SimulationExecutionAdapter, ) from .state import ( + ArticulationJointState, CoordinatedHeldObjectState, EntityState, HeldObjectState, + ObservedArticulationJointState, PlanningContext, RobotObservation, SceneSnapshot, @@ -171,6 +186,9 @@ "ActionPlanningServices", "Affordance", "AntipodalAffordance", + "ArticulationOperationAffordance", + "ArticulationOperationTarget", + "ArticulationJointState", "AssembleAffordance", "AssembleGoal", "AtomicAction", @@ -203,12 +221,14 @@ "EndpointCommandTransport", "EntityState", "EffectVerificationRequest", + "EffectVerificationRequirement", "EffectVerificationResult", "EffectVerifier", "ExecutionClock", "ExecutionFeedbackMode", "ExecutionEvent", "ExecutionEventKind", + "ExecutionPlanAttempt", "ExecutionRunner", "ExecutionRunnerCfg", "ExecutionSession", @@ -241,6 +261,10 @@ "ObjectSemantics", "OPEN_COMMAND", "ObservationProvider", + "ObservedArticulationJointState", + "OperateArticulation", + "OperateArticulationGoal", + "OperateArticulationOptions", "PickUp", "PickUpOptions", "Place", @@ -264,6 +288,7 @@ "RunnerStep", "RunnerStepCallback", "SceneProvider", + "SceneArticulationOperationGeometry", "SceneSnapshot", "SceneSnapshotSupplier", "SceneEntityPose", diff --git a/embodichain/lab/sim/atomic_actions/affordance.py b/embodichain/lab/sim/atomic_actions/affordance.py index dbe1ffea7..1a5e6c452 100644 --- a/embodichain/lab/sim/atomic_actions/affordance.py +++ b/embodichain/lab/sim/atomic_actions/affordance.py @@ -17,7 +17,11 @@ from __future__ import annotations import torch +from collections.abc import Mapping +from copy import deepcopy from dataclasses import dataclass, field +import math +from types import MappingProxyType from typing import Any, TYPE_CHECKING from embodichain.toolkits.graspkit.pg_grasp import ( @@ -236,6 +240,303 @@ def get_approach_direction(self, point_idx: int) -> torch.Tensor: ) +def _owned_se3_offset(value: torch.Tensor, *, field_name: str) -> torch.Tensor: + """Validate and own one affordance-local homogeneous transform.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.shape != (4, 4): + raise ValueError(f"{field_name} must have shape (4, 4).") + if not value.is_floating_point(): + raise TypeError(f"{field_name} must use a floating-point dtype.") + if not torch.isfinite(value).all(): + raise ValueError(f"{field_name} must contain only finite values.") + checked = value.to(dtype=torch.float64) + bottom = checked.new_tensor((0.0, 0.0, 0.0, 1.0)) + if not torch.allclose(checked[3], bottom, atol=1.0e-6, rtol=0.0): + raise ValueError(f"{field_name} must have bottom row [0, 0, 0, 1].") + rotation = checked[:3, :3] + if not torch.allclose( + rotation.T @ rotation, + torch.eye(3, dtype=checked.dtype, device=checked.device), + atol=1.0e-6, + rtol=0.0, + ) or not torch.isclose( + torch.linalg.det(rotation), + checked.new_tensor(1.0), + atol=1.0e-6, + rtol=0.0, + ): + raise ValueError(f"{field_name} must contain a proper SE(3) rotation.") + return value.clone() + + +def _finite_scalar(value: float, *, field_name: str) -> float: + """Return one finite non-boolean scalar as a float.""" + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise TypeError(f"{field_name} must be a finite scalar.") + normalized = float(value) + if not math.isfinite(normalized): + raise ValueError(f"{field_name} must be finite.") + return normalized + + +@dataclass(frozen=True, slots=True) +class ArticulationOperationTarget: + """Named joint target and handle-relative operation displacement. + + ``displacement`` is deliberately explicit: it is the full signed handle + stroke from the live source joint position captured during semantic + grounding to ``target_position``. Recovery replans scale this stroke by + the remaining live joint progress. + """ + + target_position: float + """Absolute desired articulation joint position.""" + + displacement: float + """Signed operation displacement from the currently observed handle pose.""" + + def __post_init__(self) -> None: + object.__setattr__( + self, + "target_position", + _finite_scalar( + self.target_position, + field_name="ArticulationOperationTarget.target_position", + ), + ) + object.__setattr__( + self, + "displacement", + _finite_scalar( + self.displacement, + field_name="ArticulationOperationTarget.displacement", + ), + ) + + def snapshot(self) -> ArticulationOperationTarget: + """Return an independently constructed immutable target.""" + return ArticulationOperationTarget(self.target_position, self.displacement) + + +@dataclass(eq=False) +class ArticulationOperationAffordance(Affordance): + """Declarative handle geometry for one articulated joint operation. + + The four offsets are expressed in the live handle frame. During semantic + grounding the approach and contact poses are ``handle @ offset``. The + operation and retract poses additionally insert a local translation of + ``operation_axis * displacement * position_scale`` before their offsets. + This keeps task code free of pose-matrix construction; the semantic + compiler copies the geometry into a late-bound atomic goal. + """ + + joint_id: str = "" + """Canonical joint identifier written to the atomic goal and effect.""" + + approach_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + contact_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + operation_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + retract_offset: torch.Tensor = field( + default_factory=lambda: torch.eye(4, dtype=torch.float32) + ) + operation_axis: torch.Tensor = field( + default_factory=lambda: torch.tensor((1.0, 0.0, 0.0), dtype=torch.float32) + ) + """Unit operation direction expressed in the observed handle frame.""" + + position_scale: float = 1.0 + """Positive conversion from declared displacement units to pose metres.""" + + semantic_targets: Mapping[str, ArticulationOperationTarget] = field( + default_factory=dict + ) + """Optional stable target names mapped to position/displacement pairs.""" + + def __post_init__(self) -> None: + if ( + type(self.joint_id) is not str + or not self.joint_id + or self.joint_id != self.joint_id.strip() + ): + raise ValueError( + "ArticulationOperationAffordance.joint_id must be a non-empty " + "canonical identifier." + ) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ): + setattr( + self, + field_name, + _owned_se3_offset( + getattr(self, field_name), + field_name=f"ArticulationOperationAffordance.{field_name}", + ), + ) + axis = self.operation_axis + if not isinstance(axis, torch.Tensor): + raise TypeError( + "ArticulationOperationAffordance.operation_axis must be a tensor." + ) + if axis.shape != (3,) or not axis.is_floating_point(): + raise ValueError( + "ArticulationOperationAffordance.operation_axis must be a " + "floating tensor with shape (3,)." + ) + if not torch.isfinite(axis).all(): + raise ValueError( + "ArticulationOperationAffordance.operation_axis must be finite." + ) + norm = torch.linalg.vector_norm(axis) + if float(norm) <= torch.finfo(axis.dtype).eps: + raise ValueError( + "ArticulationOperationAffordance.operation_axis must be non-zero." + ) + self.operation_axis = (axis / norm).clone() + self.position_scale = _finite_scalar( + self.position_scale, + field_name="ArticulationOperationAffordance.position_scale", + ) + if self.position_scale <= 0.0: + raise ValueError( + "ArticulationOperationAffordance.position_scale must be positive." + ) + if not isinstance(self.semantic_targets, Mapping): + raise TypeError( + "ArticulationOperationAffordance.semantic_targets must be a mapping." + ) + targets: dict[str, ArticulationOperationTarget] = {} + for target_id, target in self.semantic_targets.items(): + if ( + type(target_id) is not str + or not target_id + or target_id != target_id.strip() + ): + raise ValueError( + "Articulation operation target IDs must be non-empty canonical " + "identifiers." + ) + if type(target) is not ArticulationOperationTarget: + raise TypeError( + "semantic_targets values must be exact " + "ArticulationOperationTarget values." + ) + targets[target_id] = target.snapshot() + self.semantic_targets = MappingProxyType(targets) + + def resolve_target(self, target_id: str) -> ArticulationOperationTarget: + """Return an owned named target or raise with deterministic candidates.""" + if type(target_id) is not str or not target_id: + raise ValueError("target_id must be a non-empty string.") + try: + target = self.semantic_targets[target_id] + except KeyError as exc: + raise KeyError( + f"Unknown articulation target {target_id!r}; available targets are " + f"{sorted(self.semantic_targets)}." + ) from exc + return target.snapshot() + + def __deepcopy__(self, memo: dict[int, object]) -> ArticulationOperationAffordance: + """Copy immutable configuration despite ``MappingProxyType`` storage.""" + existing = memo.get(id(self)) + if existing is not None: + assert isinstance(existing, ArticulationOperationAffordance) + return existing + copied = ArticulationOperationAffordance( + object_label=self.object_label, + custom_config=deepcopy(self.custom_config, memo), + joint_id=self.joint_id, + approach_offset=self.approach_offset, + contact_offset=self.contact_offset, + operation_offset=self.operation_offset, + retract_offset=self.retract_offset, + operation_axis=self.operation_axis, + position_scale=self.position_scale, + semantic_targets={ + target_id: target.snapshot() + for target_id, target in self.semantic_targets.items() + }, + ) + memo[id(self)] = copied + return copied + + def ground_poses( + self, + handle_pose: torch.Tensor, + *, + displacement: float, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Ground four end-effector poses from a fresh handle observation. + + Args: + handle_pose: Live handle pose with shape ``(4, 4)`` or ``(B, 4, 4)``. + displacement: Signed displacement from this observed handle pose. + + Returns: + Approach, contact, operation, and retract pose batches. + """ + if not isinstance(handle_pose, torch.Tensor): + raise TypeError("handle_pose must be a torch.Tensor.") + if handle_pose.shape == (4, 4): + handles = handle_pose.unsqueeze(0) + elif ( + handle_pose.dim() == 3 + and handle_pose.shape[0] > 0 + and handle_pose.shape[-2:] == (4, 4) + ): + handles = handle_pose + else: + raise ValueError("handle_pose must have shape (4, 4) or (B, 4, 4).") + if not handle_pose.is_floating_point() or not torch.isfinite(handle_pose).all(): + raise ValueError("handle_pose must be a finite floating tensor.") + displacement = _finite_scalar(displacement, field_name="displacement") + offsets = tuple( + getattr(self, field_name) + .to( + device=handles.device, + dtype=handles.dtype, + ) + .unsqueeze(0) + .expand(handles.shape[0], -1, -1) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ) + ) + translation = ( + torch.eye( + 4, + dtype=handles.dtype, + device=handles.device, + ) + .unsqueeze(0) + .repeat(handles.shape[0], 1, 1) + ) + translation[:, :3, 3] = self.operation_axis.to( + device=handles.device, + dtype=handles.dtype, + ) * (displacement * self.position_scale) + approach = torch.bmm(handles, offsets[0]) + contact = torch.bmm(handles, offsets[1]) + moved_handle = torch.bmm(handles, translation) + operation = torch.bmm(moved_handle, offsets[2]) + retract = torch.bmm(moved_handle, offsets[3]) + return tuple(pose.clone() for pose in (approach, contact, operation, retract)) + + @dataclass class AssembleAffordance(Affordance): """Affordance describing how an assemble object fits onto a base object. @@ -316,6 +617,8 @@ def get_assemble_object_pose(self, base_pose: torch.Tensor) -> torch.Tensor: __all__ = [ "Affordance", "AntipodalAffordance", + "ArticulationOperationAffordance", + "ArticulationOperationTarget", "InteractionPoints", "AssembleAffordance", ] diff --git a/embodichain/lab/sim/atomic_actions/bindings.py b/embodichain/lab/sim/atomic_actions/bindings.py index d56713580..3043c5b5c 100644 --- a/embodichain/lab/sim/atomic_actions/bindings.py +++ b/embodichain/lab/sim/atomic_actions/bindings.py @@ -189,6 +189,9 @@ class EndpointBinding: resource_id: str adapter_id: str target: RuntimeEndpointTarget + task_state_key: str | None = None + """Symbolic task-state key; direct-core defaults to ``target.target_id``.""" + capabilities: frozenset[str] = frozenset() commands: Mapping[str, ControlCommand] = field(default_factory=dict) claim_tokens: frozenset[str] = frozenset() @@ -235,6 +238,14 @@ def __post_init__(self) -> None: "fingerprint." ) object.__setattr__(self, "target", target) + task_state_key = ( + target.target_id if self.task_state_key is None else self.task_state_key + ) + _validate_identifier( + task_state_key, + field_name="EndpointBinding.task_state_key", + ) + object.__setattr__(self, "task_state_key", task_state_key) object.__setattr__( self, "capabilities", @@ -344,6 +355,7 @@ def with_commands( resource_id=self.resource_id, adapter_id=self.adapter_id, target=self.target, + task_state_key=self.task_state_key, capabilities=self.capabilities, commands=merged, claim_tokens=self.claim_tokens, @@ -358,6 +370,7 @@ def snapshot(self) -> EndpointBinding: resource_id=self.resource_id, adapter_id=self.adapter_id, target=self.target, + task_state_key=self.task_state_key, capabilities=self.capabilities, commands=self.commands, claim_tokens=self.claim_tokens, diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index 707ed9233..a3d37d55e 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -41,6 +41,7 @@ ) from .plans import ( ActionPlan, + EffectVerificationRequirement, ExecutionFeedbackMode, PlannerDiagnostics, TimedTrajectory, @@ -502,9 +503,11 @@ def build_plan( success: bool | torch.Tensor, trajectory: TimedTrajectory | torch.Tensor, expected_effects: StateDelta | None = None, + effect_verification: EffectVerificationRequirement | None = None, replannable: bool = True, diagnostics: PlannerDiagnostics | None = None, segment_lengths: Mapping[str, int] | None = None, + scene_dependency_monitor_until: Mapping[str, int] | None = None, ) -> ActionPlan: """Build a validated action plan for a primitive implementation. @@ -514,10 +517,19 @@ def build_plan( success: Per-environment planning success or scalar planner result. trajectory: Full-robot timed trajectory or position tensor. expected_effects: Symbolic effects to verify after execution. + effect_verification: Optional explicit physical-effect boundary. + Use this when verification is required without a symbolic task- + state delta. replannable: Whether the execution runtime may replan this action. diagnostics: Optional retained planner diagnostics. segment_lengths: Optional ordered mapping from semantic segment names to waypoint counts. Zero-length entries are omitted. + scene_dependency_monitor_until: Optional per-entity exclusive + waypoint-index upper bound for scene-motion invalidation. An + entity is monitored while the current waypoint index is smaller + than its bound. ``0`` disables monitoring immediately; omitted + dependencies remain monitored for the full action. Once the bound + is reached, all pose changes for that entity are ignored. Returns: Side-effect-free action plan. @@ -557,9 +569,11 @@ def build_plan( success=success_mask, commands=commands, expected_effects=expected_effects, + effect_verification=effect_verification, replannable=replannable, diagnostics=diagnostics, segment_lengths=segment_lengths, + scene_dependency_monitor_until=scene_dependency_monitor_until, feedback_mode=ExecutionFeedbackMode.JOINT_POSITION, joint_trajectory=timed, ) @@ -572,9 +586,11 @@ def build_command_plan( success: bool | torch.Tensor, commands: TimedCommandSequence, expected_effects: StateDelta | None = None, + effect_verification: EffectVerificationRequirement | None = None, replannable: bool = True, diagnostics: PlannerDiagnostics | None = None, segment_lengths: Mapping[str, int] | None = None, + scene_dependency_monitor_until: Mapping[str, int] | None = None, feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED, joint_trajectory: TimedTrajectory | None = None, ) -> ActionPlan: @@ -583,6 +599,30 @@ def build_command_plan( Non-joint command sequences use timed completion unless a future endpoint-specific feedback evaluator is installed. Semantic effects remain externally verified through the execution session. + + Args: + request: Resolved invocation snapshot being planned. + context: Planning input used for the plan. + success: Per-environment planning success or scalar planner result. + commands: Transport-neutral command sequence for the action. + expected_effects: Symbolic effects to verify after execution. + effect_verification: Optional explicit physical-effect boundary. + replannable: Whether the execution runtime may replan this action. + diagnostics: Optional retained planner diagnostics. + segment_lengths: Optional ordered mapping from semantic segment names + to command-frame counts. Zero-length entries are omitted. + scene_dependency_monitor_until: Optional per-entity exclusive + command-frame-index upper bound for scene-motion invalidation. An + entity is monitored while the current frame index is smaller than + its bound. ``0`` disables monitoring immediately; omitted + dependencies remain monitored for the full action. Once the bound + is reached, all pose changes for that entity are ignored. + feedback_mode: Feedback contract used to determine target completion. + joint_trajectory: Optional joint trajectory retained for joint-position + feedback and inspection. + + Returns: + Side-effect-free action plan. """ self.require_goal(request) if not isinstance(commands, TimedCommandSequence): @@ -629,9 +669,15 @@ def build_command_plan( joint_trajectory=joint_trajectory, segments=segments, scene_dependencies=self._scene_dependencies(request), + scene_dependency_monitor_until=( + {} + if scene_dependency_monitor_until is None + else scene_dependency_monitor_until + ), collision_world_sensitive=self._uses_collision_world(request, context), replannable=replannable, expected_effects=expected_effects or StateDelta(), + effect_verification=effect_verification, invocation_id=request.invocation_id, invocation_revision=request.revision, ) diff --git a/embodichain/lab/sim/atomic_actions/effects.py b/embodichain/lab/sim/atomic_actions/effects.py index 90b80bfa5..eb33be348 100644 --- a/embodichain/lab/sim/atomic_actions/effects.py +++ b/embodichain/lab/sim/atomic_actions/effects.py @@ -29,9 +29,11 @@ from embodichain.lab.sim.common import BatchEntity from .state import ( + ArticulationJointState, CoordinatedHeldObjectState, HeldObjectState, TaskState, + _normalize_articulation_joint, _normalize_coordinated_held, _normalize_held, _normalize_mask, @@ -118,6 +120,16 @@ def _snapshot_coordinated( ) +def _snapshot_articulation_joint( + value: ArticulationJointState, +) -> ArticulationJointState: + """Return an independently owned articulation-joint effect value.""" + return ArticulationJointState( + position=value.position.clone(), + env_mask=None if value.env_mask is None else value.env_mask.clone(), + ) + + def _with_held_mask( value: HeldObjectState, env_mask: torch.Tensor, @@ -146,6 +158,14 @@ def _with_coordinated_mask( ) +def _with_articulation_joint_mask( + value: ArticulationJointState, + env_mask: torch.Tensor, +) -> ArticulationJointState: + """Copy an articulation-joint state with a replacement mask.""" + return ArticulationJointState(position=value.position, env_mask=env_mask) + + def _merge_held( previous: HeldObjectState | None, candidate: HeldObjectState | None, @@ -244,6 +264,48 @@ def _merge_coordinated( ) +def _merge_articulation_joint( + previous: ArticulationJointState | None, + candidate: ArticulationJointState | None, + update_mask: torch.Tensor, +) -> ArticulationJointState | None: + """Apply one optional articulation-joint update per environment.""" + if previous is None and candidate is None: + return None + if previous is None: + assert candidate is not None and candidate.env_mask is not None + env_mask = candidate.env_mask & update_mask + return ( + _with_articulation_joint_mask(candidate, env_mask) + if env_mask.any() + else None + ) + assert previous.env_mask is not None + if candidate is None: + env_mask = previous.env_mask & ~update_mask + return ( + _with_articulation_joint_mask(previous, env_mask) + if env_mask.any() + else None + ) + assert candidate.env_mask is not None + if candidate.position.shape != previous.position.shape: + raise ValueError( + "Cannot merge articulation-joint states with different joint widths." + ) + env_mask = torch.where(update_mask, candidate.env_mask, previous.env_mask) + if not env_mask.any(): + return None + return ArticulationJointState( + position=torch.where( + update_mask[:, None], + candidate.position, + previous.position, + ), + env_mask=env_mask, + ) + + @dataclass(frozen=True, slots=True, eq=False) class StateDelta: """Expected task-state changes that require post-execution verification. @@ -263,9 +325,15 @@ class StateDelta: ] = field(default_factory=dict) """Per-resource-pair coordinated attachment replacements or removals.""" + articulation_joint_updates: Mapping[ + tuple[str, str], ArticulationJointState | None + ] = field(default_factory=dict) + """Per-articulation/joint verified state replacements or removals.""" + def __post_init__(self) -> None: held = dict(self.held_object_updates) coordinated = dict(self.coordinated_held_object_updates) + articulation = dict(self.articulation_joint_updates) for resource, value in held.items(): if not isinstance(resource, str) or not resource: raise ValueError( @@ -289,17 +357,43 @@ def __post_init__(self) -> None: "coordinated_held_object_updates values must be " "CoordinatedHeldObjectState or None." ) + for key, value in articulation.items(): + if ( + not isinstance(key, tuple) + or len(key) != 2 + or not all( + type(item) is str and item and item == item.strip() for item in key + ) + ): + raise ValueError( + "articulation_joint_updates keys must be canonical " + "articulation/joint pairs." + ) + if value is not None and not isinstance(value, ArticulationJointState): + raise TypeError( + "articulation_joint_updates values must be " + "ArticulationJointState or None." + ) object.__setattr__(self, "held_object_updates", MappingProxyType(held)) object.__setattr__( self, "coordinated_held_object_updates", MappingProxyType(coordinated), ) + object.__setattr__( + self, + "articulation_joint_updates", + MappingProxyType(articulation), + ) @property 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 + return ( + not self.held_object_updates + and not self.coordinated_held_object_updates + and not self.articulation_joint_updates + ) def snapshot(self) -> StateDelta: """Return an independently owned symbolic-effect snapshot. @@ -319,6 +413,10 @@ def snapshot(self) -> StateDelta: resources: (None if value is None else _snapshot_coordinated(value)) for resources, value in self.coordinated_held_object_updates.items() }, + articulation_joint_updates={ + key: (None if value is None else _snapshot_articulation_joint(value)) + for key, value in self.articulation_joint_updates.items() + }, ) def apply( @@ -381,11 +479,33 @@ def apply( else: coordinated[resources] = merged + articulation = dict(state.articulation_joints) + for key, candidate in self.articulation_joint_updates.items(): + normalized = ( + None + if candidate is None + else _normalize_articulation_joint( + candidate, + batch_size=state.batch_size, + device=state.device, + ) + ) + merged = _merge_articulation_joint( + articulation.get(key), + normalized, + mask, + ) + if merged is None: + articulation.pop(key, None) + else: + articulation[key] = merged + return TaskState( batch_size=state.batch_size, device=state.device, held_objects=held, coordinated_held_objects=coordinated, + articulation_joints=articulation, ) diff --git a/embodichain/lab/sim/atomic_actions/engine.py b/embodichain/lab/sim/atomic_actions/engine.py index 2c7d446df..ff1d5eb30 100644 --- a/embodichain/lab/sim/atomic_actions/engine.py +++ b/embodichain/lab/sim/atomic_actions/engine.py @@ -248,6 +248,8 @@ def bind_control_parts( self, skill: str | AtomicAction, endpoints: Mapping[str, Mapping[str, str]], + *, + task_state_keys: Mapping[str, str] | None = None, ) -> ActionBinding: """Build an advanced direct-core binding from control-part names. @@ -255,6 +257,10 @@ def bind_control_parts( skill: Installed skill ID or an explicit action passed later to :meth:`plan_action`. endpoints: Nested ``slot_id -> endpoint_id -> control_part`` mapping. + task_state_keys: Optional explicit stable task-state key for each + resource slot. See + :meth:`ActionPlanningServices.bind_control_parts` for inference + rules when omitted. Returns: Engine-owned generic endpoint binding. @@ -279,7 +285,11 @@ def bind_control_parts( raise ValueError( f"Skill {action.skill_id!r} has no explicit SkillBindingContract." ) - return self._planning_services.bind_control_parts(contract, endpoints) + return self._planning_services.bind_control_parts( + contract, + endpoints, + task_state_keys=task_state_keys, + ) def register(self, action: AtomicAction, *, replace: bool = False) -> None: """Register one action instance using its descriptor. diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index ba21c669c..c30691c28 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -30,9 +30,11 @@ from .bindings import JointPositionTarget, RuntimeEndpointTarget from .plans import ( ActionPlan, + EffectVerificationRequirement, ExecutionFeedbackMode, TrajectorySegment, ) +from .policies import RecoveryPolicy from .runtime_commands import ( JointPositionPayload, RuntimeCommandFrame, @@ -102,9 +104,118 @@ def __post_init__(self) -> None: object.__setattr__(self, "env_mask", self.env_mask.clone()) +@dataclass(frozen=True, slots=True, eq=False) +class ExecutionPlanAttempt: + """Owned inspection snapshot for one installed action plan. + + Recovery can install several plans for one logical invocation. This value + preserves the exact scene/collision revisions and trajectory structure of + every installation, correlated with the session-local attempt generation + and row-local recovery counters. + """ + + attempt_generation: int + event_kind: ExecutionEventKind + planned_at: float + invocation_index: int + planned_mask: torch.Tensor + action_retry_counts: tuple[int, ...] + replan_counts: tuple[int, ...] + request: ResolvedActionRequest + plan: ActionPlan + + def __post_init__(self) -> None: + if type(self.attempt_generation) is not int or self.attempt_generation < 0: + raise ValueError("attempt_generation must be a non-negative integer.") + if self.event_kind not in { + ExecutionEventKind.ACTION_PLANNED, + ExecutionEventKind.INVOCATION_REVISED, + ExecutionEventKind.REPLANNED, + }: + raise ValueError("event_kind must describe an installed action plan.") + if not math.isfinite(self.planned_at) or self.planned_at < 0.0: + raise ValueError("planned_at must be finite and non-negative.") + if type(self.invocation_index) is not int or self.invocation_index < 0: + raise ValueError("invocation_index must be a non-negative integer.") + if ( + not isinstance(self.planned_mask, torch.Tensor) + or self.planned_mask.dtype != torch.bool + or self.planned_mask.dim() != 1 + ): + raise ValueError("planned_mask must be a one-dimensional bool tensor.") + retries = tuple(self.action_retry_counts) + replans = tuple(self.replan_counts) + batch_size = int(self.planned_mask.numel()) + if len(retries) != batch_size or len(replans) != batch_size: + raise ValueError("Recovery counters must contain one value per row.") + if any(type(value) is not int or value < 0 for value in (*retries, *replans)): + raise ValueError("Recovery counters must be non-negative integers.") + if not isinstance(self.request, ResolvedActionRequest): + raise TypeError("request must be a ResolvedActionRequest.") + if not isinstance(self.plan, ActionPlan): + raise TypeError("plan must be an ActionPlan.") + if ( + self.request.skill_id != self.plan.skill_id + or self.request.invocation_id != self.plan.invocation_id + or self.request.revision != self.plan.invocation_revision + ): + raise ValueError("request identity must match the installed plan.") + if self.plan.plan_success.shape != self.planned_mask.shape: + raise ValueError("plan and planned_mask batch shapes must match.") + if self.plan.plan_success.device != self.planned_mask.device: + raise ValueError("plan and planned_mask must share a device.") + object.__setattr__(self, "planned_mask", self.planned_mask.clone()) + object.__setattr__(self, "action_retry_counts", retries) + object.__setattr__(self, "replan_counts", replans) + object.__setattr__(self, "request", self.request.snapshot()) + object.__setattr__(self, "plan", self.plan.snapshot()) + + def snapshot(self) -> ExecutionPlanAttempt: + """Return an independently owned plan-attempt trace.""" + return ExecutionPlanAttempt( + attempt_generation=self.attempt_generation, + event_kind=self.event_kind, + planned_at=self.planned_at, + invocation_index=self.invocation_index, + planned_mask=self.planned_mask, + action_retry_counts=self.action_retry_counts, + replan_counts=self.replan_counts, + request=self.request, + plan=self.plan, + ) + + +@dataclass(frozen=True, slots=True) +class _ExecutionPlanAttemptRecord: + """Session-private plan reference converted to an owned public snapshot.""" + + attempt_generation: int + event_kind: ExecutionEventKind + planned_at: float + invocation_index: int + planned_mask: torch.Tensor + action_retry_counts: tuple[int, ...] + replan_counts: tuple[int, ...] + request: ResolvedActionRequest + plan: ActionPlan + + def snapshot(self) -> ExecutionPlanAttempt: + return ExecutionPlanAttempt( + attempt_generation=self.attempt_generation, + event_kind=self.event_kind, + planned_at=self.planned_at, + invocation_index=self.invocation_index, + planned_mask=self.planned_mask, + action_retry_counts=self.action_retry_counts, + replan_counts=self.replan_counts, + request=self.request, + plan=self.plan, + ) + + @dataclass(frozen=True, slots=True, eq=False) class EffectVerificationRequest: - """Typed boundary describing a semantic effect awaiting verification. + """Typed boundary describing a physical effect awaiting verification. ``requested_at`` and ``deadline`` use the same timestamp domain as :class:`RobotObservation`. Request-mask shrinkage retains both values; @@ -124,6 +235,7 @@ class EffectVerificationRequest: deadline: float env_mask: torch.Tensor expected_effects: StateDelta + effect_verification: EffectVerificationRequirement | None = None def __post_init__(self) -> None: if type(self.verification_id) is not int or self.verification_id < 0: @@ -158,10 +270,30 @@ def __post_init__(self) -> None: 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.") + if ( + self.effect_verification is not None + and type(self.effect_verification) is not EffectVerificationRequirement + ): + raise TypeError( + "effect_verification must be exactly " + "EffectVerificationRequirement or None." + ) + if self.expected_effects.is_empty and self.effect_verification is None: + raise ValueError( + "Effect verification requires expected symbolic effects or an " + "explicit physical-effect requirement." + ) object.__setattr__(self, "env_mask", self.env_mask.clone()) object.__setattr__(self, "expected_effects", self.expected_effects.snapshot()) + object.__setattr__( + self, + "effect_verification", + ( + None + if self.effect_verification is None + else self.effect_verification.snapshot() + ), + ) def snapshot(self) -> EffectVerificationRequest: """Return a request snapshot with an independently owned row mask.""" @@ -177,6 +309,7 @@ def snapshot(self) -> EffectVerificationRequest: deadline=self.deadline, env_mask=self.env_mask, expected_effects=self.expected_effects, + effect_verification=self.effect_verification, ) @@ -273,9 +406,9 @@ 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 a correlated :class:`EffectVerificationResult` for a non-empty - :class:`StateDelta`. + endpoint-command frame. A declared physical-effect boundary resolves only + after the caller supplies a correlated :class:`EffectVerificationResult`. + Non-empty expected symbolic effects are committed for verified rows only. Environment eligibility and recovery budgets are tracked per row. The waypoint cursor is batch-synchronized: a recoverable row replans the active @@ -330,6 +463,7 @@ def __init__( self._effect_failures = torch.zeros_like(self._eligible) self._effect_requested_at: float | None = None self._next_effect_verification_id = 0 + self._plan_attempt_records: list[_ExecutionPlanAttemptRecord] = [] self._status = ( ExecutionStatus.RUNNING if self._eligible.any() else ExecutionStatus.FAILED ) @@ -573,6 +707,27 @@ def active_commands(self) -> TimedCommandSequence: assert self._plan is not None return self._plan.commands.snapshot() + @property + def active_plan(self) -> ActionPlan: + """Return an independently owned snapshot of the active action plan. + + This is a read-only diagnostics boundary for runtime metadata, + visualization, and tests. Planning and recovery remain session-owned; + mutating any tensor in the returned value cannot affect execution. + """ + assert self._plan is not None + return self._plan.snapshot() + + @property + def plan_attempts(self) -> tuple[ExecutionPlanAttempt, ...]: + """Return every installed plan in deterministic recovery order. + + The initial plan has generation zero. Each invocation revision, + recovery replan, or whole-action retry appends a new generation instead + of replacing earlier scene/collision evidence. + """ + return tuple(record.snapshot() for record in self._plan_attempt_records) + def trajectory_segment(self, name: str) -> TrajectorySegment: """Return named segment metadata for the active action plan. @@ -607,7 +762,7 @@ def tick( "effect_result must be exactly EffectVerificationResult or None." ) if self._pending_effect is None: - raise ValueError("No semantic effect is awaiting verification.") + raise ValueError("No physical 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 " @@ -645,7 +800,7 @@ def tick( self._event( ExecutionEventKind.EFFECT_VERIFICATION_FAILED, known_failures, - "Expected semantic effects were not observed.", + "Required physical effects were not observed.", ) ) if planning_failed.any(): @@ -708,7 +863,7 @@ def tick( self._attempt_action_retry( retry_mask, ExecutionEventKind.EFFECT_VERIFICATION_FAILED, - "Expected semantic effects were not observed.", + "Required physical effects were not observed.", reason_mask=failed_effect, ) ) @@ -756,6 +911,18 @@ def tick( events=events, ) + if not execution_mask.any(): + command, hold_targets, completion_events = self._finish_action( + execution_mask, + 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: command = self._command_at(plan, self._waypoint_index, execution_mask) @@ -767,11 +934,15 @@ def tick( terminal_error > plan.recovery_policy.tracking_error_threshold ) if not_reached.any(): + max_terminal_error = float(terminal_error[not_reached].amax().item()) events.extend( self._attempt_replan( not_reached, ExecutionEventKind.TRACKING_ERROR, - "Terminal command has not been reached.", + "Terminal command has not been reached " + f"(max_error={max_terminal_error:.6f}, " + "threshold=" + f"{plan.recovery_policy.tracking_error_threshold:.6f}).", ) ) if self._status is not ExecutionStatus.RUNNING: @@ -905,6 +1076,23 @@ def _install_plan( self._effect_failures.zero_() self._effect_requested_at = None planned_mask = self._pending & plan.plan_success + self._plan_attempt_records.append( + _ExecutionPlanAttemptRecord( + attempt_generation=self._attempt_generation, + event_kind=event_kind, + planned_at=context.robot.timestamp, + invocation_index=self._invocation_index, + planned_mask=planned_mask.clone(), + action_retry_counts=tuple( + int(value) for value in self._action_retries.detach().cpu().tolist() + ), + replan_counts=tuple( + int(value) for value in self._replans.detach().cpu().tolist() + ), + request=self._requests[self._invocation_index].snapshot(), + plan=plan.snapshot(), + ) + ) self._queued_events.append( self._event(event_kind, planned_mask, "Planned from the latest context.") ) @@ -1011,17 +1199,25 @@ def _recover_if_needed( & (tracking_error > plan.recovery_policy.tracking_error_threshold) ) if tracking_mask.any(): + max_tracking_error = float(tracking_error[tracking_mask].amax().item()) return self._attempt_replan( tracking_mask, ExecutionEventKind.TRACKING_ERROR, - "Observed joint tracking error exceeded the policy threshold.", + "Observed joint tracking error exceeded the policy threshold " + f"(max_error={max_tracking_error:.6f}, " + "threshold=" + f"{plan.recovery_policy.tracking_error_threshold:.6f}).", ) - scene_mask = self._dynamic_scene_change_mask(plan) - if (execution_mask & scene_mask).any(): + scene_mask, scene_message = self._dynamic_scene_change( + plan, + execution_mask, + ) + if scene_mask.any(): + assert scene_message is not None return self._attempt_replan( - execution_mask & scene_mask, + scene_mask, ExecutionEventKind.DYNAMIC_GOAL_CHANGED, - "A referenced scene entity moved beyond the policy threshold.", + scene_message, ) return events @@ -1165,7 +1361,7 @@ def _finish_action( failed_effect = torch.zeros_like(execution_mask) unresolved = torch.zeros_like(execution_mask) made_progress = False - if self._plan.expected_effects.is_empty: + if not self._plan.requires_effect_verification: verified = execution_mask elif effect_result is None: if self._pending_effect is None: @@ -1174,7 +1370,7 @@ def _finish_action( self._event( ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED, execution_mask, - "Expected symbolic effects require external verification.", + "The action requires external physical-effect verification.", ) ) return None, active_targets, events @@ -1202,15 +1398,16 @@ def _finish_action( self._pending_effect = None if verified.any(): - self._task_state = self._plan.expected_effects.apply( - self._task_state, verified - ) - self._context = PlanningContext( - robot=self._context.robot, - task=self._task_state, - scene=self._context.scene, - env_ids=self._context.env_ids, - ) + if not self._plan.expected_effects.is_empty: + self._task_state = self._plan.expected_effects.apply( + self._task_state, verified + ) + self._context = PlanningContext( + robot=self._context.robot, + task=self._task_state, + scene=self._context.scene, + env_ids=self._context.env_ids, + ) self._pending &= ~verified if unresolved.any(): if made_progress: @@ -1387,21 +1584,47 @@ def _terminal_error(self, plan: ActionPlan) -> torch.Tensor: ) return torch.amax(torch.cat(errors, dim=1), dim=1) - def _dynamic_scene_change_mask(self, plan: ActionPlan) -> torch.Tensor: - """Detect material motion of entities referenced by the action goal.""" + def _dynamic_scene_change( + self, + plan: ActionPlan, + execution_mask: torch.Tensor, + ) -> tuple[torch.Tensor, str | None]: + """Detect and describe material scene-dependency invalidation.""" dependencies = plan.scene_dependencies changed = torch.zeros_like(self._eligible) if ( not dependencies or self._context.scene.version == self._planned_scene.version ): - return changed + return changed, None policy = plan.recovery_policy - for entity_id in dependencies: + details: list[str] = [] + for entity_id in sorted(dependencies): + monitor_until = plan.scene_dependency_monitor_until.get(entity_id) + if monitor_until is not None and self._waypoint_index >= monitor_until: + continue previous = self._planned_scene.entities.get(entity_id) current = self._context.scene.entities.get(entity_id) if previous is None or current is None: - changed |= self._eligible + entity_changed = execution_mask.clone() + if not entity_changed.any(): + continue + changed |= entity_changed + missing = [] + if previous is None: + missing.append("planned_scene") + if current is None: + missing.append("current_scene") + details.append( + self._scene_dependency_change_detail( + entity_id=entity_id, + monitor_until=monitor_until, + policy=policy, + max_translation=None, + max_rotation=None, + missing=",".join(missing), + ) + ) continue previous_pose = self._batched_entity_pose(previous) current_pose = self._batched_entity_pose(current) @@ -1416,10 +1639,55 @@ def _dynamic_scene_change_mask(self, plan: ActionPlan) -> torch.Tensor: (relative_rotation.diagonal(dim1=1, dim2=2).sum(dim=1) - 1.0) / 2.0 ).clamp(-1.0, 1.0) rotation = torch.acos(cosine) - changed |= (translation > policy.goal_translation_threshold) | ( - rotation > policy.goal_rotation_threshold + entity_changed = execution_mask & ( + (translation > policy.goal_translation_threshold) + | (rotation > policy.goal_rotation_threshold) + ) + if not entity_changed.any(): + continue + changed |= entity_changed + details.append( + self._scene_dependency_change_detail( + entity_id=entity_id, + monitor_until=monitor_until, + policy=policy, + max_translation=float(translation[entity_changed].amax().item()), + max_rotation=float(rotation[entity_changed].amax().item()), + missing=None, + ) ) - return changed + if not details: + return changed, None + return ( + changed, + "Scene dependency invalidated the active plan at " + f"waypoint_index={self._waypoint_index}: " + " | ".join(details) + ".", + ) + + @staticmethod + def _scene_dependency_change_detail( + *, + entity_id: str, + monitor_until: int | None, + policy: RecoveryPolicy, + max_translation: float | None, + max_rotation: float | None, + missing: str | None, + ) -> str: + """Return one stable scene-dependency diagnostic fragment.""" + cutoff = "none" if monitor_until is None else str(monitor_until) + translation = ( + "unavailable" if max_translation is None else f"{max_translation:.6f}" + ) + rotation = "unavailable" if max_rotation is None else f"{max_rotation:.6f}" + missing_detail = "" if missing is None else f", missing={missing}" + return ( + f"entity_id={entity_id!r}, monitor_cutoff={cutoff}{missing_detail}, " + f"max_translation={translation}, " + f"translation_threshold={policy.goal_translation_threshold:.6f}, " + f"max_rotation={rotation}, " + f"rotation_threshold={policy.goal_rotation_threshold:.6f}" + ) def _collision_world_change_mask(self, plan: ActionPlan) -> torch.Tensor: """Detect collision revisions newer than the active action plan.""" @@ -1486,6 +1754,7 @@ def _effect_verification_request( ), env_mask=env_mask, expected_effects=self._plan.expected_effects, + effect_verification=self._plan.effect_verification, ) def _event( @@ -1565,6 +1834,7 @@ def _tick_result( "EffectVerificationResult", "ExecutionEvent", "ExecutionEventKind", + "ExecutionPlanAttempt", "ExecutionSession", "ExecutionStatus", "ExecutionTick", diff --git a/embodichain/lab/sim/atomic_actions/goals.py b/embodichain/lab/sim/atomic_actions/goals.py index 755d3ec24..cce03c73d 100644 --- a/embodichain/lab/sim/atomic_actions/goals.py +++ b/embodichain/lab/sim/atomic_actions/goals.py @@ -21,6 +21,7 @@ import warnings from collections.abc import Mapping, Sequence from dataclasses import dataclass, fields, is_dataclass +import math from typing import Any, ClassVar, Protocol, TYPE_CHECKING import torch @@ -86,6 +87,126 @@ def snapshot(self) -> SceneEntityPose: ) +@dataclass(frozen=True, slots=True, eq=False) +class SceneArticulationOperationGeometry: + """Late-bound handle geometry for an articulation operation. + + The offsets and operation axis are immutable grounded affordance data. The + handle itself remains a :class:`SceneEntityPose`, so every atomic plan or + recovery replan resolves it from the latest :class:`SceneSnapshot`. + """ + + handle_pose: SceneEntityPose + approach_offset: torch.Tensor + contact_offset: torch.Tensor + operation_offset: torch.Tensor + retract_offset: torch.Tensor + operation_axis: torch.Tensor + position_scale: float = 1.0 + + def __post_init__(self) -> None: + if not isinstance(self.handle_pose, SceneEntityPose): + raise TypeError("handle_pose must be a SceneEntityPose.") + object.__setattr__(self, "handle_pose", self.handle_pose.snapshot()) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ): + offset = getattr(self, field_name) + validate_pose_tensor(offset, field_name, allow_waypoints=False) + if offset.shape != (4, 4): + raise ValueError(f"{field_name} must have shape (4, 4).") + if not offset.is_floating_point() or not torch.isfinite(offset).all(): + raise ValueError(f"{field_name} must be a finite floating tensor.") + object.__setattr__(self, field_name, offset.clone()) + axis = self.operation_axis + if ( + not isinstance(axis, torch.Tensor) + or axis.shape != (3,) + or not axis.is_floating_point() + ): + raise ValueError("operation_axis must be a floating tensor of shape (3,).") + if not torch.isfinite(axis).all(): + raise ValueError("operation_axis must contain only finite values.") + norm = torch.linalg.vector_norm(axis) + if float(norm) <= torch.finfo(axis.dtype).eps: + raise ValueError("operation_axis must be non-zero.") + object.__setattr__(self, "operation_axis", (axis / norm).clone()) + scale = self.position_scale + if isinstance(scale, bool) or not isinstance(scale, (int, float)): + raise TypeError("position_scale must be a finite positive scalar.") + scale = float(scale) + if not math.isfinite(scale) or scale <= 0.0: + raise ValueError("position_scale must be a finite positive scalar.") + object.__setattr__(self, "position_scale", scale) + + def resolve( + self, + context: PlanningContext, + *, + displacement: torch.Tensor, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Resolve four poses using a fresh handle and row-local displacement. + + Args: + context: Latest immutable planning observation. + displacement: Remaining signed handle displacement, shape ``(B,)``. + + Returns: + Approach, contact, operation, and retract pose batches. + """ + if not isinstance(displacement, torch.Tensor): + raise TypeError("displacement must be a torch.Tensor.") + if displacement.shape != (context.batch_size,): + raise ValueError("displacement must have one scalar for each planning row.") + if ( + not displacement.is_floating_point() + or not torch.isfinite(displacement).all() + ): + raise ValueError("displacement must be a finite floating tensor.") + handle = resolve_pose_goal( + self.handle_pose, + context, + name="handle_pose", + ) + offsets = tuple( + getattr(self, field_name) + .to(device=handle.device, dtype=handle.dtype) + .unsqueeze(0) + .expand(context.batch_size, -1, -1) + for field_name in ( + "approach_offset", + "contact_offset", + "operation_offset", + "retract_offset", + ) + ) + translation = ( + torch.eye( + 4, + dtype=handle.dtype, + device=handle.device, + ) + .unsqueeze(0) + .repeat(context.batch_size, 1, 1) + ) + axis = self.operation_axis.to(device=handle.device, dtype=handle.dtype) + translation[:, :3, 3] = ( + axis.unsqueeze(0) + * displacement.to(device=handle.device, dtype=handle.dtype).unsqueeze(1) + * self.position_scale + ) + moved_handle = torch.bmm(handle, translation) + return ( + torch.bmm(handle, offsets[0]), + torch.bmm(handle, offsets[1]), + torch.bmm(moved_handle, offsets[2]), + torch.bmm(moved_handle, offsets[3]), + ) + + PoseGoalValue = torch.Tensor | SceneEntityPose """Explicit pose tensor or a pose resolved from the latest scene snapshot.""" @@ -263,6 +384,7 @@ def __post_init__(self) -> None: "ActionGoal", "ObjectActionGoal", "PoseGoalValue", + "SceneArticulationOperationGeometry", "SceneEntityPose", "collect_scene_dependencies", "resolve_pose_goal", diff --git a/embodichain/lab/sim/atomic_actions/invocation.py b/embodichain/lab/sim/atomic_actions/invocation.py index 652cbf78a..a600ff30b 100644 --- a/embodichain/lab/sim/atomic_actions/invocation.py +++ b/embodichain/lab/sim/atomic_actions/invocation.py @@ -200,6 +200,19 @@ def __post_init__(self) -> None: object.__setattr__(self, "recovery_policy", deepcopy(self.recovery_policy)) object.__setattr__(self, "skill_options", deepcopy(self.skill_options)) + def snapshot(self) -> ResolvedActionRequest[GoalT, OptionsT]: + """Return an independently owned resolved-request snapshot.""" + return ResolvedActionRequest( + skill_id=self.skill_id, + goal=self.goal, + binding=self.binding, + motion_policy=self.motion_policy, + recovery_policy=self.recovery_policy, + skill_options=self.skill_options, + invocation_id=self.invocation_id, + revision=self.revision, + ) + __all__ = [ "ActionInvocation", diff --git a/embodichain/lab/sim/atomic_actions/plans.py b/embodichain/lab/sim/atomic_actions/plans.py index b423cf19e..7e86f16cf 100644 --- a/embodichain/lab/sim/atomic_actions/plans.py +++ b/embodichain/lab/sim/atomic_actions/plans.py @@ -18,6 +18,7 @@ from __future__ import annotations +from copy import deepcopy from dataclasses import dataclass, field from enum import Enum from types import MappingProxyType @@ -370,8 +371,17 @@ class PlannerDiagnostics: def __post_init__(self) -> None: if not isinstance(self.backend, str) or not self.backend: raise ValueError("PlannerDiagnostics.backend must be non-empty.") - object.__setattr__(self, "messages", tuple(self.messages)) - object.__setattr__(self, "metadata", MappingProxyType(dict(self.metadata))) + if not isinstance(self.metadata, Mapping): + raise TypeError("PlannerDiagnostics.metadata must be a mapping.") + messages = tuple(self.messages) + if not all(type(message) is str for message in messages): + raise TypeError("PlannerDiagnostics.messages must contain strings.") + object.__setattr__(self, "messages", messages) + object.__setattr__( + self, + "metadata", + MappingProxyType(deepcopy(dict(self.metadata))), + ) class ExecutionFeedbackMode(str, Enum): @@ -381,6 +391,36 @@ class ExecutionFeedbackMode(str, Enum): TIMED = "timed" +@dataclass(frozen=True, slots=True) +class EffectVerificationRequirement: + """Explicit physical-effect verification independent of symbolic state. + + Presence of this value on an :class:`ActionPlan` forces a terminal effect + boundary even when the plan declares no :class:`StateDelta`. The open + ``kind`` identifier lets an external runtime select an appropriate + verifier without placing backend-specific callbacks in the core plan. + + Args: + kind: Stable, non-empty discriminator for the physical effect. + """ + + kind: str + + def __post_init__(self) -> None: + if ( + type(self.kind) is not str + or not self.kind + or self.kind != self.kind.strip() + ): + raise ValueError( + "kind must be a non-empty string without outer whitespace." + ) + + def snapshot(self) -> EffectVerificationRequirement: + """Return an independently owned requirement value.""" + return EffectVerificationRequirement(kind=self.kind) + + @dataclass(frozen=True, slots=True) class TrajectorySegment: """Named half-open waypoint range inside an action trajectory. @@ -423,6 +463,15 @@ class ActionPlan: An action owns one timed command sequence and one recovery boundary. Named :class:`TrajectorySegment` values describe semantic structure within that sequence without implying independent planning or recovery boundaries. + + Attributes: + scene_dependency_monitor_until: Optional exclusive waypoint-index upper + bounds for individual ``scene_dependencies``. An entity is monitored + while the current waypoint index is smaller than its bound; ``0`` + disables monitoring immediately, while an omitted entity remains + monitored for the action's full execution. Once the bound is reached, + all pose changes for that entity are ignored, regardless of whether + they were caused by the action or by an external disturbance. """ skill_id: str @@ -436,9 +485,11 @@ class ActionPlan: joint_trajectory: TimedTrajectory | None = None segments: tuple[TrajectorySegment, ...] = () scene_dependencies: tuple[str, ...] = () + scene_dependency_monitor_until: Mapping[str, int] = field(default_factory=dict) collision_world_sensitive: bool = False replannable: bool = True expected_effects: StateDelta = field(default_factory=StateDelta) + effect_verification: EffectVerificationRequirement | None = None invocation_id: str | None = None invocation_revision: int = 0 @@ -645,13 +696,37 @@ def __post_init__(self) -> None: raise ValueError( "scene_dependencies must contain unique non-empty entity ids." ) + waypoint_count = self.commands.frame_count + monitor_until = dict(self.scene_dependency_monitor_until) + if not set(monitor_until).issubset(dependencies): + raise ValueError( + "scene_dependency_monitor_until keys must be scene dependencies." + ) + for entity_id, waypoint_index in monitor_until.items(): + if ( + type(entity_id) is not str + or not entity_id + or type(waypoint_index) is not int + or not 0 <= waypoint_index <= waypoint_count + ): + raise ValueError( + "scene_dependency_monitor_until must map non-empty entity IDs " + "to waypoint indices within the command sequence." + ) if not isinstance(self.collision_world_sensitive, bool): raise TypeError("collision_world_sensitive must be a bool.") if not isinstance(self.replannable, bool): raise TypeError("replannable must be a bool.") if not isinstance(self.expected_effects, StateDelta): raise TypeError("expected_effects must be a StateDelta.") - waypoint_count = self.commands.frame_count + if ( + self.effect_verification is not None + and type(self.effect_verification) is not EffectVerificationRequirement + ): + raise TypeError( + "effect_verification must be exactly " + "EffectVerificationRequirement or None." + ) segments = tuple(self.segments) if not all(isinstance(segment, TrajectorySegment) for segment in segments): raise TypeError("segments must contain only TrajectorySegment values.") @@ -688,14 +763,77 @@ def __post_init__(self) -> None: ), ) object.__setattr__(self, "planned_collision_world_revision", revisions) + object.__setattr__( + self, + "diagnostics", + PlannerDiagnostics( + backend=self.diagnostics.backend, + messages=self.diagnostics.messages, + metadata=self.diagnostics.metadata, + ), + ) object.__setattr__(self, "scene_dependencies", dependencies) + object.__setattr__( + self, + "scene_dependency_monitor_until", + MappingProxyType(monitor_until), + ) object.__setattr__(self, "segments", segments) + object.__setattr__( + self, + "effect_verification", + ( + None + if self.effect_verification is None + else self.effect_verification.snapshot() + ), + ) @property def success_all(self) -> bool: """Whether every environment row planned successfully.""" return bool(self.plan_success.all().item()) + def snapshot(self) -> ActionPlan: + """Return an independently owned inspection snapshot of this plan. + + Runtime tracing and visualization need access to the exact plan that + reached an execution boundary without being able to mutate the live + session. Reconstructing the value through the public constructor also + re-applies every plan invariant and snapshots all tensor-owning nested + contracts. + + Returns: + A validated plan with independently owned tensor storage. + """ + return ActionPlan( + skill_id=self.skill_id, + plan_success=self.plan_success, + commands=self.commands, + recovery_policy=self.recovery_policy, + planned_scene_version=self.planned_scene_version, + planned_collision_world_revision=self.planned_collision_world_revision, + diagnostics=self.diagnostics, + feedback_mode=self.feedback_mode, + joint_trajectory=self.joint_trajectory, + segments=self.segments, + scene_dependencies=self.scene_dependencies, + scene_dependency_monitor_until=self.scene_dependency_monitor_until, + collision_world_sensitive=self.collision_world_sensitive, + replannable=self.replannable, + expected_effects=self.expected_effects, + effect_verification=self.effect_verification, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + ) + + @property + def requires_effect_verification(self) -> bool: + """Whether execution must verify a terminal physical effect.""" + return ( + self.effect_verification is not None or not self.expected_effects.is_empty + ) + def segment(self, name: str) -> TrajectorySegment: """Return a named trajectory segment. @@ -776,6 +914,7 @@ def segment(self, action_index: int, name: str) -> TrajectorySegment: __all__ = [ "ActionPlan", "CompiledTrajectory", + "EffectVerificationRequirement", "ExecutionFeedbackMode", "PlannerDiagnostics", "TimedTrajectory", diff --git a/embodichain/lab/sim/atomic_actions/primitives/__init__.py b/embodichain/lab/sim/atomic_actions/primitives/__init__.py index 85de2c985..563972e6a 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/__init__.py +++ b/embodichain/lab/sim/atomic_actions/primitives/__init__.py @@ -41,6 +41,11 @@ MoveHeldObjectOptions, ) from .move_joints import JointPositionGoal, MoveJoints, MoveJointsOptions +from .operate_articulation import ( + OperateArticulation, + OperateArticulationGoal, + OperateArticulationOptions, +) from .pick_up import GraspGoal, PickUp, PickUpOptions from .place import AssembleGoal, Place, PlaceGoal, PlaceOptions from .press import Press, PressGoal, PressOptions @@ -55,6 +60,7 @@ CoordinatedPickment, CoordinatedPlacement, HandOver, + OperateArticulation, ) """Built-in action implementations instantiated once per action engine.""" @@ -79,6 +85,9 @@ "MoveHeldObjectOptions", "MoveJoints", "MoveJointsOptions", + "OperateArticulation", + "OperateArticulationGoal", + "OperateArticulationOptions", "PickUp", "PickUpOptions", "Place", diff --git a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py index fe3f4c60f..dae4c3016 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/_helpers.py +++ b/embodichain/lab/sim/atomic_actions/primitives/_helpers.py @@ -22,9 +22,41 @@ from embodichain.utils import logger +from ..bindings import EndpointBinding from ..state import PlanningContext +def require_shared_task_state_key( + motion: EndpointBinding, + grasp: EndpointBinding, + *, + participant: str, +) -> str: + """Return the stable task-state key shared by one participant's endpoints. + + Args: + motion: Participant endpoint used for motion control. + grasp: Participant endpoint used for grasp control. + participant: Human-readable participant name used in validation errors. + + Returns: + Stable logical key used to address held-object task state. + + Raises: + ValueError: If the participant endpoints use different task-state keys. + """ + motion_key = motion.task_state_key + grasp_key = grasp.task_state_key + if motion_key != grasp_key: + raise ValueError( + f"{participant} motion and grasp endpoints must share one " + f"task_state_key, but got {motion_key!r} and {grasp_key!r}." + ) + if not isinstance(motion_key, str) or not motion_key: + raise ValueError(f"{participant} task_state_key must be a non-empty string.") + return motion_key + + def resolve_object_target( target: torch.Tensor, *, @@ -52,4 +84,8 @@ def arm_qpos_from_state( return context.robot.qpos[:, arm_joint_ids] -__all__ = ["arm_qpos_from_state", "resolve_object_target"] +__all__ = [ + "arm_qpos_from_state", + "require_shared_task_state_key", + "resolve_object_target", +] diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index b8cbb0b42..a528fd37d 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -51,6 +51,7 @@ ) from ..state import CoordinatedHeldObjectState, PlanningContext from ..trajectory_ops import interpolate_joint_trajectory, translate_pose_world +from ._helpers import require_shared_task_state_key @dataclass(frozen=True, slots=True, eq=False) @@ -158,6 +159,8 @@ def __post_init__(self) -> None: class _CoordinatedPickResources: """Invocation-bound control parts and compatible hand commands.""" + left_task_state_key: str + right_task_state_key: str left_arm: JointPositionTarget right_arm: JointPositionTarget left_hand: JointPositionTarget @@ -425,6 +428,21 @@ def _resolve_resources( right_arm = right_motion.require_target(JointPositionTarget) left_hand = left_grasp.require_target(JointPositionTarget) right_hand = right_grasp.require_target(JointPositionTarget) + left_task_state_key = require_shared_task_state_key( + left_motion, + left_grasp, + participant="CoordinatedPickment left participant", + ) + right_task_state_key = require_shared_task_state_key( + right_motion, + right_grasp, + participant="CoordinatedPickment right participant", + ) + if left_task_state_key == right_task_state_key: + raise ValueError( + "CoordinatedPickment left and right participants must use " + "different task_state_key values." + ) if left_arm.control_part == right_arm.control_part: raise ValueError( "CoordinatedPickment left and right roles must use different " @@ -436,6 +454,8 @@ def _resolve_resources( "end-effector control parts." ) return _CoordinatedPickResources( + left_task_state_key=left_task_state_key, + right_task_state_key=right_task_state_key, left_arm=left_arm, right_arm=right_arm, left_hand=left_hand, @@ -1025,13 +1045,13 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.left_arm.control_part: None, - resources.right_arm.control_part: None, + resources.left_task_state_key: None, + resources.right_task_state_key: None, }, coordinated_held_object_updates={ ( - resources.left_arm.control_part, - resources.right_arm.control_part, + resources.left_task_state_key, + resources.right_task_state_key, ): coordinated_held_object, }, ), diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py index c00771e02..cba06598e 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -25,7 +25,6 @@ from embodichain.utils import logger -from ._helpers import resolve_object_target from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand from ..core import AtomicAction @@ -49,6 +48,7 @@ interpolate_hand_qpos, translate_pose_world, ) +from ._helpers import require_shared_task_state_key, resolve_object_target @dataclass(frozen=True, slots=True, eq=False) @@ -122,6 +122,8 @@ def __post_init__(self) -> None: class _CoordinatedPlacementResources: """Invocation-bound control parts and compatible hand commands.""" + placing_task_state_key: str + support_task_state_key: str placing_arm: JointPositionTarget support_arm: JointPositionTarget placing_hand: JointPositionTarget @@ -205,6 +207,21 @@ def _resolve_resources( support_arm = support_motion.require_target(JointPositionTarget) placing_hand = placing_grasp.require_target(JointPositionTarget) support_hand = support_grasp.require_target(JointPositionTarget) + placing_task_state_key = require_shared_task_state_key( + placing_motion, + placing_grasp, + participant="CoordinatedPlacement placing participant", + ) + support_task_state_key = require_shared_task_state_key( + support_motion, + support_grasp, + participant="CoordinatedPlacement support participant", + ) + if placing_task_state_key == support_task_state_key: + raise ValueError( + "CoordinatedPlacement placing and support participants must " + "use different task_state_key values." + ) if placing_arm.control_part == support_arm.control_part: raise ValueError( "CoordinatedPlacement placing and support roles must use " @@ -216,6 +233,8 @@ def _resolve_resources( "different end-effector control parts." ) return _CoordinatedPlacementResources( + placing_task_state_key=placing_task_state_key, + support_task_state_key=support_task_state_key, placing_arm=placing_arm, support_arm=support_arm, placing_hand=placing_hand, @@ -404,14 +423,14 @@ def _plan( ], dim=1, ) - involved_control_parts = { - resources.placing_arm.control_part, - resources.support_arm.control_part, + involved_task_state_keys = { + resources.placing_task_state_key, + resources.support_task_state_key, } coordinated_removals = { key: None for key in state.coordinated_held_objects - if not involved_control_parts.isdisjoint(key) + if not involved_task_state_keys.isdisjoint(key) } return self.build_plan( request, @@ -420,10 +439,10 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.placing_arm.control_part: ( + resources.placing_task_state_key: ( None if release else placing_held_object ), - resources.support_arm.control_part: support_held_object, + resources.support_task_state_key: support_held_object, }, coordinated_held_object_updates=coordinated_removals, ), @@ -507,20 +526,20 @@ def _resolve_target( HeldObjectState, HeldObjectState, ]: - placing_control_part = resources.placing_arm.control_part - support_control_part = resources.support_arm.control_part - placing_held_object = state.get_held_object(placing_control_part) + placing_task_state_key = resources.placing_task_state_key + support_task_state_key = resources.support_task_state_key + placing_held_object = state.get_held_object(placing_task_state_key) if placing_held_object is None: logger.log_error( - "CoordinatedPlacement requires an object held by placing control " - f"part {placing_control_part!r}.", + "CoordinatedPlacement requires an object held by placing " + f"task-state resource {placing_task_state_key!r}.", ValueError, ) - support_held_object = state.get_held_object(support_control_part) + support_held_object = state.get_held_object(support_task_state_key) if support_held_object is None: logger.log_error( - "CoordinatedPlacement requires an object held by support control " - f"part {support_control_part!r}.", + "CoordinatedPlacement requires an object held by support " + f"task-state resource {support_task_state_key!r}.", ValueError, ) placing_height_offset = ( diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 72c4bb0f4..87c98517d 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -55,6 +55,7 @@ interpolate_hand_qpos, translate_pose_world, ) +from ._helpers import require_shared_task_state_key from .pick_up import GraspGoal @@ -140,6 +141,8 @@ def __post_init__(self) -> None: class _HandOverResources: """Invocation-bound control parts and compatible hand commands.""" + transfer_task_state_key: str + receive_task_state_key: str transfer_arm: JointPositionTarget receive_arm: JointPositionTarget transfer_hand: JointPositionTarget @@ -250,6 +253,21 @@ def _resolve_resources( receive_arm = receive_motion.require_target(JointPositionTarget) transfer_hand = transfer_grasp.require_target(JointPositionTarget) receive_hand = receive_grasp.require_target(JointPositionTarget) + transfer_task_state_key = require_shared_task_state_key( + transfer_motion, + transfer_grasp, + participant="HandOver source participant", + ) + receive_task_state_key = require_shared_task_state_key( + receive_motion, + receive_grasp, + participant="HandOver destination participant", + ) + if transfer_task_state_key == receive_task_state_key: + raise ValueError( + "HandOver source and destination must use different " + "task_state_key values." + ) if transfer_arm.control_part == receive_arm.control_part: raise ValueError( "HandOver source and destination must use different manipulator " @@ -261,6 +279,8 @@ def _resolve_resources( "control parts." ) return _HandOverResources( + transfer_task_state_key=transfer_task_state_key, + receive_task_state_key=receive_task_state_key, transfer_arm=transfer_arm, receive_arm=receive_arm, transfer_hand=transfer_hand, @@ -316,7 +336,7 @@ def _plan( semantics = target.semantics transfer_object_to_eef = self._resolve_transfer_object_to_eef( state, - resources.transfer_arm.control_part, + resources.transfer_task_state_key, semantics, ) transfer_start_qpos, receive_start_qpos = self._resolve_start_qpos( @@ -600,8 +620,8 @@ def _plan( trajectory=full, expected_effects=StateDelta( held_object_updates={ - resources.transfer_arm.control_part: None, - resources.receive_arm.control_part: held_object, + resources.transfer_task_state_key: None, + resources.receive_task_state_key: held_object, } ), segment_lengths=segment_lengths, @@ -634,20 +654,20 @@ def _resolve_matrix(self, matrix: torch.Tensor, name: str) -> torch.Tensor: def _resolve_transfer_object_to_eef( self, state: PlanningContext, - transfer_control_part: str, + transfer_task_state_key: str, target_semantics: ObjectSemantics, ) -> torch.Tensor: - held = state.get_held_object(transfer_control_part) + held = state.get_held_object(transfer_task_state_key) if held is None: logger.log_error( - "HandOver requires an object held by transfer control part " - f"{transfer_control_part!r} (run PickUp first).", + "HandOver requires an object held by source task-state resource " + f"{transfer_task_state_key!r} (run PickUp first).", ValueError, ) if not _same_object_identity(target_semantics, held.semantics): raise ValueError( "HandOver target semantics must identify the object held by " - f"transfer control part {transfer_control_part!r}." + f"source task-state resource {transfer_task_state_key!r}." ) return self._resolve_matrix(held.object_to_eef, "held_object.object_to_eef") diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py index 7cb43860e..604795e0c 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -30,7 +30,11 @@ pose_inv, ) -from ._helpers import arm_qpos_from_state, resolve_object_target +from ._helpers import ( + arm_qpos_from_state, + require_shared_task_state_key, + resolve_object_target, +) from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, JointPositionCommand from ..core import AtomicAction @@ -146,6 +150,11 @@ def _plan( grasp = binding.endpoint("primary", "grasp") motion_target = motion.require_target(JointPositionTarget) grasp_target = grasp.require_target(JointPositionTarget) + task_state_key = require_shared_task_state_key( + motion, + grasp, + participant="MoveHeldObject primary participant", + ) control_part = motion_target.control_part arm_joint_ids = list(motion_target.joint_ids) hand_joint_ids = list(grasp_target.joint_ids) @@ -156,11 +165,11 @@ def _plan( dtype=context.robot.qpos.dtype, ) state = context - held_object = state.get_held_object(control_part) + held_object = state.get_held_object(task_state_key) if held_object is None: logger.log_error( - "MoveHeldObject requires an object held by control part " - f"{control_part!r} - run PickUp first.", + "MoveHeldObject requires an object held by task-state resource " + f"{task_state_key!r} - run PickUp first.", ValueError, ) object_target_pose = resolve_object_target( diff --git a/embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py b/embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py new file mode 100644 index 000000000..c40bcb491 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/primitives/operate_articulation.py @@ -0,0 +1,468 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Reusable contact-and-drag operation for articulated mechanisms.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import ClassVar + +import torch + +from ..bindings import JointPositionTarget +from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand +from ..core import AtomicAction +from ..effects import StateDelta +from ..goals import SceneArticulationOperationGeometry +from ..invocation import ActionOptions, ResolvedActionRequest +from ..plans import ActionPlan, EffectVerificationRequirement, PlannerDiagnostics +from ..requirements import ( + CARTESIAN_POSE_CAPABILITY, + DisjointSlotEndpoints, + GRASP_CAPABILITY, + JOINT_POSITION_CAPABILITY, + SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, +) +from ..state import ArticulationJointState, PlanningContext +from ..trajectory_ops import ( + build_pose_plan_states, + interpolate_hand_qpos, + resolve_pose_target, +) + + +def _validate_identifier(value: str, *, field_name: str) -> None: + """Validate one canonical articulation identifier.""" + if type(value) is not str or not value or value != value.strip(): + raise ValueError(f"{field_name} must be a non-empty canonical identifier.") + + +@dataclass(frozen=True, slots=True, eq=False) +class OperateArticulationGoal: + """Grounded interaction path and desired state for one articulation joint. + + The semantic compiler copies immutable affordance geometry and records the + live source joint position. The atomic planner combines those values with + the latest handle and joint observation, so the same resolved request can + safely replan drawers, doors, sliders, and similar interactions. + """ + + goal_kind: ClassVar[str] = "operate_articulation" + + articulation_id: str + """Canonical scene-registry articulation identifier.""" + + joint_id: str + """Canonical joint identifier within the articulation.""" + + geometry: SceneArticulationOperationGeometry + """Handle-relative geometry resolved again for every plan and replan.""" + + source_position: torch.Tensor + """Live joint position at semantic grounding, shape ``(1,)`` or ``(B, 1)``.""" + + target_position: torch.Tensor + """Absolute desired joint position, shape ``(1,)`` or ``(B, 1)``.""" + + target_displacement: float + """Signed handle displacement from source position to target position.""" + + def __post_init__(self) -> None: + _validate_identifier(self.articulation_id, field_name="articulation_id") + _validate_identifier(self.joint_id, field_name="joint_id") + if not isinstance(self.geometry, SceneArticulationOperationGeometry): + raise TypeError("geometry must be a SceneArticulationOperationGeometry.") + for field_name in ("source_position", "target_position"): + position = getattr(self, field_name) + if not isinstance(position, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if position.dim() not in (1, 2) or position.shape[-1:] != (1,): + raise ValueError(f"{field_name} must have shape (1,) or (B, 1).") + if not position.is_floating_point(): + raise TypeError(f"{field_name} must use a floating-point dtype.") + if not torch.isfinite(position).all(): + raise ValueError(f"{field_name} must contain only finite values.") + object.__setattr__(self, field_name, position.clone()) + displacement = self.target_displacement + if isinstance(displacement, bool) or not isinstance(displacement, (int, float)): + raise TypeError("target_displacement must be a finite scalar.") + displacement = float(displacement) + if not math.isfinite(displacement): + raise ValueError("target_displacement must be finite.") + object.__setattr__(self, "target_displacement", displacement) + + +@dataclass(frozen=True, slots=True, eq=False) +class OperateArticulationOptions(ActionOptions): + """Per-invocation contact sequencing for articulation operations.""" + + engage_steps: int = 5 + """Number of gripper-closing waypoints at the contact pose.""" + + release_steps: int = 5 + """Number of gripper-opening waypoints before retracting.""" + + def __post_init__(self) -> None: + for field_name in ("engage_steps", "release_steps"): + value = getattr(self, field_name) + if type(value) is not int or value <= 0: + raise ValueError(f"{field_name} must be a positive integer.") + + +class OperateArticulation( + AtomicAction[OperateArticulationGoal, OperateArticulationOptions] +): + """Approach, engage, move, release, and retract an articulated affordance.""" + + skill_id: ClassVar[str] = "operate_articulation" + GoalType: ClassVar[type] = OperateArticulationGoal + OptionsType: ClassVar[type] = OperateArticulationOptions + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } + ), + ), + SkillEndpointRequirement( + endpoint_id="interaction", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands={ + GRASP_COMMAND: JointPositionCommand, + OPEN_COMMAND: JointPositionCommand, + }, + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "interaction")),), + ), + ), + ) + + def __init__( + self, + default_options: OperateArticulationOptions | None = None, + ) -> None: + super().__init__(default_options) + + def _on_bind(self) -> None: + """Capture immutable robot dimensions from engine-owned services.""" + self.n_envs = int(self.robot.get_qpos().shape[0]) + self.robot_dof = int(self.robot.dof) + + def _plan( + self, + request: ResolvedActionRequest[ + OperateArticulationGoal, + OperateArticulationOptions, + ], + context: PlanningContext, + ) -> ActionPlan: + """Plan the complete contact interaction from one observed context.""" + goal = self.require_goal(request) + options = request.skill_options + motion = request.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + interaction = request.binding.endpoint("primary", "interaction") + interaction_target = interaction.require_target(JointPositionTarget) + arm_joint_ids = list(motion.joint_ids) + interaction_joint_ids = list(interaction_target.joint_ids) + + remaining_displacement = self._remaining_displacement(goal, context) + poses = tuple( + resolve_pose_target( + pose, + n_envs=context.batch_size, + device=self.device, + ) + for pose in goal.geometry.resolve( + context, + displacement=remaining_displacement, + ) + ) + motion_counts = self._motion_sample_counts( + request.motion_policy.sample_count, + options, + ) + arm_segments: list[torch.Tensor] = [] + phase_diagnostics: dict[str, dict[str, object]] = {} + arm_start = context.robot.qpos[:, arm_joint_ids] + success = torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ) + phase_names = ("approach", "contact", "operate", "retract") + for phase_name, pose, sample_count in zip( + phase_names, + poses, + motion_counts, + strict=True, + ): + result = self.motion_generator.generate( + build_pose_plan_states(pose), + options=request.motion_policy.to_motion_gen_options( + start_qpos=arm_start, + control_part=motion.control_part, + sample_count=sample_count, + ), + ) + if result.positions is None or not isinstance(result.success, torch.Tensor): + return self.failed_plan( + request, + context, + message=( + "The articulation motion planner returned no trajectory for " + f"phase {phase_name!r}." + ), + ) + phase_success = result.success.to( + device=success.device, + dtype=torch.bool, + ) + failed_rows = ( + (~phase_success).nonzero(as_tuple=False).flatten().detach().cpu() + ) + phase_diagnostics[phase_name] = { + "success": phase_success.detach().cpu().tolist(), + "failed_rows": failed_rows.tolist(), + "waypoint_count": int(result.positions.shape[1]), + } + arm_segments.append(result.positions) + arm_start = result.positions[:, -1] + success &= phase_success + + grasp_qpos = interaction.joint_positions( + GRASP_COMMAND, + n_envs=self.n_envs, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + open_qpos = interaction.joint_positions( + OPEN_COMMAND, + n_envs=self.n_envs, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + initial_interaction = context.robot.qpos[:, interaction_joint_ids] + engage_path = interpolate_hand_qpos( + initial_interaction, + grasp_qpos, + n_waypoints=options.engage_steps, + ) + release_path = interpolate_hand_qpos( + grasp_qpos, + open_qpos, + n_waypoints=options.release_steps, + ) + + approach_arm, contact_arm, operation_arm, retract_arm = arm_segments + lengths = { + "approach": int(approach_arm.shape[1]), + "engage": int(contact_arm.shape[1] + engage_path.shape[1]), + "operate": int(operation_arm.shape[1]), + "release": int(release_path.shape[1]), + "retract": int(retract_arm.shape[1]), + } + full = torch.empty( + ( + context.batch_size, + sum(lengths.values()), + self.robot_dof, + ), + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + full[:] = context.robot.qpos.unsqueeze(1) + cursor = 0 + + def append_motion( + segment: torch.Tensor, + interaction_qpos: torch.Tensor, + ) -> None: + nonlocal cursor + count = int(segment.shape[1]) + full[:, cursor : cursor + count, arm_joint_ids] = segment + full[:, cursor : cursor + count, interaction_joint_ids] = ( + interaction_qpos.unsqueeze(1) + ) + cursor += count + + append_motion(approach_arm, initial_interaction) + append_motion(contact_arm, initial_interaction) + full[:, cursor : cursor + options.engage_steps, arm_joint_ids] = contact_arm[ + :, -1 + ].unsqueeze(1) + full[:, cursor : cursor + options.engage_steps, interaction_joint_ids] = ( + engage_path + ) + cursor += options.engage_steps + append_motion(operation_arm, grasp_qpos) + full[:, cursor : cursor + options.release_steps, arm_joint_ids] = operation_arm[ + :, -1 + ].unsqueeze(1) + full[:, cursor : cursor + options.release_steps, interaction_joint_ids] = ( + release_path + ) + cursor += options.release_steps + append_motion(retract_arm, open_qpos) + assert cursor == full.shape[1] + + target_position = goal.target_position.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + expected = StateDelta( + articulation_joint_updates={ + (goal.articulation_id, goal.joint_id): ArticulationJointState( + target_position + ) + } + ) + return self.build_plan( + request, + context, + success=success, + trajectory=full, + expected_effects=expected, + effect_verification=EffectVerificationRequirement( + kind="articulation.joint_progress" + ), + segment_lengths=lengths, + scene_dependency_monitor_until={ + goal.geometry.handle_pose.entity_id: lengths["approach"] + + lengths["engage"] + }, + diagnostics=PlannerDiagnostics( + backend=self.planning_services.planner_name, + messages=tuple( + f"Articulation motion phase {phase_name!r} failed for rows " + f"{details['failed_rows']}." + for phase_name, details in phase_diagnostics.items() + if details["failed_rows"] + ), + metadata={"motion_phases": phase_diagnostics}, + ), + ) + + @staticmethod + def _position_batch( + value: torch.Tensor, + context: PlanningContext, + *, + field_name: str, + ) -> torch.Tensor: + """Broadcast one scalar joint position to the planning batch.""" + position = value.to( + device=context.robot.qpos.device, + dtype=context.robot.qpos.dtype, + ) + if position.shape == (1,): + return position.unsqueeze(0).expand(context.batch_size, -1).clone() + if position.shape != (context.batch_size, 1): + raise ValueError( + f"{field_name} must have shape (1,) or " f"({context.batch_size}, 1)." + ) + return position.clone() + + @classmethod + def _remaining_displacement( + cls, + goal: OperateArticulationGoal, + context: PlanningContext, + ) -> torch.Tensor: + """Map remaining joint stroke to a bounded signed handle displacement. + + For each row, ``remaining = target_displacement * clamp( + (target - current) / (target - source), 0, 1)``. A zero-length source + stroke, a reached target, and an overshot target all resolve to zero. + """ + observed = context.scene.get_articulation_joint_state( + goal.articulation_id, + goal.joint_id, + ) + address = (goal.articulation_id, goal.joint_id) + if observed is None: + raise ValueError( + "OperateArticulation recovery-safe planning requires a live " + f"ObservedArticulationJointState for {address!r}." + ) + current = cls._position_batch( + observed.position, + context, + field_name=f"observed articulation joint {address!r}", + ) + if observed.valid_mask is not None: + valid = observed.valid_mask.to(device=context.robot.qpos.device) + if not bool(valid.all()): + invalid_rows = (~valid).nonzero(as_tuple=False).flatten().tolist() + raise ValueError( + f"Live articulation joint {address!r} is invalid for planning " + f"rows {invalid_rows}." + ) + source = cls._position_batch( + goal.source_position, + context, + field_name="source_position", + ) + target = cls._position_batch( + goal.target_position, + context, + field_name="target_position", + ) + total = target - source + tolerance = torch.finfo(total.dtype).eps * 16.0 + nonzero_stroke = total.abs() > tolerance + fraction = torch.zeros_like(total) + fraction[nonzero_stroke] = ( + (target - current)[nonzero_stroke] / total[nonzero_stroke] + ).clamp(0.0, 1.0) + return fraction[:, 0] * goal.target_displacement + + @staticmethod + def _motion_sample_counts( + sample_count: int, + options: OperateArticulationOptions, + ) -> tuple[int, int, int, int]: + """Allocate the preset sample budget across four motion phases.""" + remaining = sample_count - options.engage_steps - options.release_steps + if remaining < 8: + raise ValueError( + "MotionPolicy.sample_count must leave at least two waypoints for " + "each articulation motion phase." + ) + base, remainder = divmod(remaining, 4) + counts = tuple(base + (1 if index < remainder else 0) for index in range(4)) + assert len(counts) == 4 and all(value >= 2 for value in counts) + return counts + + +__all__ = [ + "OperateArticulation", + "OperateArticulationGoal", + "OperateArticulationOptions", +] diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index a5037d661..5e199a4ab 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -32,7 +32,7 @@ quat_from_matrix, ) -from ._helpers import arm_qpos_from_state +from ._helpers import arm_qpos_from_state, require_shared_task_state_key from ..affordance import AntipodalAffordance from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand @@ -352,6 +352,11 @@ def _plan( grasp = binding.endpoint("primary", "grasp") manipulator = motion.require_target(JointPositionTarget) end_effector = grasp.require_target(JointPositionTarget) + task_state_key = require_shared_task_state_key( + motion, + grasp, + participant="PickUp primary participant", + ) hand_open_qpos = grasp.joint_positions( OPEN_COMMAND, n_envs=context.batch_size, @@ -441,7 +446,7 @@ def _plan( semantics=sem, object_to_eef=object_to_eef, grasp_xpos=grasp_xpos ) coordinated_updates = { - key: None for key in state.coordinated_held_objects if control_part in key + key: None for key in state.coordinated_held_objects if task_state_key in key } return self.build_plan( request, @@ -449,10 +454,15 @@ def _plan( success=success_mask, trajectory=full, expected_effects=StateDelta( - held_object_updates={control_part: held}, + held_object_updates={task_state_key: held}, coordinated_held_object_updates=coordinated_updates, ), segment_lengths=segment_lengths, + scene_dependency_monitor_until=( + {} + if sem.entity_id is None + else {sem.entity_id: segment_lengths["approach"]} + ), ) def _resolve_grasp_pose( diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index b4678dd02..7e65ea1c7 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -27,7 +27,11 @@ from embodichain.utils import logger from embodichain.utils.math import quat_error_magnitude, quat_from_matrix -from ._helpers import arm_qpos_from_state, resolve_object_target +from ._helpers import ( + arm_qpos_from_state, + require_shared_task_state_key, + resolve_object_target, +) from ..affordance import AssembleAffordance from ..bindings import JointPositionTarget from ..control import GRASP_COMMAND, OPEN_COMMAND, JointPositionCommand @@ -229,11 +233,15 @@ def _plan( target = self.require_goal(request) options = request.skill_options binding = request.binding - motion_target = binding.endpoint("primary", "motion").require_target( - JointPositionTarget - ) + motion = binding.endpoint("primary", "motion") grasp = binding.endpoint("primary", "grasp") + motion_target = motion.require_target(JointPositionTarget) grasp_target = grasp.require_target(JointPositionTarget) + task_state_key = require_shared_task_state_key( + motion, + grasp, + participant="Place primary participant", + ) control_part = motion_target.control_part arm_joint_ids = list(motion_target.joint_ids) hand_joint_ids = list(grasp_target.joint_ids) @@ -250,7 +258,7 @@ def _plan( dtype=context.robot.qpos.dtype, ) state = context - place_xpos = self._resolve_place_xpos(target, state, control_part) + place_xpos = self._resolve_place_xpos(target, state, task_state_key) if place_xpos.dim() == 3: place_xpos = place_xpos.unsqueeze(1) @@ -332,7 +340,7 @@ def _plan( full[:, n_down_actual + n_open :, hand_joint_ids] = hand_open_qpos.unsqueeze(1) coordinated_updates = { - key: None for key in state.coordinated_held_objects if control_part in key + key: None for key in state.coordinated_held_objects if task_state_key in key } return self.build_plan( request, @@ -340,7 +348,7 @@ def _plan( success=success, trajectory=full, expected_effects=StateDelta( - held_object_updates={control_part: None}, + held_object_updates={task_state_key: None}, coordinated_held_object_updates=coordinated_updates, ), segment_lengths={ @@ -354,13 +362,14 @@ def _resolve_place_xpos( self, target: PlaceGoal | AssembleGoal, state: PlanningContext, - control_part: str, + task_state_key: str, ) -> torch.Tensor: """Resolve the place EEF poses from a typed target. Args: target: Either an explicit EEF pose target or an assembly target. state: World state carrying the held-object transform. + task_state_key: Stable logical resource used for held-object state. Returns: Place EEF poses with shape ``(n_envs, 4, 4)`` or @@ -372,13 +381,13 @@ def _resolve_place_xpos( n_envs=self.n_envs, device=self.device, ) - return self._resolve_assemble_place_xpos(target, state, control_part) + return self._resolve_assemble_place_xpos(target, state, task_state_key) def _resolve_assemble_place_xpos( self, target: AssembleGoal, state: PlanningContext, - control_part: str, + task_state_key: str, ) -> torch.Tensor: """Derive the place EEF pose from an assembly affordance. @@ -389,6 +398,7 @@ def _resolve_assemble_place_xpos( Args: target: Assembly target carrying the base/assemble affordance. state: World state carrying the held-object transform. + task_state_key: Stable logical resource used for held-object state. Returns: Place EEF poses with shape ``(n_envs, 4, 4)``. @@ -396,11 +406,11 @@ def _resolve_assemble_place_xpos( Raises: ValueError: If no held object or base-pose source is available. """ - held = state.get_held_object(control_part) + held = state.get_held_object(task_state_key) if held is None: logger.log_error( - "Place with AssembleGoal requires an object held by control " - f"part {control_part!r} (run PickUp first).", + "Place with AssembleGoal requires an object held by task-state " + f"resource {task_state_key!r} (run PickUp first).", ValueError, ) affordance = target.affordance diff --git a/embodichain/lab/sim/atomic_actions/runtime.py b/embodichain/lab/sim/atomic_actions/runtime.py index c0530db40..13228ff37 100644 --- a/embodichain/lab/sim/atomic_actions/runtime.py +++ b/embodichain/lab/sim/atomic_actions/runtime.py @@ -98,12 +98,25 @@ def bind_control_parts( self, contract: SkillBindingContract, endpoints: Mapping[str, Mapping[str, str]], + *, + task_state_keys: Mapping[str, str] | None = None, ) -> ActionBinding: """Build a generic binding from explicit robot control-part names. This is the advanced direct-core construction path. Profile-backed callers obtain the same :class:`ActionBinding` from ``BoundRobotSkillProfile.resolve()``. + + Args: + contract: Typed endpoint contract for the bound skill. + endpoints: Nested ``slot_id -> endpoint_id -> control_part`` mapping. + task_state_keys: Optional explicit stable task-state key for each + resource slot. When omitted, a slot inherits its ``motion`` + endpoint's control part. A slot without ``motion`` can be + inferred only when all of its endpoints use one control part. + + Returns: + Engine-owned generic endpoint binding. """ if not isinstance(contract, SkillBindingContract): raise TypeError("contract must be a SkillBindingContract.") @@ -138,6 +151,36 @@ def bind_control_parts( "Direct binding must cover the skill contract exactly: " f"missing={missing}, extra={extra}." ) + slot_ids = {slot.slot_id for slot in contract.slots} + if task_state_keys is not None: + if not isinstance(task_state_keys, Mapping): + raise TypeError("task_state_keys must be a slot-to-key mapping.") + for slot_id, task_state_key in task_state_keys.items(): + if ( + not isinstance(slot_id, str) + or not slot_id + or slot_id != slot_id.strip() + ): + raise ValueError( + "task_state_keys slot IDs must be non-empty strings " + "without outer whitespace." + ) + if not isinstance(task_state_key, str) or not task_state_key.strip(): + raise ValueError( + "task_state_keys values must be non-empty strings." + ) + if task_state_key != task_state_key.strip(): + raise ValueError( + "task_state_keys values must not contain outer whitespace." + ) + supplied_task_slots = set(task_state_keys) + if supplied_task_slots != slot_ids: + missing = sorted(slot_ids - supplied_task_slots) + extra = sorted(supplied_task_slots - slot_ids) + raise ValueError( + "task_state_keys must cover the binding slots exactly: " + f"missing={missing}, extra={extra}." + ) if not expected: binding = ActionBinding(owner_id=self.binding_owner_id) self.validate_binding(binding, contract) @@ -147,6 +190,27 @@ def bind_control_parts( if not isinstance(control_parts, Mapping): raise TypeError("Direct control-part binding requires Robot.control_parts.") available = sorted(str(name) for name in control_parts) + resolved_task_state_keys: dict[str, str] + if task_state_keys is not None: + resolved_task_state_keys = dict(task_state_keys) + else: + resolved_task_state_keys = {} + for slot in contract.slots: + motion_key = (slot.slot_id, "motion") + if motion_key in supplied: + resolved_task_state_keys[slot.slot_id] = supplied[motion_key] + continue + slot_control_parts = { + supplied[(slot.slot_id, endpoint.endpoint_id)] + for endpoint in slot.endpoints + } + if len(slot_control_parts) != 1: + raise ValueError( + f"Direct binding slot {slot.slot_id!r} has no 'motion' " + "endpoint and spans multiple control parts; provide an " + "explicit task_state_keys entry for this slot." + ) + resolved_task_state_keys[slot.slot_id] = next(iter(slot_control_parts)) resolved: list[EndpointBinding] = [] for key, requirement in expected.items(): slot_id, endpoint_id = key @@ -175,6 +239,7 @@ def bind_control_parts( resource_id=f"direct.{slot_id}", adapter_id="control_part", target=JointPositionTarget(control_part, joint_ids), + task_state_key=resolved_task_state_keys[slot_id], capabilities=requirement.capabilities, commands=commands, claim_tokens=frozenset({f"robot.control_part:{control_part}"}), diff --git a/embodichain/lab/sim/atomic_actions/state.py b/embodichain/lab/sim/atomic_actions/state.py index 69cf8d044..578a43d86 100644 --- a/embodichain/lab/sim/atomic_actions/state.py +++ b/embodichain/lab/sim/atomic_actions/state.py @@ -92,6 +92,80 @@ def _broadcast_pose( return value.clone() +def _broadcast_joint_position( + value: torch.Tensor, + *, + batch_size: int, + device: torch.device, + name: str, +) -> torch.Tensor: + """Resolve an optionally batched joint-position value to a task batch.""" + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dim() == 1: + if value.numel() == 0: + raise ValueError(f"{name} must contain at least one joint value.") + value = value.unsqueeze(0).expand(batch_size, -1) + elif value.dim() != 2 or value.shape[0] != batch_size or value.shape[1] == 0: + raise ValueError( + f"{name} must have shape (n_joints,) or " f"({batch_size}, n_joints)." + ) + if not value.is_floating_point(): + raise TypeError(f"{name} must use a floating-point dtype.") + if value.device != device: + raise ValueError(f"{name} must use task-state device {device}.") + if not torch.isfinite(value).all(): + raise ValueError(f"{name} must contain only finite values.") + return value.clone() + + +@dataclass(frozen=True, slots=True, eq=False) +class ArticulationJointState: + """Verified symbolic state for one named articulation joint. + + ``position`` may describe one scalar joint or a multi-DoF joint. The + surrounding :class:`TaskState` supplies the stable articulation/joint key; + this value only owns row-local verified measurements and activity. + """ + + position: torch.Tensor + """Joint positions with shape ``(J,)`` or ``(B, J)``.""" + + env_mask: torch.Tensor | None = None + """Rows for which the verified state is present.""" + + def __post_init__(self) -> None: + if not isinstance(self.position, torch.Tensor): + raise TypeError("ArticulationJointState.position must be a tensor.") + if self.position.dim() not in (1, 2) or self.position.numel() == 0: + raise ValueError( + "ArticulationJointState.position must have shape (J,) or (B, J)." + ) + if not self.position.is_floating_point(): + raise TypeError("ArticulationJointState.position must be floating point.") + if not torch.isfinite(self.position).all(): + raise ValueError("ArticulationJointState.position must be finite.") + object.__setattr__(self, "position", self.position.clone()) + if self.env_mask is not None: + batch_size = int(self.position.shape[0]) if self.position.dim() == 2 else -1 + if batch_size <= 0: + if self.env_mask.dim() != 1 or self.env_mask.numel() == 0: + raise ValueError( + "ArticulationJointState.env_mask must be a non-empty vector." + ) + batch_size = int(self.env_mask.shape[0]) + object.__setattr__( + self, + "env_mask", + _normalize_mask( + self.env_mask, + batch_size=batch_size, + device=self.position.device, + name="ArticulationJointState.env_mask", + ), + ) + + @dataclass(frozen=True, slots=True, eq=False) class HeldObjectState: """Observed or projected relation between an object and one manipulator.""" @@ -252,6 +326,29 @@ def _normalize_coordinated_held( ) +def _normalize_articulation_joint( + value: ArticulationJointState, + *, + batch_size: int, + device: torch.device, +) -> ArticulationJointState: + """Normalize one articulation-joint state to a task-state batch.""" + return ArticulationJointState( + position=_broadcast_joint_position( + value.position, + batch_size=batch_size, + device=device, + name="ArticulationJointState.position", + ), + env_mask=_normalize_mask( + value.env_mask, + batch_size=batch_size, + device=device, + name="ArticulationJointState.env_mask", + ), + ) + + @dataclass(frozen=True, slots=True, eq=False) class TaskState: """Symbolic task state, separate from measured robot state.""" @@ -263,12 +360,17 @@ class TaskState: """Device used by per-environment masks and relation tensors.""" held_objects: Mapping[str, HeldObjectState] = field(default_factory=dict) - """Single-manipulator held-object relations keyed by control resource.""" + """Held-object relations keyed by stable logical task-state resource.""" coordinated_held_objects: Mapping[tuple[str, str], CoordinatedHeldObjectState] = ( field(default_factory=dict) ) - """Two-manipulator held-object relations keyed by ordered resource pairs.""" + """Coordinated relations keyed by ordered logical task-state resource pairs.""" + + articulation_joints: Mapping[tuple[str, str], ArticulationJointState] = field( + default_factory=dict + ) + """Verified articulation states keyed by canonical articulation and joint IDs.""" def __post_init__(self) -> None: if self.batch_size <= 0: @@ -303,6 +405,30 @@ def __post_init__(self) -> None: value, batch_size=self.batch_size, device=device ) + normalized_articulation: dict[tuple[str, str], ArticulationJointState] = {} + for key, value in self.articulation_joints.items(): + if ( + not isinstance(key, tuple) + or len(key) != 2 + or not all( + type(item) is str and item and item == item.strip() for item in key + ) + ): + raise TypeError( + "articulation_joints keys must be pairs of non-empty " + "canonical identifiers." + ) + if not isinstance(value, ArticulationJointState): + raise TypeError( + "articulation_joints values must be ArticulationJointState " + "objects." + ) + normalized_articulation[key] = _normalize_articulation_joint( + value, + batch_size=self.batch_size, + device=device, + ) + object.__setattr__(self, "device", device) object.__setattr__(self, "held_objects", MappingProxyType(normalized_held)) object.__setattr__( @@ -310,6 +436,11 @@ def __post_init__(self) -> None: "coordinated_held_objects", MappingProxyType(normalized_coordinated), ) + object.__setattr__( + self, + "articulation_joints", + MappingProxyType(normalized_articulation), + ) @classmethod def empty( @@ -340,6 +471,14 @@ def get_coordinated_held_object( """Return the relation for an ordered resource pair, if any.""" return self.coordinated_held_objects.get((first_resource, second_resource)) + def get_articulation_joint_state( + self, + articulation_id: str, + joint_id: str, + ) -> ArticulationJointState | None: + """Return verified state for one canonical articulation joint.""" + return self.articulation_joints.get((articulation_id, joint_id)) + @dataclass(frozen=True, slots=True, eq=False) class RobotObservation: @@ -424,6 +563,70 @@ def __post_init__(self) -> None: object.__setattr__(self, "pose", self.pose.clone()) +@dataclass(frozen=True, slots=True, eq=False) +class ObservedArticulationJointState: + """Live measured state for one scene articulation joint. + + This value belongs to :class:`SceneSnapshot`, not :class:`TaskState`. + ``ArticulationJointState`` records a verified symbolic effect after an + operation, while this class records the physical position used by online + grounding and recovery replans. + """ + + position: torch.Tensor + """Measured joint position with shape ``(J,)`` or ``(B, J)``.""" + + valid_mask: torch.Tensor | None = None + """Optional row-validity mask for a batched observation.""" + + def __post_init__(self) -> None: + position = self.position + if not isinstance(position, torch.Tensor): + raise TypeError("ObservedArticulationJointState.position must be a tensor.") + if position.dim() not in (1, 2) or position.numel() == 0: + raise ValueError( + "ObservedArticulationJointState.position must have shape (J,) " + "or (B, J)." + ) + if not position.is_floating_point(): + raise TypeError( + "ObservedArticulationJointState.position must be floating point." + ) + if not torch.isfinite(position).all(): + raise ValueError( + "ObservedArticulationJointState.position must contain only " + "finite values." + ) + object.__setattr__(self, "position", position.clone()) + if self.valid_mask is None: + return + valid_mask = self.valid_mask + if not isinstance(valid_mask, torch.Tensor): + raise TypeError( + "ObservedArticulationJointState.valid_mask must be a tensor or None." + ) + if position.dim() != 2: + raise ValueError( + "ObservedArticulationJointState.valid_mask requires a batched " + "position." + ) + if valid_mask.dtype != torch.bool or valid_mask.shape != (position.shape[0],): + raise ValueError( + "ObservedArticulationJointState.valid_mask must have shape (B,) " + "and dtype torch.bool." + ) + if valid_mask.device != position.device: + raise ValueError( + "ObservedArticulationJointState position and valid_mask must " + "share a device." + ) + object.__setattr__(self, "valid_mask", valid_mask.clone()) + + def snapshot(self) -> ObservedArticulationJointState: + """Return an independently owned observation value.""" + return ObservedArticulationJointState(self.position, self.valid_mask) + + class _ImmutableEntityMapping(Mapping[str, EntityState]): """Own entity states and return defensive copies on every public read.""" @@ -448,6 +651,34 @@ def __len__(self) -> int: return len(self._states) +class _ImmutableObservedArticulationJointMapping( + Mapping[tuple[str, str], ObservedArticulationJointState] +): + """Own live joint observations and copy values on every public read.""" + + __slots__ = ("_states",) + + def __init__( + self, + states: Mapping[tuple[str, str], ObservedArticulationJointState], + ) -> None: + self._states = MappingProxyType( + {key: state.snapshot() for key, state in states.items()} + ) + + def __getitem__( + self, + key: tuple[str, str], + ) -> ObservedArticulationJointState: + return self._states[key].snapshot() + + def __iter__(self) -> Iterator[tuple[str, str]]: + return iter(self._states) + + def __len__(self) -> int: + return len(self._states) + + @dataclass(frozen=True, slots=True, eq=False) class SceneSnapshot: """Versioned scene state used to ground dynamic goals and obstacles.""" @@ -461,6 +692,11 @@ class SceneSnapshot: collision_entity_ids: tuple[str, ...] = () """Entity IDs whose poses update a planner's dynamic collision world.""" + articulation_joints: Mapping[tuple[str, str], ObservedArticulationJointState] = ( + field(default_factory=dict) + ) + """Live physical joint observations keyed by articulation and joint ID.""" + def __post_init__(self) -> None: if self.timestamp < 0.0: raise ValueError("SceneSnapshot.timestamp must be non-negative.") @@ -498,6 +734,28 @@ def __post_init__(self) -> None: "SceneSnapshot entities must contain EntityState values." ) normalized[entity_id] = state + normalized_joints: dict[tuple[str, str], ObservedArticulationJointState] = {} + for key, state in self.articulation_joints.items(): + if ( + not isinstance(key, tuple) + or len(key) != 2 + or not all( + type(identifier) is str + and identifier + and identifier == identifier.strip() + for identifier in key + ) + ): + raise TypeError( + "SceneSnapshot articulation_joints keys must be canonical " + "(articulation_id, joint_id) pairs." + ) + if not isinstance(state, ObservedArticulationJointState): + raise TypeError( + "SceneSnapshot articulation_joints values must be " + "ObservedArticulationJointState objects." + ) + normalized_joints[key] = state collision_entity_ids = tuple(self.collision_entity_ids) if len(set(collision_entity_ids)) != len(collision_entity_ids) or not all( isinstance(entity_id, str) and entity_id @@ -513,8 +771,29 @@ def __post_init__(self) -> None: f"{sorted(missing)}." ) object.__setattr__(self, "entities", _ImmutableEntityMapping(normalized)) + object.__setattr__( + self, + "articulation_joints", + _ImmutableObservedArticulationJointMapping(normalized_joints), + ) object.__setattr__(self, "collision_entity_ids", collision_entity_ids) + def get_articulation_joint_state( + self, + articulation_id: str, + joint_id: str, + ) -> ObservedArticulationJointState | None: + """Return an owned live joint observation for a canonical address.""" + for value, field_name in ( + (articulation_id, "articulation_id"), + (joint_id, "joint_id"), + ): + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{field_name} must be a non-empty canonical identifier." + ) + return self.articulation_joints.get((articulation_id, joint_id)) + def collision_world_revisions(self, batch_size: int) -> tuple[int, ...]: """Expand the collision revision to one value per environment. @@ -604,6 +883,18 @@ def __post_init__(self) -> None: f"Scene entity {entity_id!r} pose batch must match the " "planning context." ) + for ( + articulation_id, + joint_id, + ), state in self.scene.articulation_joints.items(): + if ( + state.position.dim() == 2 + and state.position.shape[0] != self.robot.batch_size + ): + raise ValueError( + f"Scene articulation joint ({articulation_id!r}, {joint_id!r}) " + "position batch must match the planning context." + ) if not isinstance(self.env_ids, torch.Tensor): raise TypeError("env_ids must be a torch.Tensor.") if self.env_ids.dtype != torch.long: @@ -653,6 +944,21 @@ def get_coordinated_held_object( """Return a coordinated held-object relation, if any.""" return self.task.get_coordinated_held_object(first_resource, second_resource) + @property + def articulation_joints( + self, + ) -> Mapping[tuple[str, str], ArticulationJointState]: + """Verified articulation-joint states.""" + return self.task.articulation_joints + + def get_articulation_joint_state( + self, + articulation_id: str, + joint_id: str, + ) -> ArticulationJointState | None: + """Return verified state for one canonical articulation joint.""" + return self.task.get_articulation_joint_state(articulation_id, joint_id) + def project( self, *, @@ -677,9 +983,11 @@ def project( __all__ = [ + "ArticulationJointState", "CoordinatedHeldObjectState", "EntityState", "HeldObjectState", + "ObservedArticulationJointState", "PlanningContext", "RobotObservation", "SceneSnapshot", diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 239258b13..d9128a149 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -1488,6 +1488,15 @@ def set_qf( data_type=ArticulationGPUAPIWriteType.JOINT_FORCE, ) + def get_qf(self) -> torch.Tensor: + """Get the current generalized efforts (qf) of the articulation. + + Returns: + torch.Tensor: Joint efforts with shape (N, dof), where N is the + number of environments. + """ + return self.body_data.qf + def get_qf_limits( self, joint_ids: Sequence[int] | torch.Tensor | None = None, diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 3cfa32029..b01d1891e 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -18,6 +18,7 @@ from __future__ import annotations +from dataclasses import replace from typing import TypeVar from unittest.mock import Mock @@ -61,6 +62,10 @@ MoveJoints, MoveJointsOptions, ObjectSemantics, + ObservedArticulationJointState, + OperateArticulation, + OperateArticulationGoal, + OperateArticulationOptions, PickUp, PickUpOptions, Place, @@ -71,6 +76,7 @@ PressGoal, PressOptions, RobotObservation, + SceneArticulationOperationGeometry, SceneEntityPose, SceneSnapshot, TaskState, @@ -256,16 +262,54 @@ def _target_scene( ) +def _articulation_geometry() -> SceneArticulationOperationGeometry: + """Build late-bound identity handle geometry for atomic tests.""" + identity = torch.eye(4) + return SceneArticulationOperationGeometry( + handle_pose=SceneEntityPose("drawer_handle"), + approach_offset=identity, + contact_offset=identity, + operation_offset=identity, + retract_offset=identity, + operation_axis=torch.tensor((1.0, 0.0, 0.0)), + ) + + +def _articulation_scene( + position: torch.Tensor, + *, + handle_x: float = 0.0, + timestamp: float = 0.0, + version: int = 0, +) -> SceneSnapshot: + """Build one live handle and articulation-joint snapshot.""" + handle = torch.eye(4).repeat(NUM_ENVS, 1, 1) + handle[:, 0, 3] = handle_x + return SceneSnapshot( + timestamp=timestamp, + version=version, + entities={"drawer_handle": EntityState(handle)}, + articulation_joints={ + ("drawer", "slide"): ObservedArticulationJointState(position) + }, + ) + + def _binding( action: AtomicAction, *, motion: str = "arm", grasp: str = "hand", + task_state_key: str | None = None, ) -> ActionBinding: """Bind one single-participant action through its owning engine.""" contract = type(action).__dict__.get("binding_contract") assert contract is not None - endpoint_parts = {"motion": motion, "grasp": grasp} + endpoint_parts = { + "motion": motion, + "grasp": grasp, + "interaction": grasp, + } return _ACTION_ENGINES[id(action)].bind_control_parts( action.skill_id, { @@ -275,6 +319,11 @@ def _binding( } for slot in contract.slots }, + task_state_keys=( + None + if task_state_key is None + else {slot.slot_id: task_state_key for slot in contract.slots} + ), ) @@ -441,6 +490,8 @@ def _dual_binding( action: AtomicAction, first_slot: str, second_slot: str, + *, + task_state_keys: dict[str, str] | None = None, ) -> ActionBinding: return _ACTION_ENGINES[id(action)].bind_control_parts( action.skill_id, @@ -454,6 +505,7 @@ def _dual_binding( "grasp": "right_hand", }, }, + task_state_keys=task_state_keys, ) @@ -472,6 +524,85 @@ def _sample(obj_poses: torch.Tensor, **_kwargs: object) -> list[dict]: affordance.get_dual_arm_valid_grasp_poses = Mock(side_effect=_sample) +def _plan_segment_contract_case(case_id: str) -> ActionPlan: + """Plan one built-in used by the Version 1 trajectory-segment contract.""" + generator = _motion_generator() + sample_count = 20 + + if case_id == "move_joints": + action = _bind_action(generator, MoveJoints()) + goal = JointPositionGoal(torch.zeros(ARM_DOF)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(), + ) + + if case_id == "move_end_effector": + action = _bind_action(generator, MoveEndEffector()) + goal = EndEffectorPoseGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(), + ) + + held_task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"arm": _held()}, + ) + if case_id == "move_held_object": + action = _bind_action(generator, MoveHeldObject()) + goal = HeldObjectPoseGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(held_task), + ) + + if case_id == "place": + action = _bind_action(generator, Place()) + goal = PlaceGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(held_task), + ) + + if case_id == "assemble": + action = _bind_action(generator, Place()) + goal = AssembleGoal( + affordance=AssembleAffordance( + base_object_entity=Mock(), + assemble_to_base_pose=torch.eye(4), + ), + base_pose=SceneEntityPose("base"), + ) + base_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"base": EntityState(base_pose)}, + ) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(held_task, scene=scene), + ) + + if case_id == "press": + action = _bind_action(generator, Press()) + goal = PressGoal(torch.eye(4)) + return _plan_action( + action, + _invocation(action, goal, sample_count=sample_count), + _context(), + ) + + raise AssertionError(f"Unknown trajectory-segment contract case {case_id!r}.") + + def test_builtin_descriptors_expose_goals_not_legacy_targets() -> None: assert MoveEndEffector.GoalType is EndEffectorPoseGoal assert MoveJoints.GoalType is JointPositionGoal @@ -482,6 +613,34 @@ def test_builtin_descriptors_expose_goals_not_legacy_targets() -> None: assert CoordinatedPickment.GoalType is CoordinatedPickGoal assert CoordinatedPlacement.GoalType is CoordinatedPlacementGoal assert HandOver.GoalType is GraspGoal + assert OperateArticulation.GoalType is OperateArticulationGoal + + +@pytest.mark.parametrize( + ("case_id", "expected_names"), + ( + ("move_joints", ("move_joints",)), + ("move_end_effector", ("move_end_effector",)), + ("move_held_object", ("transport",)), + ("place", ("approach", "release", "retract")), + ("assemble", ("approach", "release", "retract")), + ("press", ("close", "press", "retract")), + ), +) +def test_builtin_trajectory_segment_names_and_ranges_are_stable( + case_id: str, + expected_names: tuple[str, ...], +) -> None: + plan = _plan_segment_contract_case(case_id) + + assert plan.success_all + assert tuple(segment.name for segment in plan.segments) == expected_names + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count @pytest.mark.parametrize( @@ -494,6 +653,7 @@ def test_builtin_descriptors_expose_goals_not_legacy_targets() -> None: CoordinatedPickmentOptions(), CoordinatedPlacementOptions(), HandOverOptions(), + OperateArticulationOptions(), ), ) def test_action_options_do_not_contain_embodiment_resources(options: object) -> None: @@ -612,13 +772,18 @@ def test_pick_and_place_declare_effects_without_mutating_context() -> None: pick_plan = _plan_action( pick, - _invocation(pick, GraspGoal(semantics=semantics, grasp_xpos=grasp)), + ActionInvocation( + skill_id="pick_up", + goal=GraspGoal(semantics=semantics, grasp_xpos=grasp), + binding=_binding(pick, task_state_key="logical_arm"), + ), initial, ) picked_task = pick_plan.expected_effects.apply(initial.task, pick_plan.plan_success) - assert initial.task.get_held_object("arm") is None - assert picked_task.get_held_object("arm") is not None + assert initial.task.get_held_object("logical_arm") is None + assert picked_task.get_held_object("logical_arm") is not None + assert picked_task.get_held_object("arm") is None place = _bind_action(generator, Place()) picked_context = PlanningContext( @@ -629,15 +794,19 @@ def test_pick_and_place_declare_effects_without_mutating_context() -> None: ) place_plan = _plan_action( place, - _invocation(place, PlaceGoal(torch.eye(4))), + ActionInvocation( + skill_id="place", + goal=PlaceGoal(torch.eye(4)), + binding=_binding(place, task_state_key="logical_arm"), + ), picked_context, ) placed_task = place_plan.expected_effects.apply( picked_task, place_plan.plan_success ) - assert picked_task.get_held_object("arm") is not None - assert placed_task.get_held_object("arm") is None + assert picked_task.get_held_object("logical_arm") is not None + assert placed_task.get_held_object("logical_arm") is None def test_move_held_object_requires_projected_attachment() -> None: @@ -648,6 +817,10 @@ def test_move_held_object_requires_projected_attachment() -> None: HeldObjectPoseGoal(torch.eye(4)), sample_count=10, ) + invocation = replace( + invocation, + binding=_binding(action, task_state_key="logical_arm"), + ) with pytest.raises(ValueError, match="requires an object held"): _plan_action(action, invocation, _context()) @@ -661,7 +834,7 @@ def test_move_held_object_requires_projected_attachment() -> None: task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"arm": held}, + held_objects={"logical_arm": held}, ) eef_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) eef_pose[:, :3, :3] = torch.tensor( @@ -674,7 +847,7 @@ def test_move_held_object_requires_projected_attachment() -> None: configured_invocation = ActionInvocation( skill_id="move_held_object", goal=HeldObjectPoseGoal(torch.eye(4)), - binding=_binding(action), + binding=_binding(action, task_state_key="logical_arm"), motion_policy=MotionPolicy(sample_count=10), skill_options=MoveHeldObjectOptions(pick_rotate_upright=0.25), ) @@ -683,6 +856,7 @@ def test_move_held_object_requires_projected_attachment() -> None: assert plan.plan_success.all() assert plan.expected_effects.is_empty + assert generator.robot.compute_fk.call_args.kwargs["name"] == "arm" current_object_pose = action._apply_configured_upright_rotation.call_args.args[2] assert torch.allclose( current_object_pose, @@ -706,6 +880,191 @@ def test_press_uses_invocation_sample_budget() -> None: assert plan.expected_effects.is_empty +def test_operate_articulation_builds_named_verified_interaction() -> None: + generator = _motion_generator() + action = _bind_action( + generator, + OperateArticulation( + OperateArticulationOptions(engage_steps=2, release_steps=2) + ), + ) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + + plan = _plan_action( + action, + _invocation(action, goal, sample_count=16), + _context(scene=_articulation_scene(torch.tensor([0.0]))), + ) + + assert plan.plan_success.tolist() == [True, True] + assert plan.commands.frame_count == 16 + assert tuple(segment.name for segment in plan.segments) == ( + "approach", + "engage", + "operate", + "release", + "retract", + ) + assert plan.requires_effect_verification + assert plan.scene_dependency_monitor_until == { + "drawer_handle": plan.segment("operate").start + } + assert plan.effect_verification is not None + assert plan.effect_verification.kind == "articulation.joint_progress" + update = plan.expected_effects.articulation_joint_updates[("drawer", "slide")] + assert update is not None + assert torch.equal(update.position, torch.tensor([0.4])) + interaction = _joint_command_positions(plan, "hand") + assert torch.all(interaction[:, -1] == 0.0) + assert torch.any(interaction == 1.0) + + +def test_operate_articulation_reports_per_phase_planning_failures() -> None: + generator = _motion_generator() + action = _bind_action( + generator, + OperateArticulation( + OperateArticulationOptions(engage_steps=2, release_steps=2) + ), + ) + phase_results = [] + for index in range(4): + phase_results.append( + PlanResult( + success=torch.tensor([index != 2, True]), + positions=torch.zeros(NUM_ENVS, 3, ARM_DOF), + ) + ) + generator.generate = Mock(side_effect=phase_results) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + + plan = _plan_action( + action, + _invocation(action, goal, sample_count=16), + _context(scene=_articulation_scene(torch.tensor([0.0]))), + ) + + assert plan.plan_success.tolist() == [False, True] + assert plan.diagnostics.messages == ( + "Articulation motion phase 'operate' failed for rows [0].", + ) + phases = plan.diagnostics.metadata["motion_phases"] + assert phases["operate"] == { + "success": [False, True], + "failed_rows": [0], + "waypoint_count": 3, + } + + +def test_operate_articulation_replan_uses_fresh_handle_and_remaining_stroke() -> None: + generator = _motion_generator() + action = _bind_action( + generator, + OperateArticulation( + OperateArticulationOptions(engage_steps=2, release_steps=2) + ), + ) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([[0.0], [0.0]]), + target_position=torch.tensor([[0.4], [0.4]]), + target_displacement=0.4, + ) + invocation = _invocation(action, goal, sample_count=16) + initial_context = _context( + scene=_articulation_scene( + torch.tensor([[0.0], [0.0]]), + handle_x=0.3, + ) + ) + session = _ACTION_ENGINES[id(action)].start((invocation,), initial_context) + first_operation = generator.robot.compute_ik.call_args_list[2].kwargs["pose"] + assert torch.allclose(first_operation[:, 0, 3], torch.tensor([0.7, 0.7])) + session.tick(initial_context) + + generator.robot.compute_ik.reset_mock() + recovered = session.tick( + _context( + scene=_articulation_scene( + torch.tensor([[0.2], [0.4]]), + handle_x=0.55, + timestamp=1.0, + version=1, + ), + timestamp=1.0, + ), + ) + recovered_operation = generator.robot.compute_ik.call_args_list[2].kwargs["pose"] + + assert torch.allclose(recovered_operation[:, 0, 3], torch.tensor([0.75, 0.55])) + event_kinds = {event.kind for event in recovered.events} + assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED in event_kinds + assert ExecutionEventKind.REPLANNED in event_kinds + assert session.trajectory_segment("operate").name == "operate" + + +def test_operate_articulation_requires_live_joint_observation() -> None: + generator = _motion_generator() + action = _bind_action(generator, OperateArticulation()) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + handle = torch.eye(4).repeat(NUM_ENVS, 1, 1) + scene = SceneSnapshot( + timestamp=0.0, + version=0, + entities={"drawer_handle": EntityState(handle)}, + ) + + with pytest.raises(ValueError, match="ObservedArticulationJointState"): + _plan_action( + action, + _invocation(action, goal, sample_count=20), + _context(scene=scene), + ) + + +def test_operate_articulation_rejects_insufficient_motion_budget() -> None: + generator = _motion_generator() + action = _bind_action(generator, OperateArticulation()) + goal = OperateArticulationGoal( + articulation_id="drawer", + joint_id="slide", + geometry=_articulation_geometry(), + source_position=torch.tensor([0.0]), + target_position=torch.tensor([0.4]), + target_displacement=0.4, + ) + + with pytest.raises(ValueError, match="at least two waypoints"): + _plan_action( + action, + _invocation(action, goal, sample_count=17), + _context(scene=_articulation_scene(torch.tensor([0.0]))), + ) + + def test_strategy_and_sample_count_are_not_action_config_fields() -> None: with pytest.raises(TypeError): MoveEndEffectorOptions(strategy="motion_gen") # type: ignore[call-arg] @@ -877,6 +1236,9 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: "lift", ] assert plan.segment("close").stop == plan.segment("lift").start + assert plan.scene_dependency_monitor_until == { + "target": plan.segment("close").start + } def test_pick_holds_only_environment_without_a_feasible_grasp() -> None: @@ -1024,7 +1386,91 @@ def test_pick_session_replans_when_late_bound_target_moves() -> None: assert ExecutionEventKind.REPLANNED in event_kinds -def test_pick_uses_binding_control_part_as_effect_resource() -> None: +@pytest.mark.parametrize( + ("waypoint_offset", "expects_replan"), + ((-1, True), (0, False)), +) +def test_pick_scene_monitoring_window_is_exclusive_at_close_boundary( + waypoint_offset: int, + expects_replan: bool, +) -> None: + """External motion replans before close, while grasp-induced motion does not.""" + generator = _motion_generator() + action = _bind_action(generator, PickUp()) + engine = _ACTION_ENGINES[id(action)] + initial_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + moved_pose = initial_pose.clone() + moved_pose[:, 1, 3] = 0.3 + invocation = _invocation( + action, + GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ), + sample_count=20, + ) + task_state = TaskState.empty(batch_size=NUM_ENVS, device="cpu") + qpos = torch.zeros(NUM_ENVS, ROBOT_DOF) + + def context_at( + pose: torch.Tensor, + *, + timestamp: float, + version: int, + ) -> PlanningContext: + return PlanningContext( + robot=RobotObservation( + timestamp=timestamp, + qpos=qpos, + qvel=torch.zeros_like(qpos), + ), + task=task_state, + scene=_target_scene(pose, timestamp=timestamp, version=version), + env_ids=torch.arange(NUM_ENVS), + ) + + session = engine.start( + (invocation,), context_at(initial_pose, timestamp=0.0, version=0) + ) + tick = session.tick(context_at(initial_pose, timestamp=0.0, version=0)) + close_start = session.plan_attempts[0].plan.segment("close").start + commands_to_issue = close_start + waypoint_offset + issued = 1 + while issued < commands_to_issue: + assert tick.command is not None + for command in tick.command.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + qpos[:, list(command.target.joint_ids)] = command.payload.positions + tick = session.tick( + context_at( + initial_pose, + timestamp=0.04 * issued, + version=0, + ) + ) + issued += 1 + + assert tick.command is not None + for command in tick.command.commands: + assert isinstance(command.target, JointPositionTarget) + assert isinstance(command.payload, JointPositionPayload) + qpos[:, list(command.target.joint_ids)] = command.payload.positions + moved = session.tick( + context_at( + moved_pose, + timestamp=0.04 * commands_to_issue, + version=1, + ) + ) + + event_kinds = {event.kind for event in moved.events} + assert (ExecutionEventKind.DYNAMIC_GOAL_CHANGED in event_kinds) is expects_replan + assert (ExecutionEventKind.REPLANNED in event_kinds) is expects_replan + assert len(session.plan_attempts) == (2 if expects_replan else 1) + + +def test_pick_uses_logical_task_state_key_and_physical_control_target() -> None: generator = _motion_generator() action = _bind_action(generator, PickUp()) invocation = ActionInvocation( @@ -1037,6 +1483,7 @@ def test_pick_uses_binding_control_part_as_effect_resource() -> None: action, motion="alternate_arm", grasp="alternate_hand", + task_state_key="logical_picker", ), motion_policy=MotionPolicy(sample_count=20), ) @@ -1052,8 +1499,74 @@ def test_pick_uses_binding_control_part_as_effect_resource() -> None: plan = action.plan(request, context) projected = plan.expected_effects.apply(context.task, plan.plan_success) - assert projected.get_held_object("alternate_arm") is not None + assert projected.get_held_object("logical_picker") is not None + assert projected.get_held_object("alternate_arm") is None assert projected.get_held_object("arm") is None + assert {target.target_id for target in plan.commands.targets} == { + "alternate_arm", + "alternate_hand", + } + + +def test_participant_motion_and_grasp_must_share_task_state_key() -> None: + generator = _motion_generator() + action = _bind_action(generator, PickUp()) + binding = _binding(action) + mismatched = ActionBinding( + owner_id=binding.owner_id, + endpoints=tuple( + ( + replace(endpoint, task_state_key="other_participant") + if endpoint.endpoint_id == "grasp" + else endpoint + ) + for endpoint in binding.endpoints + ), + ) + invocation = ActionInvocation( + skill_id="pick_up", + goal=GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ), + binding=mismatched, + ) + context = _context( + scene=_target_scene( + torch.eye(4).repeat(NUM_ENVS, 1, 1), + timestamp=0.0, + version=0, + ) + ) + + with pytest.raises(ValueError, match="must share one task_state_key"): + _plan_action(action, invocation, context) + + +def test_handover_participants_must_use_distinct_task_state_keys() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + HandOver( + default_options=HandOverOptions( + middle_object_pose=torch.eye(4), + final_object_pose=torch.eye(4), + ) + ), + ) + invocation = ActionInvocation( + skill_id="hand_over", + goal=GraspGoal(semantics=_semantics()), + binding=_dual_binding( + action, + "source", + "destination", + task_state_keys={"source": "same", "destination": "same"}, + ), + ) + + with pytest.raises(ValueError, match="different task_state_key"): + _plan_action(action, invocation, _dual_context()) def test_press_closes_hand_without_changing_projected_attachment() -> None: @@ -1083,13 +1596,26 @@ def test_press_closes_hand_without_changing_projected_attachment() -> None: assert torch.equal(projected_held.object_to_eef, held.object_to_eef) -def test_handover_does_not_mutate_cached_final_pose() -> None: +@pytest.mark.parametrize( + ("hold_steps", "expected_segments"), + ( + (0, ("transfer", "approach", "close", "release", "deliver")), + ( + 2, + ("transfer", "approach", "close", "hold", "release", "deliver"), + ), + ), +) +def test_handover_does_not_mutate_cached_final_pose_and_omits_empty_hold( + hold_steps: int, + expected_segments: tuple[str, ...], +) -> None: generator = _dual_motion_generator() handover_options = HandOverOptions( middle_object_pose=torch.eye(4), final_object_pose=torch.eye(4), hand_interp_steps=4, - hold_steps=2, + hold_steps=hold_steps, retreat_steps=5, ) action = _bind_action( @@ -1156,14 +1682,13 @@ def plan_from_start( ) assert torch.equal(handover_options.final_object_pose, original_final_pose) semantics.entity.get_local_pose.assert_not_called() - assert [segment.name for segment in plan.segments] == [ - "transfer", - "approach", - "close", - "hold", - "release", - "deliver", - ] + assert tuple(segment.name for segment in plan.segments) == expected_segments + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count def test_handover_replan_resolves_named_targets_from_latest_snapshot() -> None: @@ -1251,7 +1776,7 @@ def fail_second_receiving_arm( task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"left_arm": _held(semantics)}, + held_objects={"logical_source": _held(semantics)}, ) action = _bind_action( generator, @@ -1275,7 +1800,15 @@ def fail_second_receiving_arm( invocation = ActionInvocation( skill_id="hand_over", goal=GraspGoal(semantics=semantics), - binding=_dual_binding(action, "source", "destination"), + binding=_dual_binding( + action, + "source", + "destination", + task_state_keys={ + "source": "logical_source", + "destination": "logical_destination", + }, + ), motion_policy=MotionPolicy(sample_count=30), ) @@ -1291,9 +1824,10 @@ def fail_second_receiving_arm( context.robot.qpos[1].unsqueeze(0).expand(30, -1), ) assert all(not frame.active_mask[1].item() for frame in plan.commands.frames) - received = projected.get_held_object("right_arm") + received = projected.get_held_object("logical_destination") assert received is not None assert received.env_mask.tolist() == [True, False] + assert projected.get_held_object("right_arm") is None semantics.entity.get_local_pose.assert_not_called() @@ -1328,14 +1862,24 @@ def test_handover_rejects_goal_for_a_different_held_object() -> None: goal_semantics.entity.get_local_pose.assert_not_called() -def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None: +@pytest.mark.parametrize( + ("hold_steps", "expected_segments"), + ( + (0, ("approach", "close", "lift", "move")), + (2, ("approach", "close", "lift", "move", "hold")), + ), +) +def test_coordinated_pick_returns_full_dof_plan_and_omits_empty_hold( + hold_steps: int, + expected_segments: tuple[str, ...], +) -> None: generator = _dual_motion_generator() action = _bind_action( generator, CoordinatedPickment( default_options=CoordinatedPickmentOptions( hand_interp_steps=4, - hold_steps=2, + hold_steps=hold_steps, object_motion_keyframes=3, ), ), @@ -1358,7 +1902,15 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None object_target_pose=torch.eye(4), object_initial_pose=torch.eye(4), ), - binding=_dual_binding(action, "left", "right"), + binding=_dual_binding( + action, + "left", + "right", + task_state_keys={ + "left": "logical_left", + "right": "logical_right", + }, + ), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context() @@ -1377,19 +1929,20 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None } assert plan.scene_dependencies == () request.goal.semantics.entity.get_local_pose.assert_not_called() - assert projected.get_held_object("left_arm") is None - assert projected.get_held_object("right_arm") is None + assert projected.get_held_object("logical_left") is None + assert projected.get_held_object("logical_right") is None assert isinstance( - projected.get_coordinated_held_object("left_arm", "right_arm"), + projected.get_coordinated_held_object("logical_left", "logical_right"), CoordinatedHeldObjectState, ) - assert [segment.name for segment in plan.segments] == [ - "approach", - "close", - "lift", - "move", - "hold", - ] + assert projected.get_coordinated_held_object("left_arm", "right_arm") is None + assert tuple(segment.name for segment in plan.segments) == expected_segments + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: @@ -1462,7 +2015,7 @@ def test_assemble_place_uses_explicit_base_snapshot() -> None: task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"arm": _held(_semantics(entity_id="assemble_object"))}, + held_objects={"logical_arm": _held(_semantics(entity_id="assemble_object"))}, ) base_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) base_pose[:, 0, 3] = torch.tensor([0.2, 0.4]) @@ -1476,12 +2029,15 @@ def test_assemble_place_uses_explicit_base_snapshot() -> None: ) request = action.resolve_request( - _invocation( - action, - AssembleGoal( - affordance=affordance, - base_pose=SceneEntityPose("base"), + replace( + _invocation( + action, + AssembleGoal( + affordance=affordance, + base_pose=SceneEntityPose("base"), + ), ), + binding=_binding(action, task_state_key="logical_arm"), ) ) plan = action.plan(request, context) @@ -1634,14 +2190,26 @@ def test_coordinated_pick_fails_when_affordance_has_no_grasp() -> None: ) -def test_coordinated_placement_projects_release_and_support_attachment() -> None: +@pytest.mark.parametrize( + ("release", "hold_steps", "expected_segments"), + ( + (False, 0, ("approach", "retreat")), + (True, 3, ("approach", "hold", "release", "retreat")), + ), +) +def test_coordinated_placement_projects_effects_and_omits_empty_segments( + release: bool, + hold_steps: int, + expected_segments: tuple[str, ...], +) -> None: generator = _dual_motion_generator() action = _bind_action( generator, CoordinatedPlacement( default_options=CoordinatedPlacementOptions( + release=release, hand_interp_steps=4, - hold_steps=3, + hold_steps=hold_steps, retreat_steps=5, ), ), @@ -1655,7 +2223,10 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"left_arm": placing, "right_arm": support}, + held_objects={ + "logical_placing": placing, + "logical_support": support, + }, ) invocation = ActionInvocation( skill_id="coordinated_placement", @@ -1663,7 +2234,15 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None placing_object_target_pose=torch.eye(4), support_object_target_pose=torch.eye(4), ), - binding=_dual_binding(action, "placing", "support"), + binding=_dual_binding( + action, + "placing", + "support", + task_state_keys={ + "placing": "logical_placing", + "support": "logical_support", + }, + ), motion_policy=MotionPolicy(sample_count=30), ) context = _dual_context(task) @@ -1679,15 +2258,23 @@ def test_coordinated_placement_projects_release_and_support_attachment() -> None "right_arm", "right_hand", } - assert projected.get_held_object("left_arm") is None - assert projected.get_held_object("right_arm") is not None - assert projected.get_held_object("right_arm").semantics is support.semantics - assert [segment.name for segment in plan.segments] == [ - "approach", - "hold", - "release", - "retreat", - ] + projected_placing = projected.get_held_object("logical_placing") + if release: + assert projected_placing is None + else: + assert projected_placing is not None + assert projected_placing.semantics is placing.semantics + assert torch.equal(projected_placing.object_to_eef, placing.object_to_eef) + assert projected.get_held_object("logical_support") is not None + assert projected.get_held_object("logical_support").semantics is support.semantics + assert projected.get_held_object("right_arm") is None + assert tuple(segment.name for segment in plan.segments) == expected_segments + assert plan.segments[0].start == 0 + assert all( + previous.stop == current.start + for previous, current in zip(plan.segments, plan.segments[1:]) + ) + assert plan.segments[-1].stop == plan.commands.frame_count def test_coordinated_placement_holds_only_environment_with_ik_failure() -> None: diff --git a/tests/sim/atomic_actions/test_articulation_effects.py b/tests/sim/atomic_actions/test_articulation_effects.py new file mode 100644 index 000000000..26603cb89 --- /dev/null +++ b/tests/sim/atomic_actions/test_articulation_effects.py @@ -0,0 +1,120 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for verified articulation state and masked symbolic effects.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.sim.atomic_actions import ( + ArticulationJointState, + StateDelta, + TaskState, +) + + +def test_task_state_normalizes_and_owns_articulation_joint_state() -> None: + position = torch.tensor([0.35], dtype=torch.float32) + state = TaskState( + batch_size=2, + device="cpu", + articulation_joints={ + ("drawer", "slide"): ArticulationJointState(position), + }, + ) + + position.fill_(99.0) + observed = state.get_articulation_joint_state("drawer", "slide") + assert observed is not None + assert torch.equal(observed.position, torch.tensor([[0.35], [0.35]])) + assert torch.equal(observed.env_mask, torch.tensor([True, True])) + + +def test_state_delta_merges_articulation_rows_without_overwriting_others() -> None: + state = TaskState( + batch_size=3, + device="cpu", + articulation_joints={ + ("drawer", "slide"): ArticulationJointState( + torch.tensor([[0.0], [0.1], [0.2]]), + ) + }, + ) + candidate = ArticulationJointState( + torch.tensor([[0.5], [0.6], [0.7]]), + env_mask=torch.tensor([True, True, False]), + ) + + updated = StateDelta( + articulation_joint_updates={("drawer", "slide"): candidate} + ).apply(state, torch.tensor([True, False, True])) + + joint = updated.get_articulation_joint_state("drawer", "slide") + assert joint is not None + assert torch.equal(joint.position, torch.tensor([[0.5], [0.1], [0.7]])) + assert torch.equal(joint.env_mask, torch.tensor([True, True, False])) + + +def test_state_delta_removes_only_selected_articulation_rows() -> None: + state = TaskState( + batch_size=2, + device="cpu", + articulation_joints={ + ("drawer", "slide"): ArticulationJointState(torch.tensor([0.4])) + }, + ) + updated = StateDelta(articulation_joint_updates={("drawer", "slide"): None}).apply( + state, torch.tensor([False, True]) + ) + + joint = updated.get_articulation_joint_state("drawer", "slide") + assert joint is not None + assert torch.equal(joint.env_mask, torch.tensor([True, False])) + + removed = StateDelta(articulation_joint_updates={("drawer", "slide"): None}).apply( + updated, torch.tensor([True, False]) + ) + assert removed.get_articulation_joint_state("drawer", "slide") is None + + +def test_articulation_state_and_delta_validate_strictly() -> None: + with pytest.raises(TypeError, match="floating"): + ArticulationJointState(torch.tensor([1], dtype=torch.long)) + with pytest.raises(ValueError, match="finite"): + ArticulationJointState(torch.tensor([float("nan")])) + with pytest.raises(ValueError, match="articulation/joint pairs"): + StateDelta(articulation_joint_updates={("drawer", ""): None}) + with pytest.raises(TypeError, match="ArticulationJointState"): + StateDelta( + articulation_joint_updates={("drawer", "slide"): torch.tensor([0.1])} + ) + + +def test_articulation_state_delta_snapshot_is_independently_owned() -> None: + source = ArticulationJointState(torch.tensor([[0.2], [0.3]])) + delta = StateDelta(articulation_joint_updates={("drawer", "slide"): source}) + snapshot = delta.snapshot() + copied = snapshot.articulation_joint_updates[("drawer", "slide")] + + assert copied is not None + assert copied is not source + assert copied.position.data_ptr() != source.position.data_ptr() + assert torch.equal(copied.position, source.position) + + +__all__: list[str] = [] diff --git a/tests/sim/atomic_actions/test_control.py b/tests/sim/atomic_actions/test_control.py index 996944c09..f31408989 100644 --- a/tests/sim/atomic_actions/test_control.py +++ b/tests/sim/atomic_actions/test_control.py @@ -174,6 +174,64 @@ def test_control_profile_is_resolved_from_robot_control_part() -> None: ) +def test_direct_binding_shares_motion_task_state_key_across_slot_endpoints() -> None: + resolved = _binding(_services()) + + assert resolved.endpoint("primary", "motion").task_state_key == "arm" + assert resolved.endpoint("primary", "grasp").task_state_key == "arm" + + +def test_direct_binding_accepts_explicit_stable_task_state_key() -> None: + resolved = _services().bind_control_parts( + _contract(), + {"primary": {"motion": "arm", "grasp": "hand"}}, + task_state_keys={"primary": "logical_manipulator"}, + ) + + assert {endpoint.task_state_key for endpoint in resolved.endpoints} == { + "logical_manipulator" + } + + +def test_direct_binding_without_motion_requires_unambiguous_task_state_key() -> None: + contract = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=( + SkillEndpointRequirement(endpoint_id="grasp"), + SkillEndpointRequirement(endpoint_id="support"), + ), + ), + ) + ) + endpoints = {"primary": {"grasp": "hand", "support": "arm"}} + services = _services() + + with pytest.raises(ValueError, match="no 'motion'.*task_state_keys"): + services.bind_control_parts(contract, endpoints) + + resolved = services.bind_control_parts( + contract, + endpoints, + task_state_keys={"primary": "logical_manipulator"}, + ) + assert {endpoint.task_state_key for endpoint in resolved.endpoints} == { + "logical_manipulator" + } + + +def test_direct_binding_requires_exact_task_state_key_slot_coverage() -> None: + services = _services() + + with pytest.raises(ValueError, match="cover the binding slots exactly"): + services.bind_control_parts( + _contract(), + {"primary": {"motion": "arm", "grasp": "hand"}}, + task_state_keys={}, + ) + + def test_invocation_override_replaces_only_resolved_endpoint_snapshot() -> None: services = _services() override_source = torch.full((2,), 0.4) diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index eabb6359a..171ebd054 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -38,6 +38,7 @@ EndpointCommand, EndEffectorPoseGoal, EntityState, + EffectVerificationRequirement, ExecutionFeedbackMode, HeldObjectState, JointPositionPayload, @@ -196,6 +197,11 @@ def _action_plan( plan_success: torch.Tensor | None = None, joint_trajectory: TimedTrajectory | None = None, feedback_mode: ExecutionFeedbackMode = ExecutionFeedbackMode.TIMED, + expected_effects: StateDelta | None = None, + effect_verification: EffectVerificationRequirement | None = None, + diagnostics: PlannerDiagnostics | None = None, + scene_dependencies: tuple[str, ...] = (), + scene_dependency_monitor_until: dict[str, int] | None = None, ) -> ActionPlan: if plan_success is None: plan_success = torch.ones( @@ -210,10 +216,71 @@ def _action_plan( recovery_policy=RecoveryPolicy(), planned_scene_version=0, planned_collision_world_revision=(0,) * commands.batch_size, - diagnostics=PlannerDiagnostics(backend="test"), + diagnostics=( + PlannerDiagnostics(backend="test") if diagnostics is None else diagnostics + ), feedback_mode=feedback_mode, joint_trajectory=joint_trajectory, + scene_dependencies=scene_dependencies, + scene_dependency_monitor_until=( + {} + if scene_dependency_monitor_until is None + else scene_dependency_monitor_until + ), + expected_effects=StateDelta() if expected_effects is None else expected_effects, + effect_verification=effect_verification, + ) + + +@pytest.mark.parametrize("kind", ("", " physical", "physical ", 1, True)) +def test_effect_verification_requirement_rejects_invalid_kind(kind: object) -> None: + with pytest.raises(ValueError, match="kind"): + EffectVerificationRequirement(kind=kind) # type: ignore[arg-type] + + +def test_action_plan_owns_explicit_effect_verification_requirement() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, ) + requirement = EffectVerificationRequirement(kind="articulation.joint_progress") + + plan = _action_plan(commands, effect_verification=requirement) + requirement_snapshot = plan.effect_verification + + assert plan.requires_effect_verification is True + assert requirement_snapshot is not None + assert requirement_snapshot is not requirement + assert requirement_snapshot.kind == requirement.kind + assert requirement_snapshot.snapshot() is not requirement_snapshot + + +def test_action_plan_implicitly_verifies_nonempty_state_delta() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + effects = StateDelta(held_object_updates={"arm": _held(batch_size=1)}) + + implicit = _action_plan(commands, expected_effects=effects) + no_effect = _action_plan(commands) + + assert implicit.effect_verification is None + assert implicit.requires_effect_verification is True + assert no_effect.requires_effect_verification is False + + +def test_action_plan_rejects_untyped_effect_verification_requirement() -> None: + commands = _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ) + + with pytest.raises(TypeError, match="EffectVerificationRequirement"): + _action_plan( + commands, + effect_verification=object(), # type: ignore[arg-type] + ) class _DependencyAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): @@ -704,6 +771,28 @@ def test_build_plan_uses_action_scene_dependency_hook() -> None: assert plan.scene_dependencies == ("extra", "tracked") +def test_build_segments_omits_zero_length_entry_and_preserves_offsets() -> None: + approach_length = 2 + release_length = 3 + segment_lengths = { + "approach": approach_length, + "hold": 0, + "release": release_length, + } + + segments = AtomicAction._build_segments( + segment_lengths, + frame_count=sum(segment_lengths.values()), + ) + + assert tuple( + (segment.name, segment.start, segment.stop) for segment in segments + ) == ( + ("approach", 0, approach_length), + ("release", approach_length, approach_length + release_length), + ) + + def test_build_command_plan_rejects_unbound_runtime_destination() -> None: context = _context() generator = Mock() @@ -878,6 +967,90 @@ def test_action_plan_owns_commands_and_optional_joint_trajectory() -> None: assert torch.equal(plan.joint_trajectory.positions, trajectory_positions) +def test_planner_diagnostics_and_plan_snapshots_own_nested_metadata() -> None: + nested = {"solver": {"iterations": [3, 5]}} + diagnostics = PlannerDiagnostics(backend="test", metadata=nested) + plan = _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=1, + ), + diagnostics=diagnostics, + ) + + nested["solver"]["iterations"][0] = 99 + diagnostics.metadata["solver"]["iterations"][1] = 77 + snapshot = plan.snapshot() + plan.diagnostics.metadata["solver"]["iterations"][0] = 42 + + assert snapshot.diagnostics.metadata["solver"]["iterations"] == [3, 5] + + +def test_planner_diagnostics_rejects_non_string_messages() -> None: + with pytest.raises(TypeError, match="messages must contain strings"): + PlannerDiagnostics( + backend="test", + messages=("valid", 1), # type: ignore[arg-type] + ) + + +def test_action_plan_owns_scene_dependency_monitor_cutoffs() -> None: + source = {"disabled": 0, "full_sequence": 2} + plan = _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ), + scene_dependencies=("disabled", "full_sequence"), + scene_dependency_monitor_until=source, + ) + + source["disabled"] = 1 + source["full_sequence"] = 1 + snapshot = plan.snapshot() + + assert plan.scene_dependency_monitor_until == { + "disabled": 0, + "full_sequence": 2, + } + assert snapshot.scene_dependency_monitor_until == { + "disabled": 0, + "full_sequence": 2, + } + assert snapshot.scene_dependency_monitor_until is not ( + plan.scene_dependency_monitor_until + ) + + +@pytest.mark.parametrize("waypoint_index", (-1, 3, True, 1.5)) +def test_action_plan_rejects_invalid_scene_dependency_monitor_cutoff( + waypoint_index: object, +) -> None: + with pytest.raises(ValueError, match="waypoint indices"): + _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ), + scene_dependencies=("tracked",), + scene_dependency_monitor_until={ + "tracked": waypoint_index # type: ignore[dict-item] + }, + ) + + +def test_action_plan_rejects_monitor_cutoff_for_non_dependency() -> None: + with pytest.raises(ValueError, match="keys must be scene dependencies"): + _action_plan( + _command_sequence( + env_ids=torch.tensor([4], dtype=torch.long), + frame_count=2, + ), + scene_dependencies=("tracked",), + scene_dependency_monitor_until={"other": 1}, + ) + + def test_action_plan_allows_timed_commands_without_joint_trajectory() -> None: commands = _command_sequence( env_ids=torch.tensor([4], dtype=torch.long), diff --git a/tests/sim/atomic_actions/test_engine.py b/tests/sim/atomic_actions/test_engine.py index eed03fad8..0cee75606 100644 --- a/tests/sim/atomic_actions/test_engine.py +++ b/tests/sim/atomic_actions/test_engine.py @@ -331,11 +331,14 @@ def test_engine_resolves_action_binding_from_robot_control_parts() -> None: resolved = engine.bind_control_parts( "stub", {"primary": {"motion": "all"}}, + task_state_keys={"primary": "logical_robot"}, ) - target = resolved.endpoint("primary", "motion").require_target(JointPositionTarget) + endpoint = resolved.endpoint("primary", "motion") + target = endpoint.require_target(JointPositionTarget) assert target.control_part == "all" assert target.joint_ids == (0, 1, 2) + assert endpoint.task_state_key == "logical_robot" def test_engine_resolves_invocation_control_override_into_request() -> None: diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index eb2dcf6cb..f71f2179f 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -20,6 +20,7 @@ from collections.abc import Sequence from dataclasses import replace +import math from typing import ClassVar from unittest.mock import Mock @@ -43,6 +44,7 @@ ExecutionSession, ExecutionStatus, ExecutionTick, + EffectVerificationRequirement, EffectVerificationResult, GraspGoal, HeldObjectState, @@ -50,6 +52,7 @@ JointPositionTarget, MotionPolicy, ObjectSemantics, + PlannerDiagnostics, PlanningContext, RecoveryPolicy, ResolvedActionRequest, @@ -138,6 +141,31 @@ def _plan( ) +class VerificationOnlyAction(DynamicAction): + """Dynamic action requiring a physical check without symbolic effects.""" + + skill_id: ClassVar[str] = "verification_only" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + pose = resolve_pose_goal(goal.xpos, context, name="xpos") + target = pose[:, 0, 3].unsqueeze(1).expand_as(context.robot.qpos) + return self.build_plan( + request, + context, + success=True, + trajectory=torch.stack([context.robot.qpos, target], dim=1), + effect_verification=EffectVerificationRequirement( + kind="physical.test_completion" + ), + ) + + class FailedEffectAction(EffectAction): """Effect-declaring action whose planner fails for every environment.""" @@ -153,6 +181,65 @@ def _plan( return replace(plan, plan_success=torch.zeros_like(plan.plan_success)) +class DiagnosticAction(DynamicAction): + """Dynamic action exposing its installed plan for snapshot isolation tests.""" + + skill_id: ClassVar[str] = "diagnostic" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def __init__(self) -> None: + super().__init__() + self.metadata = {"solver": {"iterations": [3, 5]}} + self.returned_plans: list[ActionPlan] = [] + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + plan = replace( + plan, + diagnostics=PlannerDiagnostics( + backend="diagnostic", + metadata=self.metadata, + ), + ) + self.returned_plans.append(plan) + return plan + + +class WindowedDependencyAction(DynamicAction): + """Stop monitoring a goal pose once its first command was issued.""" + + skill_id: ClassVar[str] = "windowed_dependency" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + plan = super()._plan(request, context) + return replace( + plan, + scene_dependency_monitor_until={"target": 1}, + ) + + +class MultiDependencyAction(DynamicAction): + """Track the goal plus one auxiliary scene dependency.""" + + skill_id: ClassVar[str] = "multi_dependency" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _scene_dependencies( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + ) -> tuple[str, ...]: + return tuple(sorted((*super()._scene_dependencies(request), "obstacle"))) + + class MixedEffectAction(EffectAction): """Effect action whose final environment row always fails planning.""" @@ -378,6 +465,41 @@ def _context( ) +def _multi_dependency_context( + timestamp: float, + *, + target_x: float, + obstacle_x: float | None, + version: int, + target_yaw: float = 0.0, +) -> PlanningContext: + """Build one-row context with an optional auxiliary dependency.""" + context = _context(timestamp, 0.0, target_x, version) + entities = dict(context.scene.entities) + target_pose = entities["target"].pose + cosine = math.cos(target_yaw) + sine = math.sin(target_yaw) + target_pose[:, 0, 0] = cosine + target_pose[:, 0, 1] = -sine + target_pose[:, 1, 0] = sine + target_pose[:, 1, 1] = cosine + entities["target"] = EntityState(target_pose) + if obstacle_x is not None: + obstacle_pose = torch.eye(4).unsqueeze(0) + obstacle_pose[:, 0, 3] = obstacle_x + entities["obstacle"] = EntityState(obstacle_pose) + return PlanningContext( + robot=context.robot, + task=context.task, + scene=SceneSnapshot( + timestamp=timestamp, + version=version, + entities=entities, + ), + env_ids=context.env_ids, + ) + + def _collision_context( timestamp: float, qpos: torch.Tensor, @@ -460,6 +582,7 @@ def _destination_invocation( "second": "arm_b", } }, + task_state_keys={"primary": "destination_resource"}, ), recovery_policy=RecoveryPolicy( max_replans=2, @@ -476,7 +599,7 @@ def _effect_session( max_action_retries: int = 2, action_timeout: float = 30.0, eligible_mask: torch.Tensor | None = None, - action: EffectAction | None = None, + action: DynamicAction | None = None, ) -> tuple[ExecutionSession, ExecutionTick]: """Advance a test effect action to its verification boundary.""" engine, _ = _engine(batch_size=batch_size) @@ -532,6 +655,109 @@ def test_session_completes_incremental_command_sequence() -> None: assert final.eligible_mask.tolist() == [True] +def test_all_rows_planning_failure_skips_inactive_command_frames() -> None: + engine, _ = _engine() + action = FailedEffectAction() + engine.register(action) + base = _invocation(engine, max_action_retries=0) + invocation = ActionInvocation( + skill_id=action.skill_id, + 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)) + + failed = session.tick(_context(0.0, 0.0, 0.2, 0)) + + assert failed.command is None + assert failed.status is ExecutionStatus.FAILED + assert failed.eligible_mask.tolist() == [False] + assert any( + event.kind is ExecutionEventKind.ACTION_PLANNING_FAILED + for event in failed.events + ) + + +def test_plan_attempt_records_snapshot_nested_metadata_at_installation() -> None: + engine, _ = _engine() + action = DiagnosticAction() + engine.register(action) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=action.skill_id, + 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)) + + action.metadata["solver"]["iterations"][0] = 99 + action.returned_plans[0].diagnostics.metadata["solver"]["iterations"][1] = 77 + first_read = session.plan_attempts[0] + first_read.plan.diagnostics.metadata["solver"]["iterations"][0] = 42 + second_read = session.plan_attempts[0] + + assert second_read.plan.diagnostics.metadata["solver"]["iterations"] == [3, 5] + + +def test_scene_dependency_window_ignores_expected_self_motion() -> None: + engine, _ = _engine() + action = WindowedDependencyAction() + engine.register(action) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=action.skill_id, + 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)) + + first = session.tick(_context(0.0, 0.0, 0.2, 0)) + moved = session.tick(_context(0.1, 0.0, 0.8, 1)) + + assert first.command is not None + assert moved.command is not None + assert action.plan_count == 1 + assert not any( + event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED for event in moved.events + ) + + +def test_scene_dependency_window_reports_motion_before_cutoff() -> None: + engine, _ = _engine() + action = WindowedDependencyAction() + engine.register(action) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=action.skill_id, + 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)) + + moved = session.tick(_context(0.1, 0.0, 0.8, 1)) + + changed = next( + event + for event in moved.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=0: " + "entity_id='target', monitor_cutoff=1, max_translation=0.600000, " + "translation_threshold=0.020000, max_rotation=0.000000, " + "rotation_threshold=0.087266." + ) + assert action.plan_count == 2 + + def test_initial_eligibility_is_sticky_across_invocation_barriers() -> None: engine, _ = _engine(batch_size=2) invocation = _invocation(engine) @@ -706,13 +932,104 @@ def test_scene_motion_replans_late_bound_goal() -> None: tick = session.tick(_context(0.1, 0.0, 0.3, 1)) kinds = {event.kind for event in tick.events} + changed = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) assert ExecutionEventKind.DYNAMIC_GOAL_CHANGED in kinds assert ExecutionEventKind.REPLANNED in kinds + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=1: " + "entity_id='target', monitor_cutoff=none, max_translation=0.200000, " + "translation_threshold=0.020000, max_rotation=0.000000, " + "rotation_threshold=0.087266." + ) assert action.plan_count == 2 assert action.requests[0] is action.requests[1] assert tick.command is not None +def test_scene_motion_diagnostic_orders_multiple_changed_entities() -> None: + engine, _ = _engine() + action = MultiDependencyAction() + engine.register(action) + initial = _multi_dependency_context( + 0.0, + target_x=0.1, + obstacle_x=0.4, + version=0, + ) + session = engine.start( + (_invocation(engine, skill_id=action.skill_id),), + initial, + ) + session.tick(initial) + + tick = session.tick( + _multi_dependency_context( + 0.1, + target_x=0.4, + obstacle_x=0.8, + version=1, + target_yaw=0.2, + ) + ) + + changed = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=1: " + "entity_id='obstacle', monitor_cutoff=none, max_translation=0.400000, " + "translation_threshold=0.020000, max_rotation=0.000000, " + "rotation_threshold=0.087266 | " + "entity_id='target', monitor_cutoff=none, max_translation=0.300000, " + "translation_threshold=0.020000, max_rotation=0.200000, " + "rotation_threshold=0.087266." + ) + + +def test_scene_motion_diagnostic_identifies_missing_entity() -> None: + engine, _ = _engine() + action = MultiDependencyAction() + engine.register(action) + initial = _multi_dependency_context( + 0.0, + target_x=0.1, + obstacle_x=0.4, + version=0, + ) + session = engine.start( + (_invocation(engine, skill_id=action.skill_id),), + initial, + ) + session.tick(initial) + + tick = session.tick( + _multi_dependency_context( + 0.1, + target_x=0.1, + obstacle_x=None, + version=1, + ) + ) + + changed = next( + event + for event in tick.events + if event.kind is ExecutionEventKind.DYNAMIC_GOAL_CHANGED + ) + assert changed.message == ( + "Scene dependency invalidated the active plan at waypoint_index=1: " + "entity_id='obstacle', monitor_cutoff=none, missing=current_scene, " + "max_translation=unavailable, translation_threshold=0.020000, " + "max_rotation=unavailable, rotation_threshold=0.087266." + ) + + def test_recovery_replan_rejects_runtime_destination_change() -> None: engine, action = _destination_engine(("first", "second")) invocation = _destination_invocation(engine) @@ -794,6 +1111,18 @@ def test_collision_world_change_replans_with_latest_obstacle_pose() -> None: latest_obstacles = generator.bind_collision_world.call_args.kwargs["obstacle_poses"] assert latest_obstacles["obstacle"][0, 0, 3] == pytest.approx(0.6) assert tick.command is not None + attempts = session.plan_attempts + assert [attempt.attempt_generation for attempt in attempts] == [0, 1] + assert [attempt.event_kind for attempt in attempts] == [ + ExecutionEventKind.ACTION_PLANNED, + ExecutionEventKind.REPLANNED, + ] + assert [attempt.plan.planned_scene_version for attempt in attempts] == [0, 1] + assert [attempt.plan.planned_collision_world_revision for attempt in attempts] == [ + (0,), + (1,), + ] + assert [attempt.replan_counts for attempt in attempts] == [(0,), (1,)] def test_collision_world_exhaustion_only_disables_changed_environment() -> None: @@ -1107,10 +1436,15 @@ def test_session_revision_replans_from_latest_context() -> None: session.revise_current(revised) first = session.tick(_context(0.0, 0.0, 0.1, 0)) second = session.tick(_context(0.1, 0.0, 0.1, 0)) + attempts = session.plan_attempts assert action.plan_count == 2 assert action.requests[0] is not action.requests[1] assert [request.revision for request in action.requests] == [0, 1] + assert [attempt.request.revision for attempt in attempts] == [0, 1] + assert attempts[0].request is not attempts[1].request + assert attempts[1].request.motion_policy == revised.motion_policy + assert attempts[1].request.recovery_policy == revised.recovery_policy assert any( event.kind is ExecutionEventKind.INVOCATION_REVISED and event.invocation_revision == 1 @@ -1298,6 +1632,159 @@ def test_session_rejects_regressing_collision_world_revision() -> None: session.tick(regressed) +def test_explicit_verification_with_empty_delta_preserves_task_state() -> None: + session, waiting = _effect_session(action=VerificationOnlyAction()) + request = waiting.pending_effect + assert request is not None + assert request.expected_effects.is_empty + assert request.effect_verification is not None + assert request.effect_verification.kind == "physical.test_completion" + initial_task_state = waiting.task_state + + preserved = session.pending_effect + assert preserved is not None + assert preserved.effect_verification is not None + assert preserved.effect_verification is not request.effect_verification + with pytest.raises(ValueError, match="explicit physical-effect requirement"): + replace(request, effect_verification=None) + + completed = session.tick( + _context(0.21, 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 + assert completed.pending_effect is None + assert completed.task_state is initial_task_state + assert not completed.task_state.held_objects + + +def test_explicit_verification_keeps_partial_and_retry_row_lifecycle() -> None: + session, waiting = _effect_session( + batch_size=2, + max_action_retries=1, + action=VerificationOnlyAction(), + ) + first_request = waiting.pending_effect + assert first_request is not None + initial_task_state = waiting.task_state + + retry = 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, True]), + ), + ) + + assert retry.pending_effect is None + assert retry.task_state is initial_task_state + assert any( + event.kind is ExecutionEventKind.ACTION_RETRY + and event.env_mask.tolist() == [False, True] + for event in retry.events + ) + + first_command = session.tick(_context(0.22, (0.2, 0.2), (0.2, 0.2), 0)) + second_command = session.tick(_context(0.23, (0.2, 0.2), (0.2, 0.2), 0)) + second_wait = session.tick(_context(0.24, (0.2, 0.2), (0.2, 0.2), 0)) + assert first_command.command is not None + assert first_command.command.active_mask.tolist() == [False, True] + assert second_command.command is not None + assert second_command.command.active_mask.tolist() == [False, True] + second_request = second_wait.pending_effect + assert second_request is not None + assert second_request.env_mask.tolist() == [False, True] + assert second_request.verification_id != first_request.verification_id + assert second_request.deadline > first_request.deadline + + completed = session.tick( + _context(0.25, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + second_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert completed.status is ExecutionStatus.COMPLETED + assert completed.eligible_mask.tolist() == [True, True] + assert completed.task_state is initial_task_state + + +def test_explicit_verification_partial_success_shrinks_request_without_state_delta() -> ( + None +): + session, waiting = _effect_session( + batch_size=2, + action=VerificationOnlyAction(), + ) + first_request = waiting.pending_effect + assert first_request is not None + initial_task_state = waiting.task_state + + 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]), + ), + ) + + second_request = partial.pending_effect + assert second_request is not None + assert second_request.env_mask.tolist() == [False, True] + assert second_request.verification_id != first_request.verification_id + assert second_request.requested_at == first_request.requested_at + assert second_request.deadline == first_request.deadline + assert second_request.effect_verification is not None + assert second_request.effect_verification.kind == "physical.test_completion" + assert partial.task_state is initial_task_state + + completed = session.tick( + _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=EffectVerificationResult( + second_request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert completed.status is ExecutionStatus.COMPLETED + assert completed.task_state is initial_task_state + + +def test_explicit_verification_empty_delta_obeys_action_timeout() -> None: + session, waiting = _effect_session( + max_action_retries=0, + action_timeout=0.25, + action=VerificationOnlyAction(), + ) + request = waiting.pending_effect + assert request is not None + initial_task_state = waiting.task_state + + timed_out = session.tick(_context(0.3, 0.2, 0.2, 0)) + + assert timed_out.status is ExecutionStatus.FAILED + assert timed_out.pending_effect is None + assert timed_out.task_state is initial_task_state + assert any( + event.kind is ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT + for event in timed_out.events + ) + assert any( + event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED + for event in timed_out.events + ) + + def test_nonempty_effect_is_committed_only_after_external_verification() -> None: engine, _ = _engine() effect = EffectAction() @@ -1989,12 +2476,10 @@ def test_failed_effect_plan_retries_without_requesting_effect_verification() -> 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)) - - failed = session.tick(_context(0.2, 0.0, 0.2, 0)) + failed = session.tick(_context(0.0, 0.0, 0.2, 0)) assert failed.status is ExecutionStatus.FAILED + assert failed.command is None assert failed.pending_effect is None assert not any( event.kind is ExecutionEventKind.EFFECT_VERIFICATION_REQUIRED diff --git a/tests/sim/objects/test_articulation.py b/tests/sim/objects/test_articulation.py index 8a731a6f8..e294f3c0a 100644 --- a/tests/sim/objects/test_articulation.py +++ b/tests/sim/objects/test_articulation.py @@ -17,8 +17,10 @@ from __future__ import annotations import os -import torch +from types import SimpleNamespace + import pytest +import torch from embodichain.lab.sim import ( SimulationManager, @@ -41,6 +43,16 @@ NUM_ARENAS = 10 +def test_get_qf_returns_all_articulation_joint_efforts(): + expected_qf = torch.tensor([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]], dtype=torch.float32) + articulation = object.__new__(Articulation) + articulation._data = SimpleNamespace(qf=expected_qf) + + actual_qf = articulation.get_qf() + + assert torch.equal(actual_qf, expected_qf) + + def _link_static_friction(art: Articulation, link_name: str, env_idx: int = 0) -> float: return art._entities[env_idx].get_physical_attr(link_name).static_friction diff --git a/tests/sim/objects/test_robot.py b/tests/sim/objects/test_robot.py index 876781a60..e6e050f91 100644 --- a/tests/sim/objects/test_robot.py +++ b/tests/sim/objects/test_robot.py @@ -17,9 +17,11 @@ from __future__ import annotations import os -import torch -import pytest +from types import SimpleNamespace + import numpy as np +import pytest +import torch from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.objects import Robot @@ -49,6 +51,20 @@ } +def test_get_qf_selects_control_part_joint_efforts(): + full_qf = torch.tensor( + [[1.0, 2.0, 3.0, 4.0], [5.0, 6.0, 7.0, 8.0]], dtype=torch.float32 + ) + robot = object.__new__(Robot) + robot._data = SimpleNamespace(qf=full_qf) + robot.cfg = SimpleNamespace(control_parts={"arm": ["joint_3", "joint_1"]}) + robot._joint_ids = {"arm": [3, 1]} + + actual_qf = robot.get_qf(name="arm") + + assert torch.equal(actual_qf, full_qf[:, [3, 1]]) + + # Base test class for CPU and CUDA class BaseRobotTest: @classmethod