From f064f26ad4a35e68a874a6db9d7389d5afdd1ae9 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 11 Aug 2026 22:23:58 +0800 Subject: [PATCH] feat(skills): gate motion on physical effects --- .../design/declarative_expert_program_plan.md | 48 +- .../embodichain.lab.sim.atomic_actions.rst | 6 + .../overview/sim/atomic_actions/index.md | 27 +- docs/source/tutorial/atomic_actions.rst | 28 ++ .../lab/sim/atomic_actions/__init__.py | 14 +- embodichain/lab/sim/atomic_actions/core.py | 1 + .../lab/sim/atomic_actions/execution.py | 474 +++++++++++++++++- .../lab/sim/atomic_actions/invocation.py | 80 +++ embodichain/lab/sim/atomic_actions/runner.py | 62 +++ embodichain/lab/sim/skills/__init__.py | 2 + embodichain/lab/sim/skills/compiler.py | 171 +++++++ embodichain/lab/sim/skills/runtime.py | 170 ++++++- .../test_simulation_environment.py | 66 ++- .../sim/atomic_actions/test_engine_per_env.py | 240 +++++++++ tests/sim/atomic_actions/test_runner.py | 142 +++++- tests/sim/skills/test_compiler.py | 35 ++ tests/sim/skills/test_runtime.py | 116 +++++ 17 files changed, 1638 insertions(+), 44 deletions(-) diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 13f4cd66d..5e9cacb7e 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -8,7 +8,9 @@ Pick/Place/settle/validator cycle, while the full three-cycle run remains in threshold calibration. Dual-UR5/PGI HandOver has completed three consecutive supported-simulation Pick/transfer/settle/validator runs using contact - dynamics only. + dynamics only. Named trajectory-segment effect gates now block Pick lift, + Place retract, and HandOver source release until fresh physical evidence + confirms the required acquisition or release. - Baseline: `main@bcccb787dcafdafd7b944ba210e5e85f9cd1d0cb` - Last updated: 2026-08-11 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), @@ -600,8 +602,9 @@ controller intent, not physical proof. passed the selected monitor. The target contract must treat contradictory evidence, including object-to-endpoint slip, as a real effect failure, invalidate the affected row's assumed relation, and enter bounded recovery instead of -repairing the scene. The runtime now exposes the active named motion phase, -observes phase-scoped held-object invariants from fresh physical evidence, and +repairing the scene. The runtime now exposes the active named trajectory +segment, observes segment-scoped held-object invariants from fresh physical +evidence, and applies removal-only ``StateDelta`` reconciliation to failed rows before any retry or recovery hand-off. The monitor publishes one current-observation outcome per physical expectation, including the stronger proof that every @@ -615,10 +618,23 @@ external recovery, but the core owns the removal-only invalidation delta and applies it before either path. Evidence that remains unresolved at the action deadline is reconciled fail-closed: any active verified state covered by the pending effect is removed before external recovery. Workflow-level -re-acquisition and blocking acquisition gates remain open; neither may repair -the scene implicitly. For handover, success transfers the verified relation -from source to destination while the destination remains physically closed. -Releasing the destination is a separate ``Place`` or ``Release`` semantic call. +re-acquisition remains open and may not repair the scene implicitly. + +Blocking physical-effect gates are enforced at named trajectory-segment +entries. ``Pick`` requires destination attachment before ``lift``; ``Place`` +requires source detachment before ``retract``; and ``HandOver`` requires +destination attachment before the source ``release`` segment. While a gate is +unresolved, the session does not advance its waypoint cursor and replays the +preceding command for the complete synchronized active cohort, so gripper +preload or open intent remains active under real dynamics. Gate success only +unlocks motion and never commits ``TaskState``; terminal effect verification +remains authoritative. Contradiction uses the enclosing action's bounded retry +policy, stale request IDs are rejected, and the action deadline covers gate +polling. Every gate owns a fresh monitor instance independent from the terminal +monitor and in-flight loss guard. For handover, terminal success transfers the +verified relation from source to destination while the destination remains +physically closed. Releasing the destination is a separate ``Place`` or +``Release`` semantic call. The first pure-dynamics rollout uses the staged **B** continuation policy. The standard simulation factory lowers both trajectory ``control_dt`` and runner @@ -1225,7 +1241,7 @@ The backend-neutral typed state expectations, evidence addresses and sources, pose/binary/scalar/joint evidence clauses, versioned monitor registry, profile-owned monitor selection, grounded Pick/Place/HandOver/articulation effects, row-local composite hysteresis kernel, canonical `SkillRuntime`, and -production simulation evidence ports are wired end to end. Phase-scoped +production simulation evidence ports are wired end to end. Segment-scoped held-object guard requests, live evidence collection, row-local symbolic invalidation, bounded Pick retry, and typed external-recovery hand-off are also implemented. Physical simulation acceptance is partial: Open Drawer and one @@ -1233,10 +1249,10 @@ cube Pick/Place/settle/validator cycle have completed. The embodiment-owned dual-UR5/PGI HandOver slice now completes Pick, transfer, terminal physical-effect verification, settling, and target validation through real contact dynamics. Per-expectation terminal outcomes, core-owned failure -invalidation, row-local retry/recovery decisions, and fail-closed deadline -reconciliation are implemented. Blocking acquisition gates, workflow-level -re-acquisition, fault-injection coverage, and the full repeated-cube run remain -validation or design work. +invalidation, row-local retry/recovery decisions, fail-closed deadline +reconciliation, and blocking named-segment effect gates are implemented. +Workflow-level re-acquisition, fault-injection coverage, and the full +repeated-cube run remain validation or design work. Deliverables: @@ -1492,12 +1508,12 @@ The design is complete when all of the following hold: synthetic attachment, freezes the object, or overrides its pose. - [ ] Physical held-object loss is observed as effect failure, invalidates the affected symbolic relation, and exercises bounded recovery rather than - being hidden by a simulator-side attachment. The phase-aware observation, + being hidden by a simulator-side attachment. The segment-aware observation, row-local core-owned invalidation, per-expectation terminal reconciliation, fail-closed deadline handling, bounded Pick/retained-Place - retry, and typed recovery boundary are implemented; blocking acquisition, - workflow-level re-acquisition, and real-simulation fault injection remain - open. + retry, typed recovery boundary, and blocking acquisition/release gates are + implemented; workflow-level re-acquisition and real-simulation fault + injection remain open. - [x] Repeated sub-threshold motion eventually publishes the correct scene revision. - [x] Custom actions have a documented and tested intentional hard-break diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst index b2b0236d9..940e738ec 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst @@ -17,6 +17,7 @@ embodichain.lab.sim.atomic_actions ControlPartCommandProfile ActionControlOverrides ActionInvocation + PhaseEffectGateRequirement ResolvedActionRequest ActionOptions MotionPolicy @@ -73,6 +74,11 @@ embodichain.lab.sim.atomic_actions SimulationExecutionAdapter ExecutionTick EffectVerificationRequest + EffectVerificationResult + PhaseEffectGateRequest + PhaseEffectGateResult + HeldObjectGuardRequest + HeldObjectGuardResult ExecutionEvent ExecutionEventKind ExecutionStatus diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index c59bcbabf..63fb3a5de 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -837,8 +837,8 @@ and resets the history. Evidence exactly at the deadline is valid, while a due observation after the deadline is handled by session timeout without invoking the verifier. -The curated semantic runtime also installs phase-scoped, negative -held-object guards for named trajectory segments. Before a due command is +The curated semantic runtime also installs segment-scoped, negative held-object +guards for named trajectory segments. Before a due command is dispatched, `ExecutionRunner` passes a fresh observation and the current `HeldObjectGuardRequest` to its synchronous guard verifier. Each request has a single-use verification ID, the active waypoint/segment identity, and the @@ -849,10 +849,27 @@ before retrying or emitting `RECOVERY_REQUIRED`. Unavailable or unresolved evidence does not count as a physical contradiction, and the guard verifier is not invoked after the authoritative action deadline. -The current guard is observational and negative; a blocking positive -acquisition gate, outcome-aware terminal reconciliation, and workflow-level -re-acquisition remain separate policies. Neither the monitor nor runtime +The curated `Pick`, `Place`, and `HandOver` paths additionally install blocking +positive-effect gates at named trajectory-segment entries. Pick must verify the +destination attachment before `lift`, Place must verify source detachment before +`retract`, and HandOver must verify the destination attachment before source +`release`. The compiler creates a monitor instance for each gate independently +from both the terminal monitor and negative held-object guard. + +`ExecutionSession` exposes a correlated `PhaseEffectGateRequest` at the segment +boundary. While its result remains unresolved, the waypoint cursor does not +advance and the preceding command is replayed for the complete synchronized +active cohort. This preserves gripper preload or open intent instead of +replacing it with an observed-position hold. A successful +`PhaseEffectGateResult` only unlocks the segment; it does not commit +`TaskState`. Contradiction uses the enclosing action's bounded retry policy, +request IDs are single-use, and the action deadline covers all polling. Calling +`run_until_blocked()` without a gate verifier returns this boundary for an +external verifier. + +The guards and gates are observational. Neither a monitor nor the runtime creates a simulator attachment, freezes an object, or overrides its pose. +Workflow-level re-acquisition remains a separate recovery policy. ## Action Agent integration diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index 93907fd7e..5cd038ce2 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -550,6 +550,34 @@ same invocation remains physically valid. Other failed rows enter external recovery after selected invalidation. Unresolved evidence at the action deadline is reconciled fail-closed when covered verified state is still active. +Trajectory-segment effect gates +------------------------------- + +An invocation may declare a +:class:`~embodichain.lab.sim.atomic_actions.PhaseEffectGateRequirement` for a +named, non-initial trajectory segment. The execution session then exposes a +:class:`~embodichain.lab.sim.atomic_actions.PhaseEffectGateRequest` immediately +before the first frame of that segment. Curated semantic calls install these +automatically: Pick gates ``lift`` on destination attachment, Place gates +``retract`` on source detachment, and HandOver gates source ``release`` on +destination attachment. + +Supply ``phase_effect_gate_verifier(context, request)`` to ``runner.step()`` or +``runner.run_until_blocked()``. It runs on a fresh due-cycle observation and +returns a correlated +:class:`~embodichain.lab.sim.atomic_actions.PhaseEffectGateResult`. If neither +the success nor failure mask selects every remaining active row, the session +keeps the whole cohort at the boundary and resends the command immediately +before the gated segment. This preserves a close/open command and its physical +preload; it is not an observed-position hold. + +Gate success only permits the next command and does not update ``TaskState``. +The terminal effect verifier still owns the semantic commit. A contradictory +row may consume the enclosing action's retry budget; a row outside the result's +``retry_mask`` requires external recovery. The gate shares the action timeout, +and each consumed observation replaces its request ID. Without a gate verifier, +``run_until_blocked()`` returns the pending boundary for asynchronous handling. + Adding an action ---------------- diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 733097151..b1ec7d50c 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -70,6 +70,8 @@ ExecutionTick, HeldObjectGuardRequest, HeldObjectGuardResult, + PhaseEffectGateRequest, + PhaseEffectGateResult, ) from .goals import ( ActionGoal, @@ -78,7 +80,12 @@ SceneArticulationOperationGeometry, SceneEntityPose, ) -from .invocation import ActionInvocation, ActionOptions, ResolvedActionRequest +from .invocation import ( + ActionInvocation, + ActionOptions, + PhaseEffectGateRequirement, + ResolvedActionRequest, +) from .plans import ( ActionPlan, CompiledTrajectory, @@ -196,6 +203,7 @@ HeldObjectGuardVerifier, MonotonicExecutionClock, ObservationProvider, + PhaseEffectGateVerifier, RunnerStatus, RunnerStep, RunnerStepCallback, @@ -279,6 +287,10 @@ "ExecutionTick", "HeldObjectGuardRequest", "HeldObjectGuardResult", + "PhaseEffectGateRequest", + "PhaseEffectGateRequirement", + "PhaseEffectGateResult", + "PhaseEffectGateVerifier", "HeldObjectGuardVerifier", "EndpointTrackingChannelBinding", "EndpointTrackingFeedbackAddress", diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index 892bec50a..5bca5cec6 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -377,6 +377,7 @@ def resolve_request( motion_policy=invocation.motion_policy, tracking_policy=invocation.tracking_policy, recovery_policy=invocation.recovery_policy, + phase_effect_gates=invocation.phase_effect_gates, skill_options=options, invocation_id=invocation.invocation_id, revision=invocation.revision, diff --git a/embodichain/lab/sim/atomic_actions/execution.py b/embodichain/lab/sim/atomic_actions/execution.py index 6b463dfe5..0eb36e6cd 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -26,7 +26,11 @@ import torch from .effects import StateDelta -from .invocation import ActionInvocation, ResolvedActionRequest +from .invocation import ( + ActionInvocation, + PhaseEffectGateRequirement, + ResolvedActionRequest, +) from .bindings import RuntimeEndpointTarget from .plans import ( ActionPlan, @@ -74,6 +78,9 @@ class ExecutionEventKind(str, Enum): EFFECT_VERIFICATION_REQUIRED = "effect_verification_required" EFFECT_VERIFICATION_FAILED = "effect_verification_failed" EFFECT_VERIFICATION_TIMEOUT = "effect_verification_timeout" + PHASE_EFFECT_GATE_REQUIRED = "phase_effect_gate_required" + PHASE_EFFECT_GATE_SATISFIED = "phase_effect_gate_satisfied" + PHASE_EFFECT_GATE_FAILED = "phase_effect_gate_failed" HELD_OBJECT_LOST = "held_object_lost" ACTION_RETRY = "action_retry" ACTION_COMPLETED = "action_completed" @@ -511,6 +518,167 @@ def __post_init__(self) -> None: ) +@dataclass(frozen=True, slots=True, eq=False) +class PhaseEffectGateRequest: + """Correlate a blocking physical-effect check with a segment entry. + + The action's preceding command remains active while the gate is unresolved. + A gate is scoped to the enclosing action attempt and does not create a + separate planning, recovery, or timeout budget. + + Args: + verification_id: Session-local single-use request identity. + gate_id: Invocation-local stable gate identity. + skill_id: Registered action skill identity. + invocation_id: Optional logical invocation correlation identity. + invocation_revision: Active invocation revision. + invocation_index: Active invocation position in the session. + attempt_generation: Installed action-plan attempt generation. + next_waypoint_index: First command frame blocked by the gate. + segment_name: Named trajectory segment blocked by the gate. + requested_at: Request creation time in the observation timestamp domain. + deadline: Enclosing action deadline in that same timestamp domain. + env_mask: Active rows that must satisfy the gate together. + """ + + verification_id: int + gate_id: str + skill_id: str + invocation_id: str | None + invocation_revision: int + invocation_index: int + attempt_generation: int + next_waypoint_index: int + segment_name: str + requested_at: float + deadline: float + env_mask: torch.Tensor + + def __post_init__(self) -> None: + for name in ( + "verification_id", + "invocation_revision", + "invocation_index", + "attempt_generation", + "next_waypoint_index", + ): + value = getattr(self, name) + if type(value) is not int or value < 0: + raise ValueError(f"{name} must be a non-negative integer.") + for name in ("gate_id", "skill_id", "segment_name"): + value = getattr(self, name) + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{name} must be a non-empty string without outer whitespace." + ) + if self.invocation_id is not None and ( + type(self.invocation_id) is not str or not self.invocation_id + ): + raise ValueError("invocation_id 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 one-dimensional bool tensor.") + if not self.env_mask.any(): + raise ValueError("env_mask must contain at least one gated row.") + object.__setattr__(self, "env_mask", self.env_mask.clone()) + + def snapshot(self) -> PhaseEffectGateRequest: + """Return an independently owned gate request.""" + return PhaseEffectGateRequest( + verification_id=self.verification_id, + gate_id=self.gate_id, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + invocation_index=self.invocation_index, + attempt_generation=self.attempt_generation, + next_waypoint_index=self.next_waypoint_index, + segment_name=self.segment_name, + requested_at=self.requested_at, + deadline=self.deadline, + env_mask=self.env_mask, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class PhaseEffectGateResult: + """Current-observation decision for one blocking segment-entry gate. + + Rows absent from both decision masks remain unresolved. ``retry_mask`` is + a subset of failed rows for which replaying the enclosing action remains + valid; no gate outcome mutates verified task state. + + Args: + verification_id: Identity copied from the consumed gate request. + gate_id: Stable gate identity copied from the request. + attempt_generation: Action attempt copied from the request. + invocation_index: Session invocation index copied from the request. + next_waypoint_index: Blocked waypoint copied from the request. + success_mask: Rows whose current evidence satisfies the gate. + failure_mask: Rows whose current evidence contradicts the gate. + retry_mask: Failed rows allowed to retry the enclosing action. + message: Optional physical-failure diagnostic. + """ + + verification_id: int + gate_id: str + attempt_generation: int + invocation_index: int + next_waypoint_index: int + success_mask: torch.Tensor + failure_mask: torch.Tensor + retry_mask: torch.Tensor + message: str = "" + + def __post_init__(self) -> None: + for name in ( + "verification_id", + "attempt_generation", + "invocation_index", + "next_waypoint_index", + ): + value = getattr(self, name) + if type(value) is not int or value < 0: + raise ValueError(f"{name} must be a non-negative integer.") + if ( + type(self.gate_id) is not str + or not self.gate_id + or self.gate_id != self.gate_id.strip() + ): + raise ValueError( + "gate_id must be a non-empty string without outer whitespace." + ) + masks = (self.success_mask, self.failure_mask, self.retry_mask) + for name, value in zip( + ("success_mask", "failure_mask", "retry_mask"), + masks, + strict=True, + ): + 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 any(value.shape != masks[0].shape for value in masks[1:]): + raise ValueError("Phase-effect gate masks must have equal shapes.") + if any(value.device != masks[0].device for value in masks[1:]): + raise ValueError("Phase-effect gate masks must use the same device.") + if (self.success_mask & self.failure_mask).any(): + raise ValueError("Gate success and failure masks must not overlap.") + if (self.retry_mask & ~self.failure_mask).any(): + raise ValueError("retry_mask must be a subset of failure_mask.") + if type(self.message) is not str: + raise TypeError("message must be a string.") + for name in ("success_mask", "failure_mask", "retry_mask"): + object.__setattr__(self, name, getattr(self, name).clone()) + + @dataclass(frozen=True, slots=True, eq=False) class HeldObjectGuardRequest: """Describe the next in-flight command boundary for held-object checks. @@ -707,6 +875,7 @@ class ExecutionTick: events: tuple[ExecutionEvent, ...] task_state: TaskState pending_effect: EffectVerificationRequest | None = None + pending_phase_effect_gate: PhaseEffectGateRequest | None = None def __post_init__(self) -> None: if self.eligible_mask.dtype != torch.bool or self.eligible_mask.dim() != 1: @@ -717,6 +886,21 @@ def __post_init__(self) -> None: raise TypeError( "pending_effect must be an EffectVerificationRequest or None." ) + if self.pending_phase_effect_gate is not None and not isinstance( + self.pending_phase_effect_gate, + PhaseEffectGateRequest, + ): + raise TypeError( + "pending_phase_effect_gate must be a PhaseEffectGateRequest or None." + ) + if ( + self.pending_effect is not None + and self.pending_phase_effect_gate is not None + ): + raise ValueError( + "Terminal effect verification and a phase-effect gate cannot be " + "pending together." + ) if self.command is not None and not isinstance( self.command, RuntimeCommandFrame, @@ -738,6 +922,12 @@ def __post_init__(self) -> None: "pending_effect", self.pending_effect.snapshot(), ) + if self.pending_phase_effect_gate is not None: + object.__setattr__( + self, + "pending_phase_effect_gate", + self.pending_phase_effect_gate.snapshot(), + ) hold_targets: list[RuntimeEndpointTarget] = [] for target in self.hold_targets: snapshot = target.snapshot() @@ -828,6 +1018,10 @@ def __init__( self._effect_requested_at: float | None = None self._next_effect_verification_id = 0 self._next_held_object_guard_verification_id = 0 + self._pending_phase_effect_gate: PhaseEffectGateRequest | None = None + self._satisfied_phase_effect_gates: set[str] = set() + self._reported_phase_effect_gates: set[str] = set() + self._next_phase_effect_gate_verification_id = 0 self._plan_attempt_records: list[_ExecutionPlanAttemptRecord] = [] self._status = ( ExecutionStatus.RUNNING if self._eligible.any() else ExecutionStatus.FAILED @@ -874,6 +1068,17 @@ 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() + @property + def phase_effect_gate_request(self) -> PhaseEffectGateRequest | None: + """Return the blocking gate at the next trajectory-segment entry. + + Returns: + Owned request snapshot, or ``None`` when the next command is not + blocked by a physical-effect gate. + """ + request = self._phase_effect_gate_request() + return None if request is None else request.snapshot() + @property def held_object_guard_request(self) -> HeldObjectGuardRequest | None: """Describe the phase that must be checked before the next command. @@ -942,6 +1147,9 @@ def deactivate_rows( ) else: self._pending_effect = None + if self._pending_phase_effect_gate is not None: + self._pending_phase_effect_gate = None + self._next_phase_effect_gate_verification_id += 1 terminal_event = self._update_terminal_status() if terminal_event is not None: self._queued_events.append(terminal_event) @@ -995,7 +1203,11 @@ 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 or self._effect_failures.any(): + if ( + self._pending_effect is not None + or self._pending_phase_effect_gate is not None + or self._effect_failures.any() + ): raise RuntimeError( "Cannot revise while physical-effect resolution is pending; " "resolve it or cancel and start a new invocation." @@ -1017,7 +1229,11 @@ 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 or self._effect_failures.any(): + if ( + self._pending_effect is not None + or self._pending_phase_effect_gate is not None + or self._effect_failures.any() + ): raise RuntimeError( "Cannot revise while physical-effect resolution is pending; " "resolve it or cancel and start a new invocation." @@ -1127,6 +1343,7 @@ def tick( context: PlanningContext, *, effect_result: EffectVerificationResult | None = None, + phase_effect_gate_result: PhaseEffectGateResult | None = None, held_object_guard_result: HeldObjectGuardResult | None = None, ) -> ExecutionTick: """Advance execution by one observation/command cycle. @@ -1136,6 +1353,8 @@ def tick( state is replaced by the session's verified task state. effect_result: Optional correlated semantic-effect result for an action waiting at its terminal waypoint. + phase_effect_gate_result: Optional correlated physical-effect + decision for a blocked trajectory-segment entry. held_object_guard_result: Optional correlated in-flight held-object loss result for the current waypoint phase. ``None`` means the verifier found no applicable guard for this phase or no result @@ -1158,6 +1377,38 @@ def tick( "effect_result verification_id does not match the pending " "effect boundary." ) + phase_gate_request = self._phase_effect_gate_request() + if phase_effect_gate_result is not None: + if type(phase_effect_gate_result) is not PhaseEffectGateResult: + raise TypeError( + "phase_effect_gate_result must be exactly " + "PhaseEffectGateResult or None." + ) + if phase_gate_request is None: + raise ValueError("No phase-effect gate is awaiting verification.") + if phase_effect_gate_result.verification_id != ( + phase_gate_request.verification_id + ): + raise ValueError( + "phase_effect_gate_result verification_id does not match " + "the pending gate." + ) + for name in ( + "gate_id", + "attempt_generation", + "invocation_index", + "next_waypoint_index", + ): + if getattr(phase_effect_gate_result, name) != getattr( + phase_gate_request, + name, + ): + raise ValueError( + f"phase_effect_gate_result {name} does not match the " + "pending gate." + ) + self._next_phase_effect_gate_verification_id += 1 + self._pending_phase_effect_gate = None guard_request = self._held_object_guard_request() if held_object_guard_result is not None: if type(held_object_guard_result) is not HeldObjectGuardResult: @@ -1193,6 +1444,17 @@ def tick( return self._tick_result(command=None, events=events) assert self._plan is not None + if phase_effect_gate_result is not None: + assert phase_gate_request is not None + events.extend( + self._apply_phase_effect_gate_result( + phase_effect_gate_result, + phase_gate_request, + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + assert self._plan is not None if held_object_guard_result is not None: assert guard_request is not None events.extend( @@ -1372,6 +1634,13 @@ def tick( events=events, ) + phase_gate_request = self._phase_effect_gate_request() + if phase_gate_request is not None: + events.extend(self._phase_effect_gate_required_events(phase_gate_request)) + preceding_waypoint = phase_gate_request.next_waypoint_index - 1 + command = self._command_at(plan, preceding_waypoint, execution_mask) + return self._tick_result(command=command, events=events) + commands = plan.commands if self._waypoint_index < commands.frame_count: command = self._command_at(plan, self._waypoint_index, execution_mask) @@ -1544,6 +1813,7 @@ def _install_plan( replacement_tracking_routes = self._tracking_routes(plan) self._validate_destination_continuity(plan, event_kind) self._validate_tracking_continuity(plan, event_kind) + self._validate_phase_effect_gates(plan) if ( event_kind not in ( @@ -1576,6 +1846,9 @@ def _install_plan( self._pending_effect = None self._effect_failures.zero_() self._effect_requested_at = None + self._pending_phase_effect_gate = None + self._satisfied_phase_effect_gates.clear() + self._reported_phase_effect_gates.clear() planned_mask = self._pending & plan.plan_success self._plan_attempt_records.append( _ExecutionPlanAttemptRecord( @@ -1598,6 +1871,29 @@ def _install_plan( self._event(event_kind, planned_mask, "Planned from the latest context.") ) + def _validate_phase_effect_gates(self, plan: ActionPlan) -> None: + """Bind invocation-owned gates to non-initial named plan segments.""" + request = self._requests[self._invocation_index] + for requirement in request.phase_effect_gates: + if type(requirement) is not PhaseEffectGateRequirement: + raise TypeError( + "Resolved phase-effect gates must be exact " + "PhaseEffectGateRequirement values." + ) + try: + segment = plan.segment(requirement.segment_name) + except KeyError as exc: + raise ValueError( + f"Phase-effect gate {requirement.gate_id!r} references " + f"missing segment {requirement.segment_name!r}." + ) from exc + if segment.start == 0: + raise ValueError( + f"Phase-effect gate {requirement.gate_id!r} cannot block the " + "first trajectory segment because no preceding command exists " + "to preserve while evidence is acquired." + ) + def _validate_destination_continuity( self, plan: ActionPlan, @@ -2335,12 +2631,177 @@ def _batched_entity_pose(self, state: EntityState) -> torch.Tensor: raise ValueError("Scene entity pose batch does not match the session.") return pose + def _phase_effect_gate_requirement( + self, + ) -> PhaseEffectGateRequirement | None: + """Resolve a gate exactly at the next named segment's first frame.""" + if ( + self._status is not ExecutionStatus.RUNNING + or self._plan is None + or self._pending_effect is not None + or self._effect_failures.any() + or self._plan.commands.frame_count == 0 + or self._waypoint_index >= self._plan.commands.frame_count + ): + return None + segment = self._plan.segment_at(self._waypoint_index) + if self._waypoint_index != segment.start: + return None + request = self._requests[self._invocation_index] + return next( + ( + value + for value in request.phase_effect_gates + if value.segment_name == segment.name + and value.gate_id not in self._satisfied_phase_effect_gates + ), + None, + ) + + def _phase_effect_gate_request(self) -> PhaseEffectGateRequest | None: + """Build or retain the current blocking segment-entry gate request.""" + requirement = self._phase_effect_gate_requirement() + if requirement is None: + self._pending_phase_effect_gate = None + return None + assert self._plan is not None + env_mask = self._pending & self._plan.plan_success + if not env_mask.any(): + self._pending_phase_effect_gate = None + return None + current = self._pending_phase_effect_gate + if ( + current is not None + and current.gate_id == requirement.gate_id + and current.attempt_generation == self._attempt_generation + and current.next_waypoint_index == self._waypoint_index + and torch.equal(current.env_mask, env_mask) + ): + return current + invocation = self._requests[self._invocation_index] + deadline = self._action_started_at + self._plan.recovery_policy.action_timeout + current = PhaseEffectGateRequest( + verification_id=self._next_phase_effect_gate_verification_id, + gate_id=requirement.gate_id, + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + invocation_index=self._invocation_index, + attempt_generation=self._attempt_generation, + next_waypoint_index=self._waypoint_index, + segment_name=requirement.segment_name, + requested_at=min(self._context.robot.timestamp, deadline), + deadline=deadline, + env_mask=env_mask, + ) + self._pending_phase_effect_gate = current + return current + + def _phase_effect_gate_required_events( + self, + request: PhaseEffectGateRequest, + ) -> list[ExecutionEvent]: + """Emit the gate boundary once per installed action attempt.""" + if request.gate_id in self._reported_phase_effect_gates: + return [] + self._reported_phase_effect_gates.add(request.gate_id) + return [ + self._event( + ExecutionEventKind.PHASE_EFFECT_GATE_REQUIRED, + request.env_mask, + f"Physical-effect gate {request.gate_id!r} blocks segment " + f"{request.segment_name!r} until current evidence succeeds.", + ) + ] + + def _apply_phase_effect_gate_result( + self, + result: PhaseEffectGateResult, + request: PhaseEffectGateRequest, + ) -> list[ExecutionEvent]: + """Resolve one gate observation without mutating verified task state.""" + success = self._normalize_mask( + result.success_mask, + "phase_effect_gate_result.success_mask", + ) + failure = self._normalize_mask( + result.failure_mask, + "phase_effect_gate_result.failure_mask", + ) + retry = self._normalize_mask( + result.retry_mask, + "phase_effect_gate_result.retry_mask", + ) + request_mask = request.env_mask.to(self._eligible.device) + if ((success | failure | retry) & ~request_mask).any(): + raise ValueError( + "Phase-effect gate result masks must be subsets of the pending " + "request env_mask." + ) + events = self._phase_effect_gate_required_events(request) + message = result.message or ( + f"Physical evidence contradicted gate {request.gate_id!r} before " + f"segment {request.segment_name!r}." + ) + non_retry = failure & ~retry + if non_retry.any(): + self._eligible &= ~non_retry + self._pending &= ~non_retry + self._last_command_mask &= ~non_retry + events.extend( + ( + self._event( + ExecutionEventKind.PHASE_EFFECT_GATE_FAILED, + non_retry, + message, + ), + self._event( + ExecutionEventKind.RECOVERY_REQUIRED, + non_retry, + "The failed segment-entry gate requires recovery outside " + "the current action retry policy.", + ), + ) + ) + previous_generation = self._attempt_generation + if retry.any(): + events.extend( + self._attempt_action_retry( + retry, + ExecutionEventKind.PHASE_EFFECT_GATE_FAILED, + message, + ) + ) + else: + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) + if ( + self._status is not ExecutionStatus.RUNNING + or self._attempt_generation != previous_generation + ): + return events + assert self._plan is not None + remaining = request_mask & self._pending & self._plan.plan_success + if remaining.any() and torch.equal(success & remaining, remaining): + self._satisfied_phase_effect_gates.add(request.gate_id) + events.append( + self._event( + ExecutionEventKind.PHASE_EFFECT_GATE_SATISFIED, + remaining, + f"Physical-effect gate {request.gate_id!r} released segment " + f"{request.segment_name!r}.", + ) + ) + return events + def _held_object_guard_request(self) -> HeldObjectGuardRequest | None: """Build the current command-phase held-object guard request.""" if ( self._status is not ExecutionStatus.RUNNING or self._plan is None or self._pending_effect is not None + or self._phase_effect_gate_request() is not None or self._effect_failures.any() or self._plan.commands.frame_count == 0 ): @@ -2674,6 +3135,7 @@ def _update_terminal_status(self) -> ExecutionEvent | None: if not self._eligible.any() and self._status is ExecutionStatus.RUNNING: self._status = ExecutionStatus.FAILED self._pending_effect = None + self._pending_phase_effect_gate = None self._effect_failures.zero_() self._effect_requested_at = None return self._event( @@ -2691,6 +3153,9 @@ def _tick_result( hold_targets: tuple[RuntimeEndpointTarget, ...] = (), ) -> ExecutionTick: """Build an immutable tick result.""" + phase_gate = self._phase_effect_gate_request() + if phase_gate is not None: + events.extend(self._phase_effect_gate_required_events(phase_gate)) return ExecutionTick( status=self._status, eligible_mask=self._eligible, @@ -2699,6 +3164,7 @@ def _tick_result( events=tuple(events), task_state=self._task_state, pending_effect=self._pending_effect, + pending_phase_effect_gate=phase_gate, ) @@ -2714,4 +3180,6 @@ def _tick_result( "ExecutionTick", "HeldObjectGuardRequest", "HeldObjectGuardResult", + "PhaseEffectGateRequest", + "PhaseEffectGateResult", ] diff --git a/embodichain/lab/sim/atomic_actions/invocation.py b/embodichain/lab/sim/atomic_actions/invocation.py index cba5fac0f..db3d6bd27 100644 --- a/embodichain/lab/sim/atomic_actions/invocation.py +++ b/embodichain/lab/sim/atomic_actions/invocation.py @@ -47,6 +47,38 @@ class ActionOptions: OptionsT = TypeVar("OptionsT", bound=ActionOptions) +@dataclass(frozen=True, slots=True) +class PhaseEffectGateRequirement: + """Require physical-effect evidence before one trajectory segment starts. + + The requirement carries only stable core correlation data. Semantic + integrations own the corresponding observation specification and monitor; + the execution session owns blocking, timeout, and action-retry behavior. + + Args: + gate_id: Invocation-local stable gate identifier. + segment_name: Exact named trajectory segment blocked by this gate. + """ + + gate_id: str + segment_name: str + + def __post_init__(self) -> None: + for name in ("gate_id", "segment_name"): + value = getattr(self, name) + if type(value) is not str or not value or value != value.strip(): + raise ValueError( + f"{name} must be a non-empty string without outer whitespace." + ) + + def snapshot(self) -> PhaseEffectGateRequirement: + """Return an independently constructed immutable requirement.""" + return PhaseEffectGateRequirement( + gate_id=self.gate_id, + segment_name=self.segment_name, + ) + + def _goal_snapshot_memo(goal: ActionGoal) -> dict[int, object]: """Return deepcopy memo entries for live goal references and runtime caches.""" memo: dict[int, object] = {} @@ -110,6 +142,9 @@ class ActionInvocation(Generic[GoalT, OptionsT]): recovery_policy: RecoveryPolicy = field(default_factory=RecoveryPolicy) """Bounded local execution recovery settings.""" + phase_effect_gates: tuple[PhaseEffectGateRequirement, ...] = () + """Physical-effect gates enforced at named trajectory-segment entries.""" + skill_options: OptionsT | None = None """Optional per-invocation behavior override for the selected skill.""" @@ -141,6 +176,22 @@ def __post_init__(self) -> None: raise TypeError("tracking_policy must be a TrackingPolicy.") if not isinstance(self.recovery_policy, RecoveryPolicy): raise TypeError("recovery_policy must be a RecoveryPolicy.") + phase_effect_gates = tuple(self.phase_effect_gates) + if not all( + type(value) is PhaseEffectGateRequirement for value in phase_effect_gates + ): + raise TypeError( + "phase_effect_gates must contain exact " + "PhaseEffectGateRequirement values." + ) + gate_ids = [value.gate_id for value in phase_effect_gates] + segment_names = [value.segment_name for value in phase_effect_gates] + if len(set(gate_ids)) != len(gate_ids): + raise ValueError("Phase-effect gate IDs must be unique per invocation.") + if len(set(segment_names)) != len(segment_names): + raise ValueError( + "At most one phase-effect gate may block each trajectory segment." + ) if self.skill_options is not None and not isinstance( self.skill_options, ActionOptions ): @@ -153,6 +204,11 @@ def __post_init__(self) -> None: raise ValueError("invocation_id must be a non-empty string when set.") if not isinstance(self.revision, int) or self.revision < 0: raise ValueError("revision must be a non-negative integer.") + object.__setattr__( + self, + "phase_effect_gates", + tuple(value.snapshot() for value in phase_effect_gates), + ) @dataclass(frozen=True, slots=True) @@ -172,6 +228,7 @@ class ResolvedActionRequest(Generic[GoalT, OptionsT]): tracking_policy: TrackingPolicy recovery_policy: RecoveryPolicy skill_options: OptionsT + phase_effect_gates: tuple[PhaseEffectGateRequirement, ...] = () invocation_id: str | None = None revision: int = 0 @@ -186,6 +243,22 @@ def __post_init__(self) -> None: raise TypeError("tracking_policy must be a TrackingPolicy.") if not isinstance(self.recovery_policy, RecoveryPolicy): raise TypeError("recovery_policy must be a RecoveryPolicy.") + phase_effect_gates = tuple(self.phase_effect_gates) + if not all( + type(value) is PhaseEffectGateRequirement for value in phase_effect_gates + ): + raise TypeError( + "phase_effect_gates must contain exact " + "PhaseEffectGateRequirement values." + ) + gate_ids = [value.gate_id for value in phase_effect_gates] + segment_names = [value.segment_name for value in phase_effect_gates] + if len(set(gate_ids)) != len(gate_ids): + raise ValueError("Phase-effect gate IDs must be unique per request.") + if len(set(segment_names)) != len(segment_names): + raise ValueError( + "At most one phase-effect gate may block each trajectory segment." + ) if not isinstance(self.skill_options, ActionOptions): raise TypeError("skill_options must be an ActionOptions instance.") if self.invocation_id is not None and ( @@ -210,6 +283,11 @@ def __post_init__(self) -> None: object.__setattr__(self, "motion_policy", deepcopy(self.motion_policy)) object.__setattr__(self, "tracking_policy", deepcopy(self.tracking_policy)) object.__setattr__(self, "recovery_policy", deepcopy(self.recovery_policy)) + object.__setattr__( + self, + "phase_effect_gates", + tuple(value.snapshot() for value in phase_effect_gates), + ) object.__setattr__(self, "skill_options", deepcopy(self.skill_options)) def snapshot(self) -> ResolvedActionRequest[GoalT, OptionsT]: @@ -221,6 +299,7 @@ def snapshot(self) -> ResolvedActionRequest[GoalT, OptionsT]: motion_policy=self.motion_policy, tracking_policy=self.tracking_policy, recovery_policy=self.recovery_policy, + phase_effect_gates=self.phase_effect_gates, skill_options=self.skill_options, invocation_id=self.invocation_id, revision=self.revision, @@ -232,5 +311,6 @@ def snapshot(self) -> ResolvedActionRequest[GoalT, OptionsT]: "ActionOptions", "GoalT", "OptionsT", + "PhaseEffectGateRequirement", "ResolvedActionRequest", ] diff --git a/embodichain/lab/sim/atomic_actions/runner.py b/embodichain/lab/sim/atomic_actions/runner.py index 0c7601fad..bd231598b 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -38,6 +38,8 @@ ExecutionTick, HeldObjectGuardRequest, HeldObjectGuardResult, + PhaseEffectGateRequest, + PhaseEffectGateResult, ) from .invocation import ActionInvocation, ResolvedActionRequest from .runtime_commands import RuntimeCommandFrame @@ -318,6 +320,12 @@ def is_waiting(self) -> bool: ] """Synchronous phase-aware held-object verifier for one due command cycle.""" +PhaseEffectGateVerifier = Callable[ + [PlanningContext, PhaseEffectGateRequest], + PhaseEffectGateResult, +] +"""Synchronous verifier for one blocking trajectory-segment entry gate.""" + RunnerStepCallback = Callable[[RunnerStep], None] """Optional observer called after every blocking runner-loop iteration.""" @@ -485,6 +493,8 @@ def step( *, effect_result: EffectVerificationResult | None = None, effect_verifier: EffectVerifier | None = None, + phase_effect_gate_result: PhaseEffectGateResult | None = None, + phase_effect_gate_verifier: PhaseEffectGateVerifier | None = None, held_object_guard_verifier: HeldObjectGuardVerifier | None = None, ) -> RunnerStep: """Perform one due observation/session/controller update without sleeping. @@ -498,6 +508,11 @@ def step( and before the session consumes the result. It is not called after the request deadline. Mutually exclusive with ``effect_result``. + phase_effect_gate_result: Optional externally produced result for + the current blocking trajectory-segment entry gate. + phase_effect_gate_verifier: Optional synchronous verifier for the + current gate. It runs on a fresh due-cycle observation and is + mutually exclusive with ``phase_effect_gate_result``. held_object_guard_verifier: Optional synchronous phase-aware verifier. It receives a fresh observation and the current command-phase request before :meth:`ExecutionSession.tick` and @@ -512,12 +527,24 @@ def step( raise ValueError( "effect_result and effect_verifier are mutually exclusive." ) + if ( + phase_effect_gate_result is not None + and phase_effect_gate_verifier is not None + ): + raise ValueError( + "phase_effect_gate_result and phase_effect_gate_verifier are " + "mutually exclusive." + ) if effect_verifier is not None and not callable(effect_verifier): raise TypeError("effect_verifier must be callable or None.") if held_object_guard_verifier is not None and not callable( held_object_guard_verifier ): raise TypeError("held_object_guard_verifier must be callable or None.") + if phase_effect_gate_verifier is not None and not callable( + phase_effect_gate_verifier + ): + raise TypeError("phase_effect_gate_verifier must be callable or None.") now = self._clock_now() if self._status is not RunnerStatus.RUNNING: return self._result(timestamp=now) @@ -573,6 +600,29 @@ def step( context=context, ) + phase_effect_gate_request = self._session.phase_effect_gate_request + if ( + phase_effect_gate_verifier is not None + and phase_effect_gate_request is not None + and context.robot.timestamp <= phase_effect_gate_request.deadline + ): + try: + phase_effect_gate_result = phase_effect_gate_verifier( + context, + phase_effect_gate_request, + ) + if type(phase_effect_gate_result) is not PhaseEffectGateResult: + raise TypeError( + "PhaseEffectGateVerifier must return exactly " + "PhaseEffectGateResult." + ) + except Exception as exc: + return self._fail( + "Phase-effect gate verifier failed: " + f"{type(exc).__name__}: {exc}", + context=context, + ) + held_object_guard_result: HeldObjectGuardResult | None = None held_object_guard_request = self._session.held_object_guard_request if ( @@ -604,6 +654,7 @@ def step( tick = self._session.tick( context, effect_result=effect_result, + phase_effect_gate_result=phase_effect_gate_result, held_object_guard_result=held_object_guard_result, ) context = self._session.latest_context @@ -755,6 +806,7 @@ def run_until_blocked( self, *, effect_verifier: EffectVerifier | None = None, + phase_effect_gate_verifier: PhaseEffectGateVerifier | None = None, held_object_guard_verifier: HeldObjectGuardVerifier | None = None, on_step: RunnerStepCallback | None = None, max_steps: int = 100_000, @@ -766,6 +818,8 @@ def run_until_blocked( due-cycle observations while effect verification is pending. Without one, the method returns the running boundary so the caller can verify externally. + phase_effect_gate_verifier: Optional synchronous callback used on + fresh observations while a trajectory-segment entry is gated. held_object_guard_verifier: Optional synchronous phase-aware held-object verifier used before every due command cycle. on_step: Optional callback for tracing or tutorial visualization. @@ -788,6 +842,7 @@ def run_until_blocked( for _ in range(max_steps): result = self.step( effect_verifier=effect_verifier, + phase_effect_gate_verifier=phase_effect_gate_verifier, held_object_guard_verifier=held_object_guard_verifier, ) if on_step is not None: @@ -808,6 +863,12 @@ def run_until_blocked( ) if verification_required and effect_verifier is None: return result + gate_required = ( + result.tick is not None + and result.tick.pending_phase_effect_gate is not None + ) + if gate_required and phase_effect_gate_verifier is None: + return result if result.wait_duration > 0.0: try: self._clock.sleep(result.wait_duration) @@ -1004,6 +1065,7 @@ def _result( "HeldObjectGuardVerifier", "MonotonicExecutionClock", "ObservationProvider", + "PhaseEffectGateVerifier", "RunnerStatus", "RunnerStep", "RunnerStepCallback", diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 2b192950e..04c168dc1 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -35,6 +35,7 @@ from .compiler import ( AnalyzedSemanticCall, GroundedHeldObjectGuard, + GroundedPhaseEffectGate, GroundedSemanticCall, HandOverPoseProvider, HandOverPoseTargets, @@ -272,6 +273,7 @@ "FORCE_EFFECT_CHANNEL", "GRASP_AFFORDANCE_CAPABILITY", "GroundedHeldObjectGuard", + "GroundedPhaseEffectGate", "GroundedSemanticCall", "HeldObjectRelation", "HeldObjectStateExpectation", diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index 4b23d3bca..904de30b5 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -43,6 +43,7 @@ PlaceOptions, OperateArticulationGoal, OperateArticulationOptions, + PhaseEffectGateRequirement, PlanningContext, PoseGoalValue, SceneArticulationOperationGeometry, @@ -557,6 +558,55 @@ def task_state_key(self) -> str: return expectation.task_state_key +@dataclass(frozen=True, slots=True) +class GroundedPhaseEffectGate: + """One independently monitored physical-effect segment-entry gate. + + Args: + gate_id: Invocation-local stable gate identity. + segment_name: Named trajectory segment blocked by the gate. + effect_spec: Single-expectation physical observation contract. + effect_monitor: Fresh monitor instance owned only by this gate. + retry_action: Whether contradiction may retry the enclosing action. + """ + + gate_id: str + segment_name: str + effect_spec: SemanticEffectSpec + effect_monitor: EffectMonitor = field(repr=False, compare=False) + retry_action: bool = True + + def __post_init__(self) -> None: + _validate_identifier(self.gate_id, field_name="gate_id") + _validate_identifier(self.segment_name, field_name="segment_name") + if not isinstance(self.effect_spec, SemanticEffectSpec): + raise TypeError("effect_spec must be a SemanticEffectSpec.") + physical_ids = {clause.expectation_id for clause in self.effect_spec.clauses} + if ( + len(self.effect_spec.state_expectations) != 1 + or len(physical_ids) != 1 + or next(iter(physical_ids)) + != self.effect_spec.state_expectations[0].expectation_id + ): + raise ValueError( + "A phase-effect gate must own exactly one physically observed " + "state expectation." + ) + if not isinstance(self.effect_monitor, EffectMonitor): + raise TypeError("effect_monitor must be an EffectMonitor.") + if type(self.retry_action) is not bool: + raise TypeError("retry_action must be a bool.") + object.__setattr__(self, "effect_spec", self.effect_spec.snapshot()) + + @property + def requirement(self) -> PhaseEffectGateRequirement: + """Return the core-owned blocking requirement for this monitor.""" + return PhaseEffectGateRequirement( + gate_id=self.gate_id, + segment_name=self.segment_name, + ) + + @dataclass(frozen=True, slots=True, init=False) class GroundedSemanticCall: """Factory-owned call lowered from the latest observed context.""" @@ -566,6 +616,7 @@ class GroundedSemanticCall: effect_spec: SemanticEffectSpec | None effect_monitor: EffectMonitor | None = field(repr=False, compare=False) effect_guards: tuple[GroundedHeldObjectGuard, ...] + effect_gates: tuple[GroundedPhaseEffectGate, ...] _eligible_mask: torch.Tensor = field(repr=False, compare=False) def __init__(self, *args: object, **kwargs: object) -> None: @@ -585,6 +636,7 @@ def _create( effect_spec: SemanticEffectSpec | None, effect_monitor: EffectMonitor | None, effect_guards: tuple[GroundedHeldObjectGuard, ...], + effect_gates: tuple[GroundedPhaseEffectGate, ...], eligible_mask: torch.Tensor, ) -> GroundedSemanticCall: """Create one compiler-owned grounded result.""" @@ -594,6 +646,7 @@ def _create( object.__setattr__(instance, "effect_spec", effect_spec) object.__setattr__(instance, "effect_monitor", effect_monitor) object.__setattr__(instance, "effect_guards", tuple(effect_guards)) + object.__setattr__(instance, "effect_gates", tuple(effect_gates)) object.__setattr__(instance, "_eligible_mask", eligible_mask.clone()) instance.__post_init__() return instance @@ -630,6 +683,28 @@ def __post_init__(self) -> None: if guards and self.effect_spec is None: raise ValueError("Held-object guards require a terminal effect spec.") object.__setattr__(self, "effect_guards", guards) + gates = tuple(self.effect_gates) + if not all(type(value) is GroundedPhaseEffectGate for value in gates): + raise TypeError( + "effect_gates must contain exact GroundedPhaseEffectGate values." + ) + gate_ids = [value.gate_id for value in gates] + gate_segments = [value.segment_name for value in gates] + if len(set(gate_ids)) != len(gate_ids): + raise ValueError("Grounded phase-effect gate IDs must be unique.") + if len(set(gate_segments)) != len(gate_segments): + raise ValueError( + "At most one grounded phase-effect gate may block each segment." + ) + if tuple(value.requirement for value in gates) != ( + self.invocation.phase_effect_gates + ): + raise ValueError( + "Grounded phase-effect gates must match invocation requirements." + ) + if gates and self.effect_spec is None: + raise ValueError("Phase-effect gates require a terminal effect spec.") + object.__setattr__(self, "effect_gates", gates) if not isinstance(self._eligible_mask, torch.Tensor): raise TypeError("eligible_mask must be a torch.Tensor.") if self._eligible_mask.dtype != torch.bool or self._eligible_mask.dim() != 1: @@ -1226,12 +1301,23 @@ def ground( context, path=(*path, call_index, "effect_guards"), ) + effect_gates = self._ground_phase_effect_gates( + analyzed, + effect_spec, + path=(*path, call_index, "effect_gates"), + ) + if effect_gates: + invocation = replace( + invocation, + phase_effect_gates=tuple(value.requirement for value in effect_gates), + ) return GroundedSemanticCall._create( analyzed=analyzed, invocation=invocation, effect_spec=effect_spec, effect_monitor=effect_monitor, effect_guards=effect_guards, + effect_gates=effect_gates, eligible_mask=eligible, ) @@ -1864,6 +1950,90 @@ def _ground_effect_spec( clauses=tuple(clauses), ) + def _ground_phase_effect_gates( + self, + analyzed: AnalyzedSemanticCall, + effect_spec: SemanticEffectSpec | None, + *, + path: tuple[PathPart, ...], + ) -> tuple[GroundedPhaseEffectGate, ...]: + """Create blocking acquisition/release gates for built-in semantics.""" + monitor_ref = analyzed.effect_monitor_ref + if effect_spec is None or monitor_ref is None: + return () + call = analyzed.call + if type(call) is Pick: + definitions = (("destination_acquired", "lift", "destination"),) + elif type(call) is Place: + definitions = (("source_released", "retract", "source"),) + elif type(call) is HandOver: + definitions = (("destination_acquired", "release", "destination"),) + else: + return () + + gates: list[GroundedPhaseEffectGate] = [] + for gate_id, segment_name, expectation_id in definitions: + gate_spec = self._single_held_expectation_effect_spec( + effect_spec, + expectation_id=expectation_id, + ) + try: + monitor = self._effect_monitor_registry.create( + gate_spec, + monitor_ref, + ) + except (KeyError, TypeError, ValueError) as exc: + raise _diagnostic( + "effect_gate_monitor_creation_failed", + (*path, gate_id), + f"Could not create phase-effect gate monitor: {exc}", + ) from exc + gates.append( + GroundedPhaseEffectGate( + gate_id=gate_id, + segment_name=segment_name, + effect_spec=gate_spec, + effect_monitor=monitor, + retry_action=True, + ) + ) + return tuple(gates) + + @staticmethod + def _single_held_expectation_effect_spec( + terminal_spec: SemanticEffectSpec, + *, + expectation_id: str, + ) -> SemanticEffectSpec: + """Project one terminal held relation into an independent gate spec.""" + expectation = terminal_spec.state_expectation(expectation_id) + if type(expectation) is not HeldObjectStateExpectation: + raise TypeError("Phase-effect gates require held-object expectations.") + clauses = tuple( + clause + for clause in terminal_spec.clauses + if clause.expectation_id == expectation_id + ) + if not clauses: + raise ValueError( + f"Held-object expectation {expectation_id!r} has no physical clauses." + ) + effect_kind = ( + SemanticEffectKind.ATTACH + if expectation.relation is HeldObjectRelation.ATTACHED + else SemanticEffectKind.RELEASE + ) + return SemanticEffectSpec( + semantic_id=terminal_spec.semantic_id, + effect_kind=effect_kind, + skill_id=terminal_spec.skill_id, + invocation_id=terminal_spec.invocation_id, + invocation_revision=terminal_spec.invocation_revision, + env_ids=terminal_spec.env_ids, + state_expectations=(expectation,), + clauses=clauses, + ) + def _ground_held_object_guards( self, analyzed: AnalyzedSemanticCall, @@ -2487,6 +2657,7 @@ def _broadcast_joint_position( __all__ = [ "AnalyzedSemanticCall", "GroundedHeldObjectGuard", + "GroundedPhaseEffectGate", "GroundedSemanticCall", "HandOverPoseProvider", "HandOverPoseTargets", diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py index 095ce5853..0cd168965 100644 --- a/embodichain/lab/sim/skills/runtime.py +++ b/embodichain/lab/sim/skills/runtime.py @@ -38,6 +38,8 @@ ExecutionPlanAttempt, HeldObjectGuardRequest, HeldObjectGuardResult, + PhaseEffectGateRequest, + PhaseEffectGateResult, ) from ..atomic_actions.plans import TrajectorySegment from ..atomic_actions.policies import MotionPolicy, RecoveryPolicy @@ -61,6 +63,7 @@ from .calls import HandOver, Pick, Place, SemanticCallSpec from .compiler import ( GroundedHeldObjectGuard, + GroundedPhaseEffectGate, HeldObjectGuardBaseline, SemanticSkillCompiler, ) @@ -71,6 +74,8 @@ EffectMonitor, EffectMonitorDecision, EffectMonitorRef, + HeldObjectRelation, + HeldObjectStateExpectation, JointStateEvidenceBatch, PoseRelationEvidenceBatch, ScalarEffectEvidenceBatch, @@ -966,6 +971,7 @@ class SkillEffectTrace: evidence: Mapping[str, EffectEvidenceBatch] boundary_kind: str = "terminal" guard_id: str | None = None + gate_id: str | None = None segment_name: str | None = None def __post_init__(self) -> None: @@ -975,20 +981,43 @@ def __post_init__(self) -> None: raise ValueError("verification_id must be a non-negative integer.") if type(self.observation_revision) is not int or self.observation_revision < 0: raise ValueError("observation_revision must be non-negative.") - if self.boundary_kind not in {"terminal", "in_flight_guard"}: - raise ValueError("boundary_kind must be 'terminal' or 'in_flight_guard'.") - for name in ("guard_id", "segment_name"): + if self.boundary_kind not in { + "terminal", + "in_flight_guard", + "phase_effect_gate", + }: + raise ValueError( + "boundary_kind must be 'terminal', 'in_flight_guard', or " + "'phase_effect_gate'." + ) + for name in ("guard_id", "gate_id", "segment_name"): value = getattr(self, name) if value is not None and (type(value) is not str or not value): raise ValueError(f"{name} must be a non-empty string or None.") if self.boundary_kind == "terminal": - if self.guard_id is not None or self.segment_name is not None: + if ( + self.guard_id is not None + or self.gate_id is not None + or self.segment_name is not None + ): raise ValueError( - "Terminal effect traces cannot declare guard phase metadata." + "Terminal effect traces cannot declare segment-boundary metadata." ) - elif self.guard_id is None or self.segment_name is None: + elif self.boundary_kind == "in_flight_guard" and ( + self.guard_id is None + or self.gate_id is not None + or self.segment_name is None + ): + raise ValueError( + "In-flight guard traces require only guard_id and segment_name." + ) + elif self.boundary_kind == "phase_effect_gate" and ( + self.gate_id is None + or self.guard_id is not None + or self.segment_name is None + ): raise ValueError( - "In-flight guard traces require guard_id and segment_name." + "Phase-effect gate traces require only gate_id and segment_name." ) if not math.isfinite(self.timestamp) or self.timestamp < 0.0: raise ValueError("timestamp must be finite and non-negative.") @@ -1113,6 +1142,7 @@ def snapshot(self) -> SkillEffectTrace: evidence=self.evidence, boundary_kind=self.boundary_kind, guard_id=self.guard_id, + gate_id=self.gate_id, segment_name=self.segment_name, ) @@ -1158,6 +1188,13 @@ def to_metadata(self) -> dict[str, object]: "segment_name": self.segment_name, } ) + elif self.boundary_kind == "phase_effect_gate": + metadata["boundary"].update( + { + "gate_id": self.gate_id, + "segment_name": self.segment_name, + } + ) return metadata @@ -1646,6 +1683,7 @@ def __init__( self._call_effect_offset = 0 self._observation_revision = 0 self._next_guard_verification_id = 0 + self._next_gate_verification_id = 0 self._wait_duration = 0.0 self._message: str | None = None @@ -1813,8 +1851,11 @@ def step(self) -> SkillResult: verifier = self._effect_verifier if monitor is not None else None guards = tuple(getattr(grounded, "effect_guards", ())) guard_verifier = self._held_object_guard_verifier if guards else None + gates = tuple(getattr(grounded, "effect_gates", ())) + gate_verifier = self._phase_effect_gate_verifier if gates else None runner_step = runner.step( effect_verifier=verifier, + phase_effect_gate_verifier=gate_verifier, held_object_guard_verifier=guard_verifier, ) self._consume_runner_step(runner_step) @@ -1829,6 +1870,17 @@ def step(self) -> SkillResult: "semantic call did not install an effect monitor." ) return self.result + if ( + runner_step.status is RunnerStatus.RUNNING + and runner_step.tick is not None + and runner_step.tick.pending_phase_effect_gate is not None + and not gates + ): + self._abort( + "The atomic invocation requested a phase-effect gate, but the " + "grounded semantic call did not install its monitor." + ) + return self.result if runner_step.status is RunnerStatus.RUNNING: return self.result self._finish_current_call(runner_step) @@ -2083,6 +2135,7 @@ def _reset_workflow( self._call_effect_offset = 0 self._observation_revision = 0 self._next_guard_verification_id = 0 + self._next_gate_verification_id = 0 self._wait_duration = 0.0 self._message = None self._status = SkillStatus.RUNNING @@ -2134,6 +2187,7 @@ def _prepare_call(self, call_index: int) -> None: effect_spec = getattr(grounded, "effect_spec", None) effect_monitor = getattr(grounded, "effect_monitor", None) effect_guards = tuple(getattr(grounded, "effect_guards", ())) + effect_gates = tuple(getattr(grounded, "effect_gates", ())) if invocation is None: raise TypeError("Semantic compiler ground() must return an invocation.") if not isinstance(grounded_eligible, torch.Tensor) or not torch.equal( @@ -2162,6 +2216,13 @@ def _prepare_call(self, call_index: int) -> None: ) if effect_guards and effect_spec is None: raise ValueError("Grounded held-object guards require an effect spec.") + if not all(type(value) is GroundedPhaseEffectGate for value in effect_gates): + raise TypeError( + "Grounded effect_gates must contain exact " + "GroundedPhaseEffectGate values." + ) + if effect_gates and effect_spec is None: + raise ValueError("Grounded phase-effect gates require an effect spec.") self._grounded = grounded session = self._engine.start( @@ -2303,6 +2364,99 @@ def _terminal_failure_policy( return failure_mask & ~retained, torch.zeros_like(failure_mask) return invalidation, retry + def _phase_effect_gate_verifier( + self, + context: PlanningContext, + request: PhaseEffectGateRequest, + ) -> PhaseEffectGateResult: + """Observe one blocking segment-entry effect on a fresh due cycle.""" + grounded = self._require_grounded() + gates = tuple(getattr(grounded, "effect_gates", ())) + matches = tuple(value for value in gates if value.gate_id == request.gate_id) + if len(matches) != 1: + raise RuntimeError( + f"Grounded call must own exactly one phase-effect gate " + f"{request.gate_id!r}." + ) + gate = matches[0] + if gate.segment_name != request.segment_name: + raise ValueError( + "Phase-effect gate request segment does not match its grounded " + "monitor." + ) + session = self._require_runner().session + monitor_request = EffectVerificationRequest( + verification_id=self._next_gate_verification_id, + skill_id=request.skill_id, + invocation_id=request.invocation_id, + invocation_revision=request.invocation_revision, + invocation_index=request.invocation_index, + attempt_generation=request.attempt_generation, + terminal_segment=request.segment_name, + requested_at=request.requested_at, + deadline=request.deadline, + env_mask=request.env_mask, + expected_effects=self._phase_effect_gate_expected_effects( + gate, + session.active_plan.expected_effects, + ), + ) + self._next_gate_verification_id += 1 + decision = self._observe_effect_monitor( + context, + monitor_request, + spec=gate.effect_spec, + monitor=gate.effect_monitor, + boundary_kind="phase_effect_gate", + gate_id=gate.gate_id, + segment_name=gate.segment_name, + ) + return PhaseEffectGateResult( + verification_id=request.verification_id, + gate_id=request.gate_id, + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + retry_mask=( + decision.failure_mask + if gate.retry_action + else torch.zeros_like(decision.failure_mask) + ), + message=( + f"Physical evidence contradicted gate {gate.gate_id!r} before " + f"segment {gate.segment_name!r}." + if decision.failure_mask.any() + else "" + ), + ) + + @staticmethod + def _phase_effect_gate_expected_effects( + gate: GroundedPhaseEffectGate, + action_effects: StateDelta, + ) -> StateDelta: + """Project the action-owned held relation required by one gate.""" + expectation = gate.effect_spec.state_expectations[0] + if type(expectation) is not HeldObjectStateExpectation: + raise TypeError("Built-in phase-effect gates require held-object state.") + key = expectation.task_state_key + if key not in action_effects.held_object_updates: + raise ValueError(f"Active action does not declare gate state key {key!r}.") + candidate = action_effects.held_object_updates[key] + if expectation.relation is HeldObjectRelation.ATTACHED: + if not isinstance(candidate, HeldObjectState): + raise ValueError( + f"Attached gate {gate.gate_id!r} requires an action-owned " + "HeldObjectState candidate." + ) + elif candidate is not None: + raise ValueError( + f"Detached gate {gate.gate_id!r} requires an action-owned removal." + ) + return StateDelta(held_object_updates={key: candidate}) + def _held_object_guard_verifier( self, context: PlanningContext, @@ -2447,6 +2601,7 @@ def _observe_effect_monitor( monitor: EffectMonitor, boundary_kind: str = "terminal", guard_id: str | None = None, + gate_id: str | None = None, segment_name: str | None = None, ) -> EffectMonitorDecision: """Collect evidence, run one monitor, and append an auditable trace.""" @@ -2501,6 +2656,7 @@ def _observe_effect_monitor( evidence=evidence, boundary_kind=boundary_kind, guard_id=guard_id, + gate_id=gate_id, segment_name=segment_name, ) self._effect_traces.append(trace) diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index 6f1a5279c..2c566fca8 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -1388,8 +1388,8 @@ def _evidence_profile_binding() -> SimulationRobotSkillProfileBinding: def _pick_evidence_plan(action: Any, request: Any, context: Any) -> Any: """Build one grasp frame and an identity object-to-endpoint expectation.""" goal = action.require_goal(request) - trajectory = context.robot.qpos.unsqueeze(1).clone() - trajectory[:, 0, 1] = _HAND_GRASP_POSITION + trajectory = context.robot.qpos.unsqueeze(1).repeat(1, 2, 1) + trajectory[:, :, 1] = _HAND_GRASP_POSITION relation = torch.eye(4).repeat(context.batch_size, 1, 1) held = HeldObjectState( semantics=goal.semantics, @@ -1405,14 +1405,15 @@ def _pick_evidence_plan(action: Any, request: Any, context: Any) -> Any: held_object_updates={"manipulator": held}, ), replannable=False, + segment_lengths={"close": 1, "lift": 1}, scene_dependency_monitor_until={"cube": 0}, ) def _place_evidence_plan(action: Any, request: Any, context: Any) -> Any: """Build one open frame and the matching held-object removal delta.""" - trajectory = context.robot.qpos.unsqueeze(1).clone() - trajectory[:, 0, 1] = _HAND_OPEN_POSITION + trajectory = context.robot.qpos.unsqueeze(1).repeat(1, 2, 1) + trajectory[:, :, 1] = _HAND_OPEN_POSITION return action.build_plan( request, context, @@ -1422,6 +1423,7 @@ def _place_evidence_plan(action: Any, request: Any, context: Any) -> Any: held_object_updates={"manipulator": None}, ), replannable=False, + segment_lengths={"release": 1, "retract": 1}, ) @@ -1529,11 +1531,17 @@ def _sample_effect( """Advance one fresh environment tick and return its production trace.""" if advance_clock: assembly.clock.advance_after_env_step() - result = assembly.runtime.step() - assert len(result.effects) == expected_trace_count - while assembly.command_sink.pending_count: - _consume_buffered_action(assembly, robot) - return result, result.effects[-1] + for _ in range(4): + result = assembly.runtime.step() + while assembly.command_sink.pending_count: + _consume_buffered_action(assembly, robot) + if len(result.effects) == expected_trace_count: + return result, result.effects[-1] + assert len(result.effects) < expected_trace_count + assembly.clock.advance_after_env_step() + raise AssertionError( + f"Expected {expected_trace_count} effect traces, got {len(result.effects)}." + ) class _SynchronousEvidenceClock: @@ -1787,6 +1795,14 @@ def _run_evidence_pick_place( assert result.status is SkillStatus.COMPLETED assert verified_pick is not None assert result.task_state.get_held_object("manipulator") is None + assert { + (effect.call_index, effect.gate_id, effect.segment_name) + for effect in result.effects + if effect.boundary_kind == "phase_effect_gate" + } == { + (0, "destination_acquired", "lift"), + (1, "source_released", "retract"), + } return result, verified_pick @@ -2644,8 +2660,38 @@ def test_standard_factory_rejects_adapter_live_route_declaration_drift( def test_pick_place_effects_require_accepted_hand_state_and_live_pose() -> None: - """Production Pick/Place evidence stays conjunctive through runtime traces.""" + """Terminal Pick/Place evidence stays conjunctive through runtime traces.""" assembly, robot, cube = _evidence_runtime() + + def without_phase_gates( + self: Any, + analyzed: Any, + effect_spec: Any, + *, + path: tuple[Any, ...], + ) -> tuple[()]: + del self, analyzed, effect_spec, path + return () + + def without_in_flight_guards( + self: Any, + analyzed: Any, + effect_spec: Any, + context: Any, + *, + path: tuple[Any, ...], + ) -> tuple[()]: + del self, analyzed, effect_spec, context, path + return () + + assembly.compiler._ground_phase_effect_gates = MethodType( + without_phase_gates, + assembly.compiler, + ) + assembly.compiler._ground_held_object_guards = MethodType( + without_in_flight_guards, + assembly.compiler, + ) assert type(assembly.accepted_command_observer) is ( ControlCommandStateEvidenceTracker ) diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index 3386af1c7..9fad65547 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -59,6 +59,9 @@ ObjectSemantics, PlannerDiagnostics, PlanningContext, + PhaseEffectGateRequest, + PhaseEffectGateRequirement, + PhaseEffectGateResult, RecoveryPolicy, ResolvedActionRequest, RobotObservation, @@ -145,6 +148,32 @@ def _plan( ) +class PhaseGateAction(DynamicAction): + """Three-frame action with one gate before its terminal segment.""" + + skill_id: ClassVar[str] = "phase_gate" + binding_contract: ClassVar[SkillBindingContract] = DynamicAction.binding_contract + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + self.plan_count += 1 + self.requests.append(request) + pose = resolve_pose_goal(goal.xpos, context, name="xpos") + target = pose[:, 0, 3].unsqueeze(1).expand_as(context.robot.qpos) + midpoint = torch.lerp(context.robot.qpos, target, 0.5) + return self.build_plan( + request, + context, + success=True, + trajectory=torch.stack([context.robot.qpos, midpoint, target], dim=1), + segment_lengths={"prepare": 2, "commit": 1}, + ) + + class EffectAction(DynamicAction): """Dynamic test action that declares an attachment effect.""" @@ -583,6 +612,26 @@ def _held_object_loss_result( ) +def _phase_gate_result( + request: PhaseEffectGateRequest, + *, + success_mask: torch.Tensor, + failure_mask: torch.Tensor, + retry_mask: torch.Tensor | None = None, +) -> PhaseEffectGateResult: + """Build one result exactly correlated with a pending segment-entry gate.""" + return PhaseEffectGateResult( + verification_id=request.verification_id, + gate_id=request.gate_id, + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + success_mask=success_mask, + failure_mask=failure_mask, + retry_mask=(failure_mask.clone() if retry_mask is None else retry_mask), + ) + + def _multi_dependency_context( timestamp: float, *, @@ -710,6 +759,29 @@ def _destination_invocation( ) +def _phase_gate_invocation( + engine: AtomicActionEngine, + *, + segment_name: str = "commit", + max_action_retries: int = 2, +) -> ActionInvocation[EndEffectorPoseGoal]: + """Build a test invocation whose core owns one named segment gate.""" + base = _invocation( + engine, + skill_id=PhaseGateAction.skill_id, + max_action_retries=max_action_retries, + ) + return replace( + base, + phase_effect_gates=( + PhaseEffectGateRequirement( + gate_id="physical_ready", + segment_name=segment_name, + ), + ), + ) + + def _effect_session( *, batch_size: int = 1, @@ -776,6 +848,174 @@ def test_session_completes_incremental_command_sequence() -> None: assert final.eligible_mask.tolist() == [True] +@pytest.mark.parametrize( + ("segment_name", "message"), + (("missing", "missing segment"), ("prepare", "first trajectory segment")), +) +def test_phase_effect_gate_requires_a_noninitial_named_segment( + segment_name: str, + message: str, +) -> None: + engine, _ = _engine() + engine.register(PhaseGateAction()) + + with pytest.raises(ValueError, match=message): + engine.start( + (_phase_gate_invocation(engine, segment_name=segment_name),), + _context(0.0, 0.0, 0.2, 0), + ) + + +def test_unresolved_phase_effect_gate_replays_preceding_command_for_full_cohort() -> ( + None +): + engine, _ = _engine(batch_size=2) + action = PhaseGateAction() + engine.register(action) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) + session = engine.start((_phase_gate_invocation(engine),), initial) + + first = session.tick(initial) + boundary = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.4), 0)) + request = boundary.pending_phase_effect_gate + assert request is not None + assert request.gate_id == "physical_ready" + assert request.segment_name == "commit" + assert request.next_waypoint_index == 2 + assert request.env_mask.tolist() == [True, True] + assert torch.allclose(_joint_positions(first.command), torch.zeros(2, 2)) + predecessor = _joint_positions(boundary.command) + assert torch.allclose(predecessor, torch.tensor([[0.1, 0.1], [0.2, 0.2]])) + + unresolved = session.tick( + _context(0.2, (0.1, 0.2), (0.2, 0.4), 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([True, False]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert unresolved.status is ExecutionStatus.RUNNING + assert unresolved.pending_phase_effect_gate is not None + assert unresolved.pending_phase_effect_gate.verification_id == ( + request.verification_id + 1 + ) + assert unresolved.pending_phase_effect_gate.next_waypoint_index == 2 + assert torch.equal(_joint_positions(unresolved.command), predecessor) + assert unresolved.command is not None + assert unresolved.command.active_mask.tolist() == [True, True] + assert unresolved.task_state.held_objects == {} + kinds = [event.kind for event in (*boundary.events, *unresolved.events)] + assert kinds.count(ExecutionEventKind.PHASE_EFFECT_GATE_REQUIRED) == 1 + assert ExecutionEventKind.PHASE_EFFECT_GATE_SATISFIED not in kinds + assert action.plan_count == 1 + + +def test_phase_effect_gate_success_unlocks_segment_without_committing_task_state() -> ( + None +): + engine, _ = _engine(batch_size=2) + engine.register(PhaseGateAction()) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) + session = engine.start((_phase_gate_invocation(engine),), initial) + session.tick(initial) + boundary = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.4), 0)) + request = boundary.pending_phase_effect_gate + assert request is not None + + released = session.tick( + _context(0.2, (0.1, 0.2), (0.2, 0.4), 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([True, True]), + failure_mask=torch.tensor([False, False]), + ), + ) + + assert released.pending_phase_effect_gate is None + assert torch.allclose( + _joint_positions(released.command), + torch.tensor([[0.2, 0.2], [0.4, 0.4]]), + ) + assert released.task_state.held_objects == {} + satisfied = next( + event + for event in released.events + if event.kind is ExecutionEventKind.PHASE_EFFECT_GATE_SATISFIED + ) + assert satisfied.env_mask.tolist() == [True, True] + + +def test_phase_effect_gate_contradiction_retries_action_without_state_mutation() -> ( + None +): + engine, _ = _engine(batch_size=2) + action = PhaseGateAction() + engine.register(action) + initial = _context(0.0, (0.0, 0.0), (0.2, 0.4), 0) + session = engine.start( + (_phase_gate_invocation(engine, max_action_retries=1),), + initial, + ) + session.tick(initial) + boundary = session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.4), 0)) + request = boundary.pending_phase_effect_gate + assert request is not None + + retried = session.tick( + _context(0.2, (0.1, 0.2), (0.2, 0.4), 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([True, False]), + retry_mask=torch.tensor([True, False]), + ), + ) + + assert retried.status is ExecutionStatus.RUNNING + assert retried.pending_phase_effect_gate is None + assert retried.command is not None + assert retried.command.active_mask.tolist() == [True, True] + assert action.plan_count == 2 + assert session.plan_attempts[-1].attempt_generation == 1 + assert session.plan_attempts[-1].action_retry_counts == (1, 0) + assert retried.task_state.held_objects == {} + kinds = [event.kind for event in retried.events] + assert ExecutionEventKind.PHASE_EFFECT_GATE_FAILED in kinds + assert ExecutionEventKind.ACTION_RETRY in kinds + + +def test_stale_phase_effect_gate_result_is_rejected_after_unresolved_poll() -> None: + engine, _ = _engine() + engine.register(PhaseGateAction()) + initial = _context(0.0, 0.0, 0.2, 0) + session = engine.start((_phase_gate_invocation(engine),), initial) + session.tick(initial) + boundary = session.tick(_context(0.1, 0.0, 0.2, 0)) + request = boundary.pending_phase_effect_gate + assert request is not None + unresolved = session.tick( + _context(0.2, 0.1, 0.2, 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([False]), + failure_mask=torch.tensor([False]), + ), + ) + assert unresolved.pending_phase_effect_gate is not None + + with pytest.raises(ValueError, match="verification_id"): + session.tick( + _context(0.3, 0.1, 0.2, 0), + phase_effect_gate_result=_phase_gate_result( + request, + success_mask=torch.tensor([True]), + failure_mask=torch.tensor([False]), + ), + ) + + def test_held_object_loss_retries_only_failed_row_with_reconciled_state() -> None: engine, _ = _engine(batch_size=2) initial = _with_held_object(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index b10f273f5..a17d374cc 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -50,6 +50,9 @@ JointPositionTarget, MotionPolicy, ObjectSemantics, + PhaseEffectGateRequest, + PhaseEffectGateRequirement, + PhaseEffectGateResult, PlanningContextTrackingFeedbackProvider, PlanningContext, RecoveryPolicy, @@ -106,6 +109,27 @@ def _effect_result( ) +def _phase_gate_result( + request: PhaseEffectGateRequest, + *, + success: bool, + batch_size: int, +) -> PhaseEffectGateResult: + """Build one all-row gate decision correlated with a runner request.""" + success_mask = torch.full((batch_size,), success, dtype=torch.bool) + failure_mask = torch.zeros(batch_size, dtype=torch.bool) + return PhaseEffectGateResult( + verification_id=request.verification_id, + gate_id=request.gate_id, + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + success_mask=success_mask, + failure_mask=failure_mask, + retry_mask=failure_mask, + ) + + class FakeClock: """Deterministic clock used by non-blocking runner tests.""" @@ -298,9 +322,15 @@ class TimedAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): ) ) - def __init__(self, *, with_effect: bool = False) -> None: + def __init__( + self, + *, + with_effect: bool = False, + with_phase_gate: bool = False, + ) -> None: super().__init__() self.with_effect = with_effect + self.with_phase_gate = with_phase_gate self.plan_count = 0 def _plan( @@ -345,6 +375,9 @@ def _plan( success=True, trajectory=trajectory, expected_effects=effects, + segment_lengths=( + {"prepare": 2, "commit": 1} if self.with_phase_gate else None + ), ) @@ -359,6 +392,7 @@ def _timed_action_binding(action: TimedAction) -> ActionBinding: def _make_runner( *, with_effect: bool = False, + with_phase_gate: bool = False, batch_size: int = BATCH_SIZE, control_joint_ids: tuple[int, ...] | None = None, max_action_retries: int = 2, @@ -388,7 +422,10 @@ def _make_runner( generator.robot = robot generator.device = torch.device("cpu") generator.planner.cfg.planner_type = "stub" - action = TimedAction(with_effect=with_effect) + action = TimedAction( + with_effect=with_effect, + with_phase_gate=with_phase_gate, + ) engine = AtomicActionEngine(generator, tracking_runtime=tracking_runtime) engine.register(action) initial_task = TaskState.empty(batch_size, "cpu") @@ -405,6 +442,16 @@ def _make_runner( max_action_retries=max_action_retries, action_timeout=action_timeout, ), + phase_effect_gates=( + ( + PhaseEffectGateRequirement( + gate_id="physical_ready", + segment_name="commit", + ), + ) + if with_phase_gate + else () + ), ) session = engine.start((invocation,), initial_context) runner = ExecutionRunner( @@ -558,6 +605,97 @@ def verifier( assert "guard evidence unavailable" in failed.message +def test_runner_phase_gate_polls_fresh_state_and_replays_preceding_command() -> None: + runner, clock, _, sink, _ = _make_runner(with_phase_gate=True) + requests: list[tuple[float, PhaseEffectGateRequest]] = [] + + first = runner.step() + clock.advance(first.wait_duration) + boundary = runner.step() + assert boundary.tick is not None + assert boundary.tick.pending_phase_effect_gate is not None + clock.advance(boundary.wait_duration) + + def verifier( + context: PlanningContext, + request: PhaseEffectGateRequest, + ) -> PhaseEffectGateResult: + requests.append((context.robot.timestamp, request)) + return _phase_gate_result( + request, + success=len(requests) == 2, + batch_size=context.batch_size, + ) + + unresolved = runner.step(phase_effect_gate_verifier=verifier) + assert unresolved.tick is not None + assert unresolved.tick.pending_phase_effect_gate is not None + clock.advance(unresolved.wait_duration) + released = runner.step(phase_effect_gate_verifier=verifier) + + assert released.status is RunnerStatus.RUNNING + assert released.tick is not None + assert released.tick.pending_phase_effect_gate is None + assert [value[1].verification_id for value in requests] == [0, 1] + assert [value[1].next_waypoint_index for value in requests] == [2, 2] + assert [value[1].segment_name for value in requests] == ["commit", "commit"] + assert requests[0][0] < requests[1][0] + assert len(sink.sent) == 4 + boundary_payload = sink.sent[1].commands[0].payload + replay_payload = sink.sent[2].commands[0].payload + released_payload = sink.sent[3].commands[0].payload + assert isinstance(boundary_payload, JointPositionPayload) + assert isinstance(replay_payload, JointPositionPayload) + assert isinstance(released_payload, JointPositionPayload) + assert torch.equal(replay_payload.positions, boundary_payload.positions) + assert torch.allclose( + released_payload.positions, + torch.full((BATCH_SIZE, ROBOT_DOF), TARGET_POSITION), + ) + assert any( + event.kind is ExecutionEventKind.PHASE_EFFECT_GATE_SATISFIED + for event in released.tick.events + ) + + +def test_blocking_runner_returns_unverified_phase_gate_boundary() -> None: + runner, _, _, sink, _ = _make_runner(with_phase_gate=True) + + blocked = runner.run_until_blocked() + + assert blocked.status is RunnerStatus.RUNNING + assert blocked.tick is not None + assert blocked.tick.pending_phase_effect_gate is not None + assert blocked.tick.pending_phase_effect_gate.segment_name == "commit" + assert len(sink.sent) == 2 + + +def test_runner_phase_gate_verifier_exception_performs_safe_stop() -> None: + runner, clock, _, sink, _ = _make_runner(with_phase_gate=True) + first = runner.step() + clock.advance(first.wait_duration) + boundary = runner.step() + clock.advance(boundary.wait_duration) + + def verifier( + context: PlanningContext, + request: PhaseEffectGateRequest, + ) -> PhaseEffectGateResult: + del context, request + raise RuntimeError("gate evidence unavailable") + + failed = runner.step(phase_effect_gate_verifier=verifier) + + assert failed.status is RunnerStatus.FAILED + assert [dispatch.operation for dispatch in failed.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert failed.message is not None + assert "gate evidence unavailable" in failed.message + + def test_runner_dispatches_transport_neutral_endpoint_frames() -> None: runner, _, _, sink, _ = _make_runner() diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index c6328310d..005674de9 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -763,6 +763,17 @@ def test_pick_effect_spec_binds_destination_and_fresh_monitor_per_grounding() -> assert guard.effect_monitor is not first.effect_monitor assert guard.effect_spec.effect_kind is SemanticEffectKind.ATTACH assert repeated.effect_guards[0].effect_monitor is not guard.effect_monitor + assert len(first.effect_gates) == 1 + gate = first.effect_gates[0] + assert gate.gate_id == "destination_acquired" + assert gate.segment_name == "lift" + assert gate.retry_action is True + assert gate.effect_monitor is not first.effect_monitor + assert gate.effect_monitor is not guard.effect_monitor + assert gate.effect_spec.state_expectations[0].expectation_id == "destination" + assert gate.effect_spec.effect_kind is SemanticEffectKind.ATTACH + assert first.invocation.phase_effect_gates == (gate.requirement,) + assert repeated.effect_gates[0].effect_monitor is not gate.effect_monitor def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: @@ -828,6 +839,17 @@ def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: assert guard_pose.baseline_object_to_endpoint is None assert isinstance(guard_constraint, BinaryEffectClause) assert guard_constraint.expected is True + assert len(grounded.effect_gates) == 1 + gate = grounded.effect_gates[0] + assert gate.gate_id == "source_released" + assert gate.segment_name == "retract" + assert gate.retry_action is True + assert gate.effect_spec.effect_kind is SemanticEffectKind.RELEASE + gate_relation = gate.effect_spec.state_expectations[0] + assert isinstance(gate_relation, HeldObjectStateExpectation) + assert gate_relation.expectation_id == "source" + assert gate_relation.relation is HeldObjectRelation.DETACHED + assert grounded.invocation.phase_effect_gates == (gate.requirement,) def test_handover_effect_spec_binds_source_and_destination_relations() -> None: @@ -922,6 +944,17 @@ def test_handover_effect_spec_binds_source_and_destination_relations() -> None: assert destination_guard.task_state_key == "right" assert destination_guard.invalidation_task_state_keys == ("left", "right") assert destination_guard.retry_action is False + assert len(grounded.effect_gates) == 1 + gate = grounded.effect_gates[0] + assert gate.gate_id == "destination_acquired" + assert gate.segment_name == "release" + assert gate.retry_action is True + assert gate.effect_monitor is not destination_guard.effect_monitor + gate_relation = gate.effect_spec.state_expectations[0] + assert isinstance(gate_relation, HeldObjectStateExpectation) + assert gate_relation.expectation_id == "destination" + assert gate_relation.relation is HeldObjectRelation.ATTACHED + assert grounded.invocation.phase_effect_gates == (gate.requirement,) def test_registered_call_without_monitor_has_no_effect_contract() -> None: @@ -953,6 +986,8 @@ def test_registered_call_without_monitor_has_no_effect_contract() -> None: assert workflow.calls[0].effect_monitor_ref is None assert grounded.effect_spec is None assert grounded.effect_monitor is None + assert grounded.effect_gates == () + assert grounded.invocation.phase_effect_gates == () options = grounded.invocation.skill_options assert type(options) is PickUpOptions assert options.pre_grasp_distance == 0.07 diff --git a/tests/sim/skills/test_runtime.py b/tests/sim/skills/test_runtime.py index 0f912ac38..cc8c977fb 100644 --- a/tests/sim/skills/test_runtime.py +++ b/tests/sim/skills/test_runtime.py @@ -47,6 +47,7 @@ JointPositionTarget, MotionPolicy, ObjectSemantics, + PhaseEffectGateRequest, PlanningContext, RecoveryPolicy, ResolvedActionRequest, @@ -63,6 +64,7 @@ from embodichain.lab.sim.skills.calls import HandOver, Place, RegisteredSemanticCall from embodichain.lab.sim.skills.compiler import ( GroundedHeldObjectGuard, + GroundedPhaseEffectGate, HeldObjectGuardBaseline, SemanticSkillCompiler, ) @@ -801,6 +803,120 @@ def test_in_flight_guard_collects_live_evidence_and_builds_loss_reconciliation() assert torch.equal(system.collector.calls[0][2], torch.tensor([0, 1])) +def test_phase_effect_gate_uses_independent_monitor_and_records_boundary_trace() -> ( + None +): + system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="object", + entity_id="cube", + ) + poses = torch.eye(4).repeat(BATCH_SIZE, 1, 1) + held = HeldObjectState( + semantics=semantics, + object_to_eef=poses, + grasp_xpos=poses, + env_mask=_mask(True, True), + ) + expectation = HeldObjectStateExpectation( + expectation_id="destination", + relation=HeldObjectRelation.ATTACHED, + object_id="cube", + slot_id="primary", + resource_id="arm", + task_state_key="arm", + ) + spec = SemanticEffectSpec( + semantic_id="pick", + effect_kind=SemanticEffectKind.ATTACH, + skill_id="pick_up", + invocation_id="workflow:0", + invocation_revision=0, + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + state_expectations=(expectation,), + clauses=( + BinaryEffectClause( + clause_id="destination.constraint", + expectation_id="destination", + source=EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress("hand", "constraint"), + ), + evidence_kind=BinaryEvidenceKind.CONSTRAINT, + expected=True, + ), + ), + ) + terminal_monitor = _DecisionMonitor( + spec, + EffectMonitorDecision(_mask(True, True), _mask(False, False)), + ) + gate_monitor = _DecisionMonitor( + spec, + EffectMonitorDecision(_mask(False, True), _mask(True, False)), + ) + gate = GroundedPhaseEffectGate( + gate_id="destination_acquired", + segment_name="lift", + effect_spec=spec, + effect_monitor=gate_monitor, + retry_action=True, + ) + system.runtime._grounded = SimpleNamespace( + analyzed=SimpleNamespace(effect_monitor_ref=None), + effect_monitor=terminal_monitor, + effect_gates=(gate,), + ) + system.runtime._runner = SimpleNamespace( + session=SimpleNamespace( + active_plan=SimpleNamespace( + expected_effects=StateDelta(held_object_updates={"arm": held}) + ) + ) + ) + system.runtime._current_call_index = 0 + context = system.observation.observe(TaskState.empty(BATCH_SIZE, "cpu")) + request = PhaseEffectGateRequest( + verification_id=7, + gate_id="destination_acquired", + skill_id="pick_up", + invocation_id="workflow:0", + invocation_revision=0, + invocation_index=0, + attempt_generation=3, + next_waypoint_index=4, + segment_name="lift", + requested_at=0.0, + deadline=10.0, + env_mask=_mask(True, True), + ) + + result = system.runtime._phase_effect_gate_verifier(context, request) + + assert result.verification_id == 7 + assert result.gate_id == "destination_acquired" + assert result.attempt_generation == 3 + assert result.next_waypoint_index == 4 + assert torch.equal(result.success_mask, _mask(False, True)) + assert torch.equal(result.failure_mask, _mask(True, False)) + assert torch.equal(result.retry_mask, _mask(True, False)) + assert terminal_monitor.calls == 0 + assert gate_monitor.calls == 1 + assert gate_monitor.requests[0].terminal_segment == "lift" + candidate = gate_monitor.requests[0].expected_effects.held_object_updates["arm"] + assert isinstance(candidate, HeldObjectState) + assert candidate.semantics.entity_id == "cube" + trace = system.runtime._effect_traces[0] + assert trace.boundary_kind == "phase_effect_gate" + assert trace.guard_id is None + assert trace.gate_id == "destination_acquired" + assert trace.segment_name == "lift" + assert torch.equal(system.collector.calls[0][2], torch.tensor([0, 1])) + + def test_result_metadata_is_json_safe_and_contains_typed_runtime_trace() -> None: system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),))