diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index ec1489b17..e9adf2285 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -442,15 +442,27 @@ asynchronous integrations instead pass `effect_result` explicitly on a due `step()` call. ```python +import torch + request = tick.pending_effect effect_result = EffectVerificationResult( verification_id=request.verification_id, success_mask=observed_success, failure_mask=observed_failure, + invalidation_mask=observed_failure, + retry_mask=torch.zeros_like(observed_failure), ) result = runner.step(effect_result=effect_result) ``` +Both failure-policy masks must be subsets of `failure_mask`. +`invalidation_mask` selects rows on which the core applies the request-owned, +removal-only `failure_invalidation` delta; it does not let the verifier inject +state. `retry_mask` is reserved for rows whose physical preconditions still +make replay of the same invocation valid. Other failed rows require external +recovery. Unresolved evidence at the action deadline is reconciled fail-closed +when the pending effect covers active verified state. + The semantic layer keeps physical observation separate from symbolic effect commit. `SkillPolicyPreset.effect_monitors` maps exact semantic call IDs to versioned, bounded-declarative `EffectMonitorRef` values. Omitting the mapping @@ -472,6 +484,13 @@ command-only `RuntimeEndpointTarget`. This keeps motion, mobile, whole-body, articulation, and custom controller transports extensible without treating a control part as symbolic state identity. +`HeldObjectState` is verified symbolic knowledge only. Neither it nor the +standard effect runtime creates simulator joints, managed attachments, +kinematic parents, frozen bodies, or pose overrides. Physical grasp retention +therefore depends on the configured controller, collision geometry, materials, +contact solver, and rigid-body parameters. A command-state evidence value is +only accepted controller intent and never physical contact proof by itself. + Providers emit raw `PoseRelationEvidenceBatch`, `BinaryEffectEvidenceBatch`, `ScalarEffectEvidenceBatch`, or `JointStateEvidenceBatch` values with stable environment IDs, per-row validity/acquisition diagnostics, timestamps, and @@ -491,6 +510,17 @@ Cause events (`ACTION_PLANNING_FAILED`, `EFFECT_VERIFICATION_FAILED`, and `EFFECT_VERIFICATION_TIMEOUT`) are distinct from the `ACTION_RETRY` recovery event. `SESSION_COMPLETED` and `SESSION_FAILED` are distinct terminal events. +Effect verification is currently a terminal action boundary. There is no +in-flight physical-invariant monitor for the held-object relation during Pick +lift or HandOver transfer/release/delivery. A slip can therefore be detected at +the terminal monitor but cannot interrupt the trajectory at the frame where it +occurs. On a failed HandOver, the success-only `StateDelta` is not committed, +but an already verified source-held relation also is not reconciled from +failure evidence; blindly retrying after both grippers lost the object can use +stale symbolic state. Pure-dynamics recovery needs a typed, phase-aware +in-flight guard plus failure-outcome reconciliation rather than a simulator-side +attachment. + Recovery replans reuse the current immutable `ResolvedActionRequest`, including its owned goal snapshot. Mutable goal values are copied, while simulator-backed `BatchEntity` handles retain their runtime identity. To change a goal, option, @@ -693,7 +723,13 @@ profile catalog rather than duplicate robot data across tasks. The Open Drawer vertical slice has completed its supported-simulation physical run and reached the configured drawer joint target. Repeated cube pick/place has completed one physical Pick/Place/settle/validator cycle; the full three-cycle -run remains in threshold calibration. +run remains in threshold calibration. The dual-UR5/PGI HandOver slice has +completed three consecutive supported-simulation Pick/transfer/settle/validator +runs using contact dynamics only. Its calibrated profile drives only the PGI +master joints, keeps mimic-child drives disabled, uses a 0.011 close target with +stiffness 2000, damping 50, and maximum effort 140, models the can at 0.33 kg, +and uses 200 motion samples. The default 0.05-rad tracking gate and bounded +replanning remain active. When no explicit contact or constraint callback is installed, simulation grasp and release evidence combines the live object-to-endpoint pose relation with @@ -707,9 +743,28 @@ contact by itself. `DynamicSettleMonitor` is shared by reset events and the Expert Program `wait_stable` post-policy. It owns threshold, cadence, consecutive-check, -settled, and timeout state but never steps simulation. The demo policy yields -full-qpos holds through the normal environment step path. Segment validators -remain a separate dataset/task boundary. +settled, and timeout state but never steps simulation. Eligible rows reuse live +target qpos so a contact-blocked position gripper retains closure preload; +initially inactive rows use fresh measured-qpos holds. Early-settled eligible +rows keep their targets until the active cohort terminates. Every action still +passes through the normal environment-step path, and segment validators remain +a separate dataset/task boundary. + +The standard simulation factory lowers both `MotionPolicy.control_dt` and +`ExecutionRunnerCfg.minimum_cycle_time` to the authoritative Gym `step_dt`. +When `hold_during_effect_verification=False`, runner polling emits no +observed-position HOLD; the bridge advances physics by replaying the last +accepted environment action. HandOver also sets `hold_on_completion=False`, so +its subsequent `wait_stable` policy continues the existing targets rather than +neutralizing the gripper at its contact-displaced qpos. Cancellation and +failure still perform cancel followed by an observed-position safe hold. + +This staged B behavior solves the validated joint-position HandOver path but is +not a generic continuation contract for mobile-base or whole-body transports. +Those endpoints need a typed transport-owned continuation command rather than a +joint-qpos latch. `wait_stable` also runs only after terminal effect verification +and symbolic-state commit, so its timeout is a post-policy failure and does not +trigger atomic-action recovery. Runtime and demo results expose deterministic JSON-safe metadata. Call traces include invocation identity, masks, command counts, execution/recovery events, diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index e52240826..aa2726ee4 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -6,8 +6,15 @@ tests are explicitly enabled. Open Drawer has completed its supported-simulation physical run; repeated cube pick/place has completed one Pick/Place/settle/validator cycle, while the full three-cycle run remains in - threshold calibration. -- Baseline: `main@bcccb787e8f9165e9c8acf6f39f165ba6ac752a4` + threshold calibration. Dual-UR5/PGI HandOver has completed three consecutive + supported-simulation Pick/transfer/settle/validator runs using contact + 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. Preset-owned, row-local + workflow recovery now executes a real re-acquisition `Pick` when the source + relation is lost, or directly retries the failed semantic call when verified + state proves the source relation remains. +- Baseline: `main@bcccb787dcafdafd7b944ba210e5e85f9cd1d0cb` - Last updated: 2026-08-11 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), [#474](https://github.com/DexForce/EmbodiChain/issues/474) @@ -423,7 +430,8 @@ an `arm + tool` schema. It contains a generic resource DAG: embodiment data owned by generic profile IDs selected by each endpoint adapter; only the current core bridge lowers applicable profiles to robot control-part keys; -- versioned `SkillPolicyPreset` values own motion, recovery, and runner policy; +- versioned `SkillPolicyPreset` values own motion, atomic recovery, bounded + workflow recovery, and runner policy; - per-skill defaults map every skill-local slot to one resource ID. Resource and endpoint declarations are owned snapshots. A custom endpoint with @@ -586,6 +594,90 @@ handover, and articulation-joint progress. Hardware can implement the same contract with perception, force, or controller feedback. Custom monitors stay an advanced extension point. +Grasp and handover must remain real dynamics outcomes. A simulation effect +monitor is observational: it must not create a fixed joint, managed attachment, +kinematic parent, frozen body, or pose override to make a held-object relation +persist. Grasp retention comes from embodiment-owned drive settings, collision +geometry, material/contact parameters, solver settings, and the commands sent +through the normal controller path. An accepted semantic ``grasp`` command is +controller intent, not physical proof. + +``HeldObjectState`` records a relation only after live physical evidence has +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 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 +clause reached its inverse band. ``Pick`` can use the existing bounded action +retry. ``Place`` retries only when that complete inverse proof shows the source +is still attached; otherwise it invalidates the relation and emits a typed +``RECOVERY_REQUIRED`` boundary. ``HandOver`` always hands terminal failure to +workflow recovery, retaining the source relation only when complete inverse +evidence proves it is still attached. A verifier selects row-local retry versus +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 is owned by the same ``SkillRuntime`` and may not repair the +scene implicitly. ``SkillPolicyPreset`` schema version 3 adds a +``WorkflowRecoveryPolicy`` whose per-row attempt budget defaults to zero. The +runtime consults it only after the atomic core emits ``RECOVERY_REQUIRED``. A +row whose reconciled ``TaskState`` still proves the source held-object relation +retries the failed semantic call from a fresh observation. A row whose source +relation was invalidated executes a real semantic ``Pick`` using the failed +call's resolved source resource, then retries the original call. Each recovery +call receives normal analysis, grounding, planning, command dispatch, physical +effect verification, and trace metadata; it is not a state edit or simulator +repair. Attempts are bounded independently per row, while already successful +rows wait at the existing shared call barrier. This is runtime policy, not an +Expert Program ``Retry`` node or a second workflow executor. + +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 +``minimum_cycle_time`` to the authoritative Gym step. A persistent +joint-position task may disable observed-position holds during terminal effect +verification and on successful completion; bridge wait steps then replay the +last accepted environment action, and a following ``wait_stable`` policy keeps +eligible rows on their live drive targets. Cancellation and failure retain the +normal cancel-then-observed-position safe stop. This split preserves physical +gripper preload without converting a success continuation into a universal +safe-state policy. + +The validated dual-UR5/PGI slice drives only each PGI master joint (the mimic +child drive is disabled), uses a ``0.011`` close target with +``stiffness=2000``, ``damping=50``, and ``max_effort=140``, models the can at +``0.33 kg``, and executes a 200-sample motion policy. These values are task and +embodiment calibration, not effect-monitor success shortcuts: the normal +``0.05 rad`` tracking gate, bounded replanning, physical effect evidence, and +settling thresholds remain enabled. + +The B policy is the complete continuation scope of this refactor. A generic +mobile-base or whole-body continuation abstraction is deliberately excluded +from the implementation plan and acceptance checklist. If a later transport +requires persistence beyond its normal command contract, it should be proposed +and validated independently instead of becoming a blocker for the declarative +expert-program rollout. + ## 8. Expert Program configuration ### 8.1 Version 1 schema @@ -1165,10 +1257,19 @@ 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. Physical simulation -acceptance is partial: Open Drawer and one cube Pick/Place/settle/validator -cycle have completed, while the full repeated-cube run and embodiment-owned -HandOver pose integration remain validation work. +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 +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, fail-closed deadline +reconciliation, and blocking named-segment effect gates are implemented. +Workflow-level re-acquisition is implemented through the preset-owned bounded +policy and canonical runtime. Real-simulation fault-injection coverage and the +full repeated-cube run remain validation work. Deliverables: @@ -1271,7 +1372,7 @@ migration is outside the current scope because it would require modifying Action Bank code. The current follow-up also makes task registration the sole standard-runtime -extension owner. `SkillPolicyPreset` schema version 2 requires exact typed +extension owner. `SkillPolicyPreset` schema version 3 requires exact typed action-option templates for every reachable semantic call; lowering may fill only explicitly compiler-owned dynamic target fields. Endpoint adapters, ordered Gym transports, and a parallel-safety factory are declared on @@ -1416,7 +1517,21 @@ The design is complete when all of the following hold: goals without caller duplication. - [x] `Place` is object-centric and consumes verified held-object state. - [ ] Built-in grasp, release, handover, and supported articulation effect - monitors work in simulation. + monitors work in simulation. The dual-UR5/PGI HandOver vertical slice is + physically validated; remaining skill/embodiment coverage keeps this + aggregate item open. +- [ ] Grasp and handover simulation gates retain objects through configured + drive/contact dynamics only; no monitor or runtime path creates a + 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 segment-aware observation, + row-local core-owned invalidation, per-expectation terminal + reconciliation, fail-closed deadline handling, bounded Pick/retained-Place + retry, typed recovery boundary, and blocking acquisition/release gates are + implemented. Preset-owned per-row workflow re-acquisition now performs + real `Pick` and semantic-call retries; real-simulation fault injection + remains 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 6ad3ae46c..63fb3a5de 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -775,6 +775,8 @@ At the terminal waypoint, an `ExecutionSession` requests an external, correlated per-environment result before committing a non-empty effect: ```python +import torch + from embodichain.lab.sim.atomic_actions import EffectVerificationResult tick = session.tick(latest_context) @@ -785,6 +787,8 @@ if tick.pending_effect is not None: verification_id=request.verification_id, success_mask=success_mask, failure_mask=failure_mask, + invalidation_mask=failure_mask, + retry_mask=torch.zeros_like(failure_mask), ) tick = session.tick(latest_context, effect_result=effect_result) ``` @@ -797,6 +801,14 @@ and failure masks are disjoint subsets of the request mask; omitted request rows remain unresolved. Request IDs change after mask shrinkage or whole-action retry, so a delayed result cannot commit a newer attempt. +Every result also classifies failed rows with `invalidation_mask` and +`retry_mask`, both subsets of `failure_mask`. Invalidation applies the +request's core-owned removal-only `failure_invalidation`; the verifier cannot +inject replacement state. Retry is valid only when the same invocation's +physical preconditions remain satisfied. Failed rows outside `retry_mask` +enter external recovery, and unresolved evidence at the action deadline removes +covered active verified state before recovery. + `request.deadline` is expressed in the robot-observation timestamp domain. `RecoveryPolicy.action_timeout` covers both trajectory execution and the terminal effect wait; a retry invalidates the old request ID. With @@ -825,6 +837,40 @@ 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 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 +action-owned symbolic key/object identities that may be invalidated. A +contradictory result must name that canonical object and carry a removal-only +`StateDelta`; `ExecutionSession` applies that delta to only the failed rows +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 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 An MLLM should not construct `ActionInvocation` by copying arbitrary JSON into diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index adceebbed..5cd038ce2 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -489,6 +489,8 @@ correlated per-environment verification result: .. code-block:: python + import torch + from embodichain.lab.sim.atomic_actions import EffectVerificationResult def verify_effect(context, request): @@ -497,6 +499,8 @@ correlated per-environment verification result: verification_id=request.verification_id, success_mask=success_mask, failure_mask=failure_mask, + invalidation_mask=failure_mask, + retry_mask=torch.zeros_like(failure_mask), ) result = runner.run_until_blocked(effect_verifier=verify_effect) @@ -518,6 +522,8 @@ can later resume from the *current* pending request: verification_id=request.verification_id, success_mask=success_mask, failure_mask=failure_mask, + invalidation_mask=failure_mask, + retry_mask=torch.zeros_like(failure_mask), ) resumed = runner.step(effect_result=verified) if resumed.is_waiting: @@ -536,7 +542,41 @@ terminal effect wait. A result submitted after timeout cannot satisfy the new retry attempt because its old ID is invalid. The runner remembers the pending boundary even though the session emits its event only once. The durable state is ``tick.pending_effect`` (an ``EffectVerificationRequest``), not the presence of -that one-time event. +that one-time event. ``invalidation_mask`` and ``retry_mask`` must both be +subsets of ``failure_mask``. Invalidation selects rows for the request's +core-owned, removal-only ``failure_invalidation`` delta; a verifier cannot +publish arbitrary replacement state. Set a retry row only when replaying the +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/gym/envs/expert_program/__init__.py b/embodichain/lab/gym/envs/expert_program/__init__.py index a3cae5178..43eec1892 100644 --- a/embodichain/lab/gym/envs/expert_program/__init__.py +++ b/embodichain/lab/gym/envs/expert_program/__init__.py @@ -152,6 +152,7 @@ SimulationPlanningObservationProvider, create_simulation_expert_program_adapter, ) +from .simulation_handover import ConfiguredHandOverPoseProvider from .simulation_policies import ( SimulationSegmentPolicyPort, default_simulation_settle_presets, @@ -183,6 +184,7 @@ "ControlPartCommandPreset", "ControlPartEndpointBinding", "ControlPartResourceBinding", + "ConfiguredHandOverPoseProvider", "CyclicPoseTargetCfg", "DeclarativeCfgValue", "DemoBridgeError", diff --git a/embodichain/lab/gym/envs/expert_program/catalog.py b/embodichain/lab/gym/envs/expert_program/catalog.py index 8db7f334a..b4d2f17ee 100644 --- a/embodichain/lab/gym/envs/expert_program/catalog.py +++ b/embodichain/lab/gym/envs/expert_program/catalog.py @@ -1092,22 +1092,26 @@ def validate_bound_endpoint_extensions( ) -def _profile_with_control_dt( +def _profile_with_step_dt( profile: RobotSkillProfile, *, - control_dt: float, + step_dt: float, ) -> RobotSkillProfile: - """Return the registration profile aligned to one Gym control cadence.""" + """Return the registration profile aligned to one Gym runtime cadence.""" return replace( profile, presets={ preset_id: SkillPolicyPreset( preset_id=preset.preset_id, schema_version=preset.schema_version, - motion_policy=replace(preset.motion_policy, control_dt=control_dt), + motion_policy=replace(preset.motion_policy, control_dt=step_dt), tracking_policy=preset.tracking_policy, recovery_policy=preset.recovery_policy, - runner_cfg=preset.runner_cfg, + workflow_recovery_policy=preset.workflow_recovery_policy, + runner_cfg=replace( + preset.runner_cfg, + minimum_cycle_time=step_dt, + ), effect_monitors=preset.effect_monitors, action_option_templates=preset.action_option_templates, ) @@ -1476,9 +1480,9 @@ def validate_robot_profile( self.assert_unchanged() if type(profile) is not RobotSkillProfile: raise TypeError("profile must be exactly RobotSkillProfile.") - expected = _profile_with_control_dt( + expected = _profile_with_step_dt( self.catalog.robot_profile, - control_dt=step_dt, + step_dt=step_dt, ) if _canonical_json(profile) != _canonical_json(expected): raise IntegrationFingerprintMismatch( diff --git a/embodichain/lab/gym/envs/expert_program/simulation_environment.py b/embodichain/lab/gym/envs/expert_program/simulation_environment.py index abcacd901..37718121d 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_environment.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_environment.py @@ -718,10 +718,11 @@ class SimulationExpertProgramFactory(ExpertProgramEnvironmentFactory): translation_threshold: Material scene translation threshold. rotation_threshold: Material scene rotation threshold. - Every profile policy is rebuilt with ``control_dt == step_dt``. The Gym - cadence is authoritative because commands cannot be emitted between - environment steps; silently retaining a preset's unrelated fallback - cadence would make trajectory timing unrepresentable at the bridge. + Every profile policy is rebuilt with ``control_dt == step_dt`` and + ``minimum_cycle_time == step_dt``. The Gym cadence is authoritative because + commands and fresh feedback cannot be produced between environment steps; + silently retaining a preset's unrelated fallback cadence would make runtime + timing unrepresentable at the bridge. """ def __init__( @@ -872,7 +873,7 @@ def create_scene_registry(self) -> SceneRegistry: return registry def create_robot_skill_profile(self) -> RobotSkillProfile: - """Build a profile whose every motion policy uses the Gym cadence.""" + """Build a profile whose motion and runner policies use Gym cadence.""" profile = self._robot_profile_binding.build(self._robot) aligned_presets = { preset_id: SkillPolicyPreset( @@ -884,7 +885,11 @@ def create_robot_skill_profile(self) -> RobotSkillProfile: ), tracking_policy=preset.tracking_policy, recovery_policy=preset.recovery_policy, - runner_cfg=preset.runner_cfg, + workflow_recovery_policy=preset.workflow_recovery_policy, + runner_cfg=replace( + preset.runner_cfg, + minimum_cycle_time=self._step_dt, + ), effect_monitors=preset.effect_monitors, action_option_templates=preset.action_option_templates, ) @@ -893,9 +898,10 @@ def create_robot_skill_profile(self) -> RobotSkillProfile: aligned = replace(profile, presets=aligned_presets) if any( preset.motion_policy.control_dt != self._step_dt + or preset.runner_cfg.minimum_cycle_time != self._step_dt for preset in aligned.presets.values() ): - raise AssertionError("Profile motion policies were not cadence-aligned.") + raise AssertionError("Profile runtime policies were not cadence-aligned.") self._registration.validate_robot_profile( aligned, step_dt=self._step_dt, diff --git a/embodichain/lab/gym/envs/expert_program/simulation_handover.py b/embodichain/lab/gym/envs/expert_program/simulation_handover.py new file mode 100644 index 000000000..9ac4d6f50 --- /dev/null +++ b/embodichain/lab/gym/envs/expert_program/simulation_handover.py @@ -0,0 +1,146 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Configured simulation integration for semantic hand-over poses.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import TYPE_CHECKING, ClassVar + +from embodichain.lab.sim.skills import ( + HandOverPoseProvider, + HandOverPoseTargets, + SemanticObjectTarget, + SemanticPose, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.atomic_actions import PlanningContext + from embodichain.lab.sim.skills import BoundSemanticCall, HandOver + + +def _validated_pose( + position: tuple[float, float, float], + quaternion_wxyz: tuple[float, float, float, float], + *, + field_name: str, +) -> SemanticPose: + """Build and validate one unbatched semantic pose declaration.""" + if type(position) is not tuple or len(position) != 3: + raise TypeError(f"{field_name}_position must be an exact 3-tuple.") + if type(quaternion_wxyz) is not tuple or len(quaternion_wxyz) != 4: + raise TypeError(f"{field_name}_quaternion_wxyz must be an exact 4-tuple.") + try: + return SemanticPose( + position=position, + quaternion_wxyz=quaternion_wxyz, + ) + except (TypeError, ValueError) as exc: + raise type(exc)(f"Invalid {field_name} hand-over pose: {exc}") from exc + + +@dataclass(frozen=True, slots=True) +class ConfiguredHandOverPoseProvider(HandOverPoseProvider): + """Resolve hand-over targets from immutable embodiment configuration. + + The provider carries object-space poses rather than arm trajectories. The + shared semantic compiler and atomic ``HandOver`` implementation remain + responsible for grasp selection, IK, motion generation, transfer, release, + and delivery. Keeping the numeric declaration as tuple fields also makes + the provider suitable for task-registration catalog fingerprinting. + + Args: + middle_position: World-frame object position at transfer time. + middle_quaternion_wxyz: World-frame object orientation at transfer time. + final_position: World-frame object delivery position. + final_quaternion_wxyz: World-frame object delivery orientation. + """ + + provider_id: ClassVar[str] = "simulation.configured_handover_pose" + + middle_position: tuple[float, float, float] + middle_quaternion_wxyz: tuple[float, float, float, float] + final_position: tuple[float, float, float] + final_quaternion_wxyz: tuple[float, float, float, float] + + def __post_init__(self) -> None: + middle = _validated_pose( + self.middle_position, + self.middle_quaternion_wxyz, + field_name="middle", + ) + final = _validated_pose( + self.final_position, + self.final_quaternion_wxyz, + field_name="final", + ) + object.__setattr__( + self, + "middle_position", + tuple(float(value) for value in middle.position.tolist()), + ) + object.__setattr__( + self, + "middle_quaternion_wxyz", + tuple(float(value) for value in middle.quaternion_wxyz.tolist()), + ) + object.__setattr__( + self, + "final_position", + tuple(float(value) for value in final.position.tolist()), + ) + object.__setattr__( + self, + "final_quaternion_wxyz", + tuple(float(value) for value in final.quaternion_wxyz.tolist()), + ) + + def resolve( + self, + call: HandOver, + *, + context: PlanningContext, + bound: BoundSemanticCall, + ) -> HandOverPoseTargets: + """Return independently owned object-space transfer targets. + + Args: + call: Canonical hand-over semantic call. + context: Latest immutable planning observation. + bound: Engine/profile-bound hand-over call. + + Returns: + Configured middle and final object-space targets. + """ + del call, context, bound + return HandOverPoseTargets( + middle=SemanticObjectTarget( + pose=SemanticPose( + position=self.middle_position, + quaternion_wxyz=self.middle_quaternion_wxyz, + ) + ), + final=SemanticObjectTarget( + pose=SemanticPose( + position=self.final_position, + quaternion_wxyz=self.final_quaternion_wxyz, + ) + ), + ) + + +__all__ = ["ConfiguredHandOverPoseProvider"] diff --git a/embodichain/lab/gym/envs/expert_program/simulation_policies.py b/embodichain/lab/gym/envs/expert_program/simulation_policies.py index 408e7cac0..8795ce03a 100644 --- a/embodichain/lab/gym/envs/expert_program/simulation_policies.py +++ b/embodichain/lab/gym/envs/expert_program/simulation_policies.py @@ -19,8 +19,10 @@ The port in this module deliberately consumes the same explicit :class:`SimulationSceneBinding` used to construct the semantic scene registry. It never scans a simulation or guesses a native entity from a canonical name. -Post-policy actions are full-qpos holds and therefore remain inside the normal -Gym ``env.step()`` path owned by :class:`AtomicDemoBridge`. +Post-policy actions remain inside the normal Gym ``env.step()`` path owned by +:class:`AtomicDemoBridge`. Rows eligible for the policy reuse full drive targets +so physical contact does not erase position-control preload; rows already +inactive use fresh measured-position holds. """ from __future__ import annotations @@ -92,7 +94,8 @@ class SimulationSegmentPolicyPort: Args: simulation: Live simulation used only for UIDs declared in ``scene_binding``. - robot: Live robot used to produce controller-safe full-qpos holds. + robot: Live robot used to produce full target-qpos holds while the + post-policy observes settling. scene_binding: Exact canonical-to-native scene declaration. settle_presets: Named settling policies. ``None`` installs the shared ``rigid_object`` preset. @@ -116,7 +119,9 @@ def __init__( ) -> None: if type(scene_binding) is not SimulationSceneBinding: raise TypeError("scene_binding must be exactly SimulationSceneBinding.") - qpos = self._read_robot_qpos(robot) + qpos = self._read_robot_qpos(robot, target=False) + self._robot_qpos_shape = qpos.shape + self._robot_qpos_device = qpos.device if env_ids is None: env_ids = torch.arange( qpos.shape[0], @@ -221,7 +226,7 @@ def actions( segment: Any, active_mask: torch.Tensor, ) -> Iterator[torch.Tensor]: - """Yield full-qpos hold actions until active rows settle or time out. + """Yield full target-qpos hold actions until rows settle or time out. Args: policy: Exact compiled ``wait_stable`` policy. @@ -231,7 +236,10 @@ def actions( not participate in settling, timeout, or success results. Yields: - Fresh full-qpos hold commands consumed by ordinary ``env.step()``. + Fresh full target-qpos hold commands consumed by ordinary + ``env.step()``. Reading drive targets instead of measured joint + positions preserves contact preload in position-controlled tools. + Rows inactive when the policy starts use fresh measured qpos holds. Timeout is a normal row-local result boundary. Timed-out rows are exposed through :meth:`post_policy_result` and @@ -293,7 +301,7 @@ def actions( return if bool(state.timeout_mask.any().item()): return - yield self._read_robot_qpos(self._robot) + yield self._hold_robot_qpos(active_mask) elapsed_steps += 1 def post_policy_result( @@ -413,12 +421,16 @@ def validator_metadata( return deepcopy(metadata) @staticmethod - def _read_robot_qpos(robot: Robot) -> torch.Tensor: - """Capture one finite full-robot position batch.""" + def _read_robot_qpos(robot: Robot, *, target: bool) -> torch.Tensor: + """Capture one finite current- or target-qpos full-robot batch.""" + if type(target) is not bool: + raise TypeError("target must be a bool.") + mode = "target" if target else "current" + call = f"robot.get_qpos(target={target})" get_qpos = getattr(robot, "get_qpos", None) if not callable(get_qpos): - raise TypeError("robot must provide get_qpos().") - qpos = get_qpos() + raise TypeError(f"robot must provide {call}.") + qpos = get_qpos(target=target) if ( not isinstance(qpos, torch.Tensor) or not qpos.is_floating_point() @@ -426,11 +438,40 @@ def _read_robot_qpos(robot: Robot) -> torch.Tensor: or qpos.shape[0] == 0 or qpos.shape[1] == 0 ): - raise ValueError("robot.get_qpos() must return floating shape (B, J).") + raise ValueError( + f"{call} must return {mode} floating full-qpos shape (B, J)." + ) if not bool(torch.isfinite(qpos).all().item()): - raise ValueError("robot.get_qpos() must contain finite values.") + raise ValueError(f"{call} must return finite {mode} qpos values.") return qpos.clone() + def _hold_robot_qpos(self, active_mask: torch.Tensor) -> torch.Tensor: + """Keep initial active rows on targets and inactive rows on current qpos.""" + target_qpos = self._read_robot_qpos(self._robot, target=True) + if ( + target_qpos.shape != self._robot_qpos_shape + or target_qpos.device != self._robot_qpos_device + ): + raise ValueError( + "robot.get_qpos(target=True) target full qpos must match the " + "construction-time current full qpos shape and device." + ) + if bool(active_mask.all().item()): + return target_qpos + + current_qpos = self._read_robot_qpos(self._robot, target=False) + if ( + current_qpos.shape != self._robot_qpos_shape + or current_qpos.device != self._robot_qpos_device + ): + raise ValueError( + "robot.get_qpos(target=False) current full qpos must match the " + "construction-time current full qpos shape and device." + ) + hold_qpos = current_qpos.clone() + hold_qpos[active_mask] = target_qpos[active_mask] + return hold_qpos + def _validate_active_mask(self, active_mask: torch.Tensor) -> torch.Tensor: """Return one owned row mask aligned with the simulator batch.""" if not isinstance(active_mask, torch.Tensor): diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index 8611766fb..b1ec7d50c 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -59,6 +59,7 @@ unregister_action, ) from .execution import ( + EffectExpectationResult, EffectVerificationRequest, EffectVerificationResult, ExecutionEvent, @@ -67,6 +68,10 @@ ExecutionSession, ExecutionStatus, ExecutionTick, + HeldObjectGuardRequest, + HeldObjectGuardResult, + PhaseEffectGateRequest, + PhaseEffectGateResult, ) from .goals import ( ActionGoal, @@ -75,7 +80,12 @@ SceneArticulationOperationGeometry, SceneEntityPose, ) -from .invocation import ActionInvocation, ActionOptions, ResolvedActionRequest +from .invocation import ( + ActionInvocation, + ActionOptions, + PhaseEffectGateRequirement, + ResolvedActionRequest, +) from .plans import ( ActionPlan, CompiledTrajectory, @@ -190,8 +200,10 @@ ExecutionClock, ExecutionRunner, ExecutionRunnerCfg, + HeldObjectGuardVerifier, MonotonicExecutionClock, ObservationProvider, + PhaseEffectGateVerifier, RunnerStatus, RunnerStep, RunnerStepCallback, @@ -259,6 +271,7 @@ "EndpointCommandRouter", "EndpointCommandTransport", "EntityState", + "EffectExpectationResult", "EffectVerificationRequest", "EffectVerificationRequirement", "EffectVerificationResult", @@ -272,6 +285,13 @@ "ExecutionSession", "ExecutionStatus", "ExecutionTick", + "HeldObjectGuardRequest", + "HeldObjectGuardResult", + "PhaseEffectGateRequest", + "PhaseEffectGateRequirement", + "PhaseEffectGateResult", + "PhaseEffectGateVerifier", + "HeldObjectGuardVerifier", "EndpointTrackingChannelBinding", "EndpointTrackingFeedbackAddress", "FeedbackTerminalAcceptance", 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 4876a0c25..0eb36e6cd 100644 --- a/embodichain/lab/sim/atomic_actions/execution.py +++ b/embodichain/lab/sim/atomic_actions/execution.py @@ -18,7 +18,7 @@ from __future__ import annotations -from dataclasses import dataclass +from dataclasses import dataclass, field from enum import Enum import math from typing import TYPE_CHECKING @@ -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,8 +78,13 @@ 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" + RECOVERY_REQUIRED = "recovery_required" RECOVERY_EXHAUSTED = "recovery_exhausted" ROWS_DEACTIVATED = "rows_deactivated" SESSION_COMPLETED = "session_completed" @@ -227,6 +236,8 @@ class EffectVerificationRequest: only a newly installed plan starts a new attempt deadline. ``attempt_generation`` is session-local and remains stable when partial resolution or row deactivation replaces only the request ID. + ``failure_invalidation`` is a core-owned removal-only delta; verification + results may select failed rows on which to apply it but cannot replace it. """ verification_id: int @@ -241,6 +252,7 @@ class EffectVerificationRequest: env_mask: torch.Tensor expected_effects: StateDelta effect_verification: EffectVerificationRequirement | None = None + failure_invalidation: StateDelta = field(default_factory=StateDelta) def __post_init__(self) -> None: if type(self.verification_id) is not int or self.verification_id < 0: @@ -288,8 +300,32 @@ def __post_init__(self) -> None: "Effect verification requires expected symbolic effects or an " "explicit physical-effect requirement." ) + if not isinstance(self.failure_invalidation, StateDelta): + raise TypeError("failure_invalidation must be a StateDelta.") + if ( + any( + value is not None + for value in self.failure_invalidation.held_object_updates.values() + ) + or any( + value is not None + for value in self.failure_invalidation.coordinated_held_object_updates.values() + ) + or any( + value is not None + for value in self.failure_invalidation.articulation_joint_updates.values() + ) + ): + raise ValueError( + "failure_invalidation may only remove previously verified state." + ) object.__setattr__(self, "env_mask", self.env_mask.clone()) object.__setattr__(self, "expected_effects", self.expected_effects.snapshot()) + object.__setattr__( + self, + "failure_invalidation", + self.failure_invalidation.snapshot(), + ) object.__setattr__( self, "effect_verification", @@ -315,6 +351,74 @@ def snapshot(self) -> EffectVerificationRequest: env_mask=self.env_mask, expected_effects=self.expected_effects, effect_verification=self.effect_verification, + failure_invalidation=self.failure_invalidation, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class EffectExpectationResult: + """Current-observation outcome for one physical state expectation. + + ``inverse_satisfied_mask`` is stronger than contradiction: every clause + must have reached its explicit inverse band for the monitor's complete + hysteresis window. It may therefore be used to retain a pre-existing + relation during failure reconciliation, while a single contradictory + clause may not. + """ + + expectation_id: str + satisfied_mask: torch.Tensor + contradicted_mask: torch.Tensor + inverse_satisfied_mask: torch.Tensor + + def __post_init__(self) -> None: + if ( + type(self.expectation_id) is not str + or not self.expectation_id + or self.expectation_id != self.expectation_id.strip() + ): + raise ValueError( + "expectation_id must be a non-empty string without outer whitespace." + ) + for name in ( + "satisfied_mask", + "contradicted_mask", + "inverse_satisfied_mask", + ): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + masks = ( + self.satisfied_mask, + self.contradicted_mask, + self.inverse_satisfied_mask, + ) + if any(mask.shape != masks[0].shape for mask in masks[1:]): + raise ValueError("Expectation-result masks must have equal shapes.") + if any(mask.device != masks[0].device for mask in masks[1:]): + raise ValueError("Expectation-result masks must use the same device.") + if (self.satisfied_mask & self.contradicted_mask).any(): + raise ValueError("satisfied_mask and contradicted_mask must not overlap.") + if (self.inverse_satisfied_mask & ~self.contradicted_mask).any(): + raise ValueError( + "inverse_satisfied_mask must be a subset of contradicted_mask." + ) + for name in ( + "satisfied_mask", + "contradicted_mask", + "inverse_satisfied_mask", + ): + object.__setattr__(self, name, getattr(self, name).clone()) + + def snapshot(self) -> EffectExpectationResult: + """Return an independently owned expectation outcome.""" + return EffectExpectationResult( + expectation_id=self.expectation_id, + satisfied_mask=self.satisfied_mask, + contradicted_mask=self.contradicted_mask, + inverse_satisfied_mask=self.inverse_satisfied_mask, ) @@ -322,32 +426,442 @@ def snapshot(self) -> EffectVerificationRequest: class EffectVerificationResult: """Correlated per-environment update for one effect boundary. - Rows absent from both masks remain unresolved. This lets one shared batch - barrier commit verified rows while other rows continue observing the same - physical effect. + Rows absent from both ``success_mask`` and ``failure_mask`` remain + unresolved. ``invalidation_mask`` and ``retry_mask`` classify only failed + rows: the former selects the request's core-owned removal delta, while the + latter authorizes replay of the same invocation. Failed rows outside the + retry mask require external recovery. This lets one shared batch barrier + commit verified rows while other rows continue observing the same physical + effect. """ verification_id: int success_mask: torch.Tensor failure_mask: torch.Tensor + invalidation_mask: torch.Tensor + retry_mask: torch.Tensor + expectation_results: tuple[EffectExpectationResult, ...] = () def __post_init__(self) -> None: if type(self.verification_id) is not int or self.verification_id < 0: raise ValueError("verification_id must be a non-negative integer.") - for name in ("success_mask", "failure_mask"): + for name in ( + "success_mask", + "failure_mask", + "invalidation_mask", + "retry_mask", + ): value = getattr(self, name) if not isinstance(value, torch.Tensor): raise TypeError(f"{name} must be a torch.Tensor.") if value.dtype != torch.bool or value.dim() != 1: raise ValueError(f"{name} must be a one-dimensional bool tensor.") - if self.success_mask.shape != self.failure_mask.shape: - raise ValueError("success_mask and failure_mask must have equal shapes.") - if self.success_mask.device != self.failure_mask.device: - raise ValueError("success_mask and failure_mask must use the same device.") + masks = ( + self.success_mask, + self.failure_mask, + self.invalidation_mask, + self.retry_mask, + ) + if any(mask.shape != masks[0].shape for mask in masks[1:]): + raise ValueError("Effect-result masks must have equal shapes.") + if any(mask.device != masks[0].device for mask in masks[1:]): + raise ValueError("Effect-result masks must use the same device.") if (self.success_mask & self.failure_mask).any(): raise ValueError("success_mask and failure_mask must not overlap.") - object.__setattr__(self, "success_mask", self.success_mask.clone()) + if (self.invalidation_mask & ~self.failure_mask).any(): + raise ValueError("invalidation_mask must be a subset of failure_mask.") + if (self.retry_mask & ~self.failure_mask).any(): + raise ValueError("retry_mask must be a subset of failure_mask.") + expectation_results = tuple(self.expectation_results) + if not all( + type(value) is EffectExpectationResult for value in expectation_results + ): + raise TypeError( + "expectation_results must contain exact EffectExpectationResult values." + ) + expectation_ids = [value.expectation_id for value in expectation_results] + if len(set(expectation_ids)) != len(expectation_ids): + raise ValueError("Effect expectation-result IDs must be unique.") + if expectation_results: + expected_success = torch.ones_like(self.success_mask) + expected_failure = torch.zeros_like(self.failure_mask) + for value in expectation_results: + if value.satisfied_mask.shape != self.success_mask.shape: + raise ValueError( + "Expectation and aggregate result masks must have equal shapes." + ) + if value.satisfied_mask.device != self.success_mask.device: + raise ValueError( + "Expectation and aggregate result masks must use the same device." + ) + expected_success &= value.satisfied_mask + expected_failure |= value.contradicted_mask + if not torch.equal(self.success_mask, expected_success): + raise ValueError( + "success_mask must equal the conjunction of expectation results." + ) + if not torch.equal(self.failure_mask, expected_failure): + raise ValueError( + "failure_mask must equal the union of expectation results." + ) + for name in ( + "success_mask", + "failure_mask", + "invalidation_mask", + "retry_mask", + ): + object.__setattr__(self, name, getattr(self, name).clone()) + object.__setattr__( + self, + "expectation_results", + tuple(value.snapshot() for value in expectation_results), + ) + + +@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. + + A request is correlated to one installed action-plan attempt and one next + waypoint. The named segment lets an external verifier select phase-aware + physical evidence without teaching the execution core skill-specific + phases. ``deadline`` uses the observation timestamp domain. + """ + + verification_id: int + skill_id: str + invocation_id: str | None + invocation_revision: int + invocation_index: int + attempt_generation: int + next_waypoint_index: int + segment_name: str + env_mask: torch.Tensor + allowed_held_object_relations: tuple[tuple[str, str], ...] + allowed_coordinated_held_object_relations: tuple[tuple[str, str, str], ...] + deadline: float + + def __post_init__(self) -> None: + if type(self.verification_id) is not int or self.verification_id < 0: + raise ValueError("verification_id must be a non-negative integer.") + if type(self.skill_id) is not str or not self.skill_id: + raise ValueError("skill_id must be a non-empty string.") + 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.") + for name in ( + "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.") + if type(self.segment_name) is not str or not self.segment_name: + raise ValueError("segment_name must be a non-empty string.") + 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 guarded row.") + held_relations = tuple(self.allowed_held_object_relations) + if len(set(held_relations)) != len(held_relations) or not all( + type(value) is tuple + and len(value) == 2 + and all(type(item) is str and item for item in value) + for value in held_relations + ): + raise ValueError( + "allowed_held_object_relations must contain unique " + "(task_state_key, object_id) pairs." + ) + coordinated_relations = tuple(self.allowed_coordinated_held_object_relations) + if len(set(coordinated_relations)) != len(coordinated_relations) or not all( + type(value) is tuple + and len(value) == 3 + and all(type(item) is str and item for item in value) + for value in coordinated_relations + ): + raise ValueError( + "allowed_coordinated_held_object_relations must contain unique " + "(first_key, second_key, object_id) triples." + ) + if not math.isfinite(self.deadline) or self.deadline < 0.0: + raise ValueError("deadline must be finite and non-negative.") + object.__setattr__(self, "env_mask", self.env_mask.clone()) + object.__setattr__(self, "allowed_held_object_relations", held_relations) + object.__setattr__( + self, + "allowed_coordinated_held_object_relations", + coordinated_relations, + ) + + def snapshot(self) -> HeldObjectGuardRequest: + """Return an independently owned guard request. + + Returns: + Request with an independently owned environment mask. + """ + return HeldObjectGuardRequest( + verification_id=self.verification_id, + skill_id=self.skill_id, + invocation_id=self.invocation_id, + invocation_revision=self.invocation_revision, + invocation_index=self.invocation_index, + attempt_generation=self.attempt_generation, + next_waypoint_index=self.next_waypoint_index, + segment_name=self.segment_name, + env_mask=self.env_mask, + allowed_held_object_relations=self.allowed_held_object_relations, + allowed_coordinated_held_object_relations=( + self.allowed_coordinated_held_object_relations + ), + deadline=self.deadline, + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class HeldObjectGuardResult: + """Correlated in-flight held-object loss and recovery decision. + + ``state_invalidation`` may only remove single-resource or coordinated + held-object relations. It is applied to ``failure_mask`` before recovery + planning, so a retry always observes reconciled symbolic state. + """ + + verification_id: int + object_id: str + attempt_generation: int + invocation_index: int + next_waypoint_index: int + failure_mask: torch.Tensor + state_invalidation: StateDelta + retry_mask: torch.Tensor + message: str = "" + + def __post_init__(self) -> None: + if type(self.verification_id) is not int or self.verification_id < 0: + raise ValueError("verification_id must be a non-negative integer.") + if type(self.object_id) is not str or not self.object_id: + raise ValueError("object_id must be a non-empty string.") + for name in ( + "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.") + for name in ("failure_mask", "retry_mask"): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + if self.failure_mask.shape != self.retry_mask.shape: + raise ValueError("failure_mask and retry_mask must have equal shapes.") + if self.failure_mask.device != self.retry_mask.device: + raise ValueError("failure_mask and retry_mask must use the same device.") + if (self.retry_mask & ~self.failure_mask).any(): + raise ValueError("retry_mask must be a subset of failure_mask.") + if not isinstance(self.state_invalidation, StateDelta): + raise TypeError("state_invalidation must be a StateDelta.") + if any( + value is not None + for value in self.state_invalidation.held_object_updates.values() + ) or any( + value is not None + for value in self.state_invalidation.coordinated_held_object_updates.values() + ): + raise ValueError( + "state_invalidation may only remove held-object relations." + ) + if self.state_invalidation.articulation_joint_updates: + raise ValueError( + "state_invalidation cannot update articulation-joint state." + ) + has_invalidation = bool( + self.state_invalidation.held_object_updates + or self.state_invalidation.coordinated_held_object_updates + ) + if bool(self.failure_mask.any().item()) != has_invalidation: + raise ValueError( + "state_invalidation must contain relation removals exactly when " + "failure_mask contains failed rows." + ) + if type(self.message) is not str: + raise TypeError("message must be a string.") object.__setattr__(self, "failure_mask", self.failure_mask.clone()) + object.__setattr__(self, "retry_mask", self.retry_mask.clone()) + object.__setattr__( + self, + "state_invalidation", + self.state_invalidation.snapshot(), + ) @dataclass(frozen=True, slots=True, eq=False) @@ -361,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: @@ -371,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, @@ -392,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() @@ -481,6 +1017,11 @@ def __init__( self._effect_failures = torch.zeros_like(self._eligible) 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 @@ -527,6 +1068,33 @@ 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. + + The request remains available while terminal acceptance is settling, + using the final waypoint and segment identity. Once terminal physical + effect verification begins, that verifier owns the boundary and this + property returns ``None``. + + Returns: + Owned phase-aware guard request, or ``None`` when no command-phase + guard is active. + """ + request = self._held_object_guard_request() + return None if request is None else request.snapshot() + def deactivate_rows( self, env_mask: torch.Tensor, @@ -579,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) @@ -632,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." @@ -654,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." @@ -764,6 +1343,8 @@ 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. @@ -772,6 +1353,12 @@ 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 + was supplied. Returns: Status, optional command, events, and current verified task state. @@ -790,10 +1377,95 @@ 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: + raise TypeError( + "held_object_guard_result must be exactly " + "HeldObjectGuardResult or None." + ) + if guard_request is None: + raise ValueError("No held-object guard is active for this phase.") + if held_object_guard_result.verification_id != ( + guard_request.verification_id + ): + raise ValueError( + "held_object_guard_result verification_id does not match the " + "active guard request." + ) + for name in ( + "attempt_generation", + "invocation_index", + "next_waypoint_index", + ): + if getattr(held_object_guard_result, name) != getattr( + guard_request, + name, + ): + raise ValueError( + f"held_object_guard_result {name} does not match the " + "active guard request." + ) + if guard_request is not None: + self._next_held_object_guard_verification_id += 1 if self._status is not ExecutionStatus.RUNNING: 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( + self._apply_held_object_guard_result( + held_object_guard_result, + guard_request, + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + assert self._plan is not None if not self._pending.any(): command, hold_targets, completion_events = self._finish_action( self._pending, @@ -811,12 +1483,36 @@ def tick( self._pending_effect.env_mask & self._pending & self._plan.plan_success ) if self._action_timed_out(self._plan, execution_mask): + pending_request = self._pending_effect timed_out = execution_mask.clone() known_failures = self._effect_failures.clone() planning_failed = self._pending & ~self._plan.plan_success - retry_mask = timed_out | known_failures | planning_failed + invalidation_presence = self._failure_invalidation_presence_mask( + pending_request.failure_invalidation + ) + external_recovery = timed_out & invalidation_presence + retry_mask = ( + (timed_out & ~external_recovery) | known_failures | planning_failed + ) + self._apply_effect_failure_invalidation( + pending_request.failure_invalidation, + timed_out, + ) self._pending_effect = None self._effect_failures.zero_() + if external_recovery.any(): + self._eligible &= ~external_recovery + self._pending &= ~external_recovery + self._last_command_mask &= ~external_recovery + events.append( + self._event( + ExecutionEventKind.RECOVERY_REQUIRED, + external_recovery, + "Effect evidence remained unresolved at the action " + "deadline, so previously verified state was " + "invalidated before external recovery.", + ) + ) if known_failures.any(): events.append( self._event( @@ -938,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) @@ -1110,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 ( @@ -1142,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( @@ -1164,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, @@ -1507,6 +2237,8 @@ def _finish_action( ) return None, active_targets, events else: + assert self._pending_effect is not None + pending_request = self._pending_effect success_input = self._normalize_mask( effect_result.success_mask, "effect_result.success_mask", @@ -1515,17 +2247,67 @@ def _finish_action( effect_result.failure_mask, "effect_result.failure_mask", ) + invalidation_input = self._normalize_mask( + effect_result.invalidation_mask, + "effect_result.invalidation_mask", + ) + retry_input = self._normalize_mask( + effect_result.retry_mask, + "effect_result.retry_mask", + ) reported = success_input | failure_input if (reported & ~execution_mask).any(): raise ValueError( "Effect verification masks must be subsets of the pending " "effect request env_mask." ) + for outcome in effect_result.expectation_results: + for name in ( + "satisfied_mask", + "contradicted_mask", + "inverse_satisfied_mask", + ): + outcome_mask = self._normalize_mask( + getattr(outcome, name), + f"effect_result.expectation_results.{name}", + ) + if (outcome_mask & ~execution_mask).any(): + raise ValueError( + "Effect expectation-result masks must be subsets " + "of the pending effect request env_mask." + ) verified = execution_mask & success_input failed_effect = execution_mask & failure_input unresolved = execution_mask & ~reported made_progress = bool(reported.any().item()) - self._effect_failures |= failed_effect + invalidated = failed_effect & invalidation_input + retryable_failure = failed_effect & retry_input + external_recovery = failed_effect & ~retry_input + self._apply_effect_failure_invalidation( + pending_request.failure_invalidation, + invalidated, + ) + self._effect_failures |= retryable_failure + if external_recovery.any(): + self._eligible &= ~external_recovery + self._pending &= ~external_recovery + self._effect_failures &= ~external_recovery + self._last_command_mask &= ~external_recovery + events.extend( + ( + self._event( + ExecutionEventKind.EFFECT_VERIFICATION_FAILED, + external_recovery, + "Required physical effects were contradicted.", + ), + self._event( + ExecutionEventKind.RECOVERY_REQUIRED, + external_recovery, + "The reconciled effect failure cannot safely replay " + "the current invocation.", + ), + ) + ) if not unresolved.any(): self._pending_effect = None @@ -1545,8 +2327,12 @@ def _finish_action( if made_progress: self._pending_effect = self._effect_verification_request(unresolved) return None, active_targets, events - retry_mask = self._effect_failures | planning_failed - if retry_mask.any(): + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) + return None, active_targets, events + retry_candidates = self._effect_failures | planning_failed + if retry_candidates.any(): effect_failure_mask = self._effect_failures.clone() self._effect_failures.zero_() reason = ( @@ -1555,7 +2341,7 @@ def _finish_action( else ExecutionEventKind.ACTION_PLANNING_FAILED ) reason_mask = ( - effect_failure_mask if effect_failure_mask.any() else retry_mask + effect_failure_mask if effect_failure_mask.any() else retry_candidates ) if effect_failure_mask.any() and planning_failed.any(): events.append( @@ -1567,7 +2353,7 @@ def _finish_action( ) events.extend( self._attempt_action_retry( - retry_mask, + retry_candidates, reason, "Planning or expected-effect verification failed.", reason_mask=reason_mask, @@ -1845,6 +2631,365 @@ 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 + ): + return None + env_mask = self._pending & self._plan.plan_success + if not env_mask.any(): + return None + next_waypoint_index = min( + self._waypoint_index, + self._plan.commands.frame_count - 1, + ) + segment = self._plan.segment_at(next_waypoint_index) + invocation = self._requests[self._invocation_index] + ( + allowed_held_object_relations, + allowed_coordinated_held_object_relations, + ) = self._authorized_held_object_invalidation_relations( + invocation=invocation, + ) + return HeldObjectGuardRequest( + verification_id=self._next_held_object_guard_verification_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=next_waypoint_index, + segment_name=segment.name, + env_mask=env_mask, + allowed_held_object_relations=allowed_held_object_relations, + allowed_coordinated_held_object_relations=( + allowed_coordinated_held_object_relations + ), + deadline=( + self._action_started_at + self._plan.recovery_policy.action_timeout + ), + ) + + def _apply_held_object_guard_result( + self, + result: HeldObjectGuardResult, + request: HeldObjectGuardRequest, + ) -> list[ExecutionEvent]: + """Reconcile lost relations and enter row-local bounded recovery.""" + failure_mask = self._normalize_mask( + result.failure_mask, + "held_object_guard_result.failure_mask", + ) + retry_mask = self._normalize_mask( + result.retry_mask, + "held_object_guard_result.retry_mask", + ) + request_mask = request.env_mask.to(self._eligible.device) + if (failure_mask & ~request_mask).any(): + raise ValueError( + "Held-object guard failure_mask must be a subset of the active " + "request env_mask." + ) + if (retry_mask & ~request_mask).any(): + raise ValueError( + "Held-object guard retry_mask must be a subset of the active " + "request env_mask." + ) + self._validate_held_object_invalidation_authorization( + result.state_invalidation, + object_id=result.object_id, + allowed_held_object_relations=request.allowed_held_object_relations, + allowed_coordinated_held_object_relations=( + request.allowed_coordinated_held_object_relations + ), + ) + if not failure_mask.any(): + return [] + + self._task_state = result.state_invalidation.apply( + self._task_state, + failure_mask, + ) + self._context = PlanningContext( + robot=self._context.robot, + task=self._task_state, + scene=self._context.scene, + env_ids=self._context.env_ids, + ) + message = result.message or ( + "Physical evidence contradicted the verified held-object relation." + ) + non_retry_mask = failure_mask & ~retry_mask + events: list[ExecutionEvent] = [] + if non_retry_mask.any(): + self._eligible &= ~non_retry_mask + self._pending &= ~non_retry_mask + self._effect_failures &= ~non_retry_mask + self._last_command_mask &= ~non_retry_mask + events.extend( + ( + self._event( + ExecutionEventKind.HELD_OBJECT_LOST, + non_retry_mask, + message, + ), + self._event( + ExecutionEventKind.RECOVERY_REQUIRED, + non_retry_mask, + "Held-object loss requires recovery outside the current " + "action retry policy.", + ), + ) + ) + if retry_mask.any(): + events.extend( + self._attempt_action_retry( + retry_mask, + ExecutionEventKind.HELD_OBJECT_LOST, + message, + ) + ) + else: + terminal_event = self._update_terminal_status() + if terminal_event is not None: + events.append(terminal_event) + return events + + def _authorized_held_object_invalidation_relations( + self, + *, + invocation: ResolvedActionRequest | None = None, + ) -> tuple[tuple[tuple[str, str], ...], tuple[tuple[str, str, str], ...]]: + """Return action-owned key/object identities eligible for removal.""" + assert self._plan is not None + active_invocation = ( + self._requests[self._invocation_index] if invocation is None else invocation + ) + binding_task_state_keys = { + endpoint.task_state_key for endpoint in active_invocation.binding.endpoints + } + held_relations: set[tuple[str, str]] = set() + for key, candidate in self._task_state.held_objects.items(): + object_id = candidate.semantics.entity_id + if key in binding_task_state_keys and object_id is not None: + held_relations.add((key, object_id)) + for key, candidate in self._plan.expected_effects.held_object_updates.items(): + if candidate is not None and candidate.semantics.entity_id is not None: + held_relations.add((key, candidate.semantics.entity_id)) + + related_keys = {key for key, _ in held_relations} + coordinated_relations: set[tuple[str, str, str]] = set() + for resources, candidate in self._task_state.coordinated_held_objects.items(): + object_id = candidate.semantics.entity_id + if not set(resources).isdisjoint(related_keys) and object_id is not None: + coordinated_relations.add((*resources, object_id)) + for ( + resources, + candidate, + ) in self._plan.expected_effects.coordinated_held_object_updates.items(): + if candidate is not None and candidate.semantics.entity_id is not None: + coordinated_relations.add((*resources, candidate.semantics.entity_id)) + return tuple(sorted(held_relations)), tuple(sorted(coordinated_relations)) + + def _validate_held_object_invalidation_authorization( + self, + state_invalidation: StateDelta, + *, + object_id: str, + allowed_held_object_relations: tuple[tuple[str, str], ...], + allowed_coordinated_held_object_relations: tuple[tuple[str, str, str], ...], + ) -> None: + """Reject removals outside the action-owned key/object identity set.""" + invalidated_held_relations = { + (key, object_id) for key in state_invalidation.held_object_updates + } + if not invalidated_held_relations.issubset(allowed_held_object_relations): + raise ValueError( + "Held-object state invalidation contains a key/object identity " + "outside the active action's authorized relation set." + ) + invalidated_coordinated_relations = { + (*resources, object_id) + for resources in state_invalidation.coordinated_held_object_updates + } + if not invalidated_coordinated_relations.issubset( + allowed_coordinated_held_object_relations + ): + raise ValueError( + "Held-object state invalidation contains a coordinated key/object " + "identity outside the active action's authorized relation set." + ) + def _normalize_mask(self, value: torch.Tensor, name: str) -> torch.Tensor: """Validate and copy a per-environment boolean mask.""" if not isinstance(value, torch.Tensor): @@ -1883,7 +3028,68 @@ def _effect_verification_request( env_mask=env_mask, expected_effects=self._plan.expected_effects, effect_verification=self._plan.effect_verification, + failure_invalidation=self._effect_failure_invalidation(), + ) + + def _effect_failure_invalidation(self) -> StateDelta: + """Build the core-owned fail-closed state removal for this effect.""" + assert self._plan is not None + expected = self._plan.expected_effects + held_keys = set(expected.held_object_updates) + coordinated_keys = set(expected.coordinated_held_object_updates) + coordinated_keys.update( + resources + for resources in self._task_state.coordinated_held_objects + if not set(resources).isdisjoint(held_keys) ) + return StateDelta( + held_object_updates={key: None for key in held_keys}, + coordinated_held_object_updates={ + resources: None for resources in coordinated_keys + }, + articulation_joint_updates={ + key: None for key in expected.articulation_joint_updates + }, + ) + + def _apply_effect_failure_invalidation( + self, + state_invalidation: StateDelta, + env_mask: torch.Tensor, + ) -> None: + """Apply a request-owned failure delta and refresh planning context.""" + if not env_mask.any() or state_invalidation.is_empty: + return + self._task_state = state_invalidation.apply(self._task_state, env_mask) + self._context = PlanningContext( + robot=self._context.robot, + task=self._task_state, + scene=self._context.scene, + env_ids=self._context.env_ids, + ) + + def _failure_invalidation_presence_mask( + self, + state_invalidation: StateDelta, + ) -> torch.Tensor: + """Return rows whose verified state would actually be removed.""" + present = torch.zeros_like(self._eligible) + for key in state_invalidation.held_object_updates: + value = self._task_state.held_objects.get(key) + if value is not None: + assert value.env_mask is not None + present |= value.env_mask.to(present.device) + for key in state_invalidation.coordinated_held_object_updates: + value = self._task_state.coordinated_held_objects.get(key) + if value is not None: + assert value.env_mask is not None + present |= value.env_mask.to(present.device) + for key in state_invalidation.articulation_joint_updates: + value = self._task_state.articulation_joints.get(key) + if value is not None: + assert value.env_mask is not None + present |= value.env_mask.to(present.device) + return present def _event( self, @@ -1929,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( @@ -1946,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, @@ -1954,10 +3164,12 @@ def _tick_result( events=tuple(events), task_state=self._task_state, pending_effect=self._pending_effect, + pending_phase_effect_gate=phase_gate, ) __all__ = [ + "EffectExpectationResult", "EffectVerificationRequest", "EffectVerificationResult", "ExecutionEvent", @@ -1966,4 +3178,8 @@ def _tick_result( "ExecutionSession", "ExecutionStatus", "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 8dac661a6..bd231598b 100644 --- a/embodichain/lab/sim/atomic_actions/runner.py +++ b/embodichain/lab/sim/atomic_actions/runner.py @@ -36,6 +36,10 @@ ExecutionSession, ExecutionStatus, ExecutionTick, + HeldObjectGuardRequest, + HeldObjectGuardResult, + PhaseEffectGateRequest, + PhaseEffectGateResult, ) from .invocation import ActionInvocation, ResolvedActionRequest from .runtime_commands import RuntimeCommandFrame @@ -236,6 +240,15 @@ class ExecutionRunnerCfg: hold_on_completion: bool = True """Whether to issue a final hold after the session completes.""" + hold_during_effect_verification: bool = True + """Whether to hold observed state while terminal effects are pending. + + Disable this only for persistent transports whose last accepted command + remains active without refresh, such as a position-controlled gripper that + must retain contact preload. Failure and cancellation still perform the + normal cancel-then-observed-hold safe stop. + """ + def __post_init__(self) -> None: for name in ("command_timeout", "safe_stop_timeout"): value = getattr(self, name) @@ -245,6 +258,8 @@ def __post_init__(self) -> None: raise ValueError("minimum_cycle_time must be finite and non-negative.") if not isinstance(self.hold_on_completion, bool): raise TypeError("hold_on_completion must be a bool.") + if not isinstance(self.hold_during_effect_verification, bool): + raise TypeError("hold_during_effect_verification must be a bool.") class RunnerStatus(str, Enum): @@ -299,6 +314,18 @@ def is_waiting(self) -> bool: ] """Synchronous verifier called on a fresh due-cycle observation.""" +HeldObjectGuardVerifier = Callable[ + [PlanningContext, HeldObjectGuardRequest], + HeldObjectGuardResult | None, +] +"""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.""" @@ -466,6 +493,9 @@ 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. @@ -478,6 +508,16 @@ 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 + command dispatch. Returning ``None`` means the current phase + has no applicable held-object guard. Returns: Runner status, optional session tick, controller acknowledgements, @@ -487,8 +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) @@ -512,6 +568,19 @@ def step( ) self._last_context = context + try: + if self._pending_revision is not None: + self._session._install_prepared_revision( + self._pending_revision, + context, + ) + self._pending_revision = None + except Exception as exc: + return self._fail( + f"Execution session failed: {type(exc).__name__}: {exc}", + context=context, + ) + pending_effect = self._session.pending_effect if ( effect_verifier is not None @@ -531,14 +600,63 @@ def step( context=context, ) - try: - if self._pending_revision is not None: - self._session._install_prepared_revision( - self._pending_revision, + 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, ) - self._pending_revision = None - tick = self._session.tick(context, effect_result=effect_result) + 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 ( + held_object_guard_verifier is not None + and held_object_guard_request is not None + and context.robot.timestamp <= held_object_guard_request.deadline + ): + try: + held_object_guard_result = held_object_guard_verifier( + context, + held_object_guard_request, + ) + if ( + held_object_guard_result is not None + and type(held_object_guard_result) is not HeldObjectGuardResult + ): + raise TypeError( + "HeldObjectGuardVerifier must return exactly " + "HeldObjectGuardResult or None." + ) + except Exception as exc: + return self._fail( + "Held-object guard verifier failed: " + f"{type(exc).__name__}: {exc}", + context=context, + ) + + try: + 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 self._last_context = context except Exception as exc: @@ -581,7 +699,9 @@ def step( self._command_count += 1 interval = self._command_interval(tick.command) self._next_step_at = self._clock_now() + interval - elif tick.hold_targets: + elif tick.hold_targets and ( + tick.pending_effect is None or self.cfg.hold_during_effect_verification + ): self._remember_targets(tick.hold_targets) hold_dispatch = self._dispatch( CommandOperation.HOLD, @@ -605,6 +725,7 @@ def step( ) self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time elif tick.pending_effect is not None: + self._remember_targets(tick.hold_targets) self._next_step_at = self._clock_now() + self.cfg.minimum_cycle_time else: self._next_step_at = self._clock_now() @@ -685,6 +806,8 @@ 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, ) -> RunnerStep: @@ -695,6 +818,10 @@ 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. max_steps: Hard bound on loop iterations. @@ -713,7 +840,11 @@ def run_until_blocked( if self.effect_verification_pending and effect_verifier is None: return last_result for _ in range(max_steps): - result = self.step(effect_verifier=effect_verifier) + 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: try: on_step(result) @@ -732,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) @@ -925,8 +1062,10 @@ def _result( "ExecutionClock", "ExecutionRunner", "ExecutionRunnerCfg", + "HeldObjectGuardVerifier", "MonotonicExecutionClock", "ObservationProvider", + "PhaseEffectGateVerifier", "RunnerStatus", "RunnerStep", "RunnerStepCallback", diff --git a/embodichain/lab/sim/skills/__init__.py b/embodichain/lab/sim/skills/__init__.py index 9bd87c54a..94a315f19 100644 --- a/embodichain/lab/sim/skills/__init__.py +++ b/embodichain/lab/sim/skills/__init__.py @@ -34,9 +34,12 @@ ) from .compiler import ( AnalyzedSemanticCall, + GroundedHeldObjectGuard, + GroundedPhaseEffectGate, GroundedSemanticCall, HandOverPoseProvider, HandOverPoseTargets, + HeldObjectGuardBaseline, RegisteredSemanticLowerer, RelationTargetGrounder, SemanticEffectDependency, @@ -67,6 +70,7 @@ EffectEvidenceAddress, EffectEvidenceBatch, EffectEvidenceSourceRef, + EffectExpectationDecision, EffectMonitor, EffectMonitorDecision, EffectMonitorFactory, @@ -167,6 +171,7 @@ RobotSkillProfile, SkillPolicyPreset, UnsupportedSkillError, + WorkflowRecoveryPolicy, ) from .scene import ( ARTICULATION_OPERATION_AFFORDANCE_CAPABILITY, @@ -209,6 +214,8 @@ SkillRuntimeProvider, SkillScene, SkillStatus, + SkillWorkflowRecoveryRole, + SkillWorkflowRecoveryTrace, task_state_to_metadata, ) @@ -258,6 +265,7 @@ "EffectEvidenceQuery", "EffectEvidenceQueryValue", "EffectEvidenceSourceRef", + "EffectExpectationDecision", "EffectMonitor", "EffectMonitorDecision", "EffectMonitorFactory", @@ -267,12 +275,15 @@ "EffectStateExpectation", "FORCE_EFFECT_CHANNEL", "GRASP_AFFORDANCE_CAPABILITY", + "GroundedHeldObjectGuard", + "GroundedPhaseEffectGate", "GroundedSemanticCall", "HeldObjectRelation", "HeldObjectStateExpectation", "HandOver", "HandOverPoseProvider", "HandOverPoseTargets", + "HeldObjectGuardBaseline", "LinkedSemanticCall", "JOINT_STATE_EFFECT_CHANNEL", "JointStateEffectClause", @@ -377,8 +388,11 @@ "SkillRuntimeProvider", "SkillScene", "SkillStatus", + "SkillWorkflowRecoveryRole", + "SkillWorkflowRecoveryTrace", "task_state_to_metadata", "UnsupportedSkillError", + "WorkflowRecoveryPolicy", "UnsupportedSceneAffordanceError", "build_effect_evidence_queries", "align_parallel_commands", diff --git a/embodichain/lab/sim/skills/compiler.py b/embodichain/lab/sim/skills/compiler.py index 98040c412..904de30b5 100644 --- a/embodichain/lab/sim/skills/compiler.py +++ b/embodichain/lab/sim/skills/compiler.py @@ -22,6 +22,7 @@ from collections.abc import Iterable, Mapping from copy import deepcopy from dataclasses import dataclass, field, replace +from enum import Enum from types import MappingProxyType from typing import ClassVar, TypeVar from uuid import uuid4 @@ -42,6 +43,7 @@ PlaceOptions, OperateArticulationGoal, OperateArticulationOptions, + PhaseEffectGateRequirement, PlanningContext, PoseGoalValue, SceneArticulationOperationGeometry, @@ -77,6 +79,8 @@ JointStateEffectClause, PoseRelationClause, PoseRelationExpectation, + ScalarEffectClause, + ScalarExpectation, SemanticEffectKind, SemanticEffectSpec, SymbolicStateKey, @@ -475,6 +479,134 @@ def resolve( """ +class HeldObjectGuardBaseline(str, Enum): + """Source of the verified pose baseline used by an in-flight guard.""" + + VERIFIED_TASK_STATE = "verified_task_state" + PLANNED_EFFECT = "planned_effect" + + +@dataclass(frozen=True, slots=True) +class GroundedHeldObjectGuard: + """One grounded, phase-scoped physical invariant for a held object. + + Named trajectory segments only activate observation of the invariant; they + do not create an independent planning, timeout, or recovery boundary. The + enclosing atomic action continues to own the recovery budget. + """ + + guard_id: str + active_segments: tuple[str, ...] + baseline: HeldObjectGuardBaseline + effect_spec: SemanticEffectSpec + effect_monitor: EffectMonitor = field(repr=False, compare=False) + invalidation_task_state_keys: tuple[str, ...] + retry_action: bool + + def __post_init__(self) -> None: + _validate_identifier(self.guard_id, field_name="guard_id") + segments = tuple(self.active_segments) + if not segments or len(set(segments)) != len(segments): + raise ValueError( + "active_segments must contain unique non-empty segment names." + ) + for segment in segments: + _validate_identifier(segment, field_name="active segment") + if not isinstance(self.baseline, HeldObjectGuardBaseline): + raise TypeError("baseline must be a HeldObjectGuardBaseline.") + if not isinstance(self.effect_spec, SemanticEffectSpec): + raise TypeError("effect_spec must be a SemanticEffectSpec.") + if self.effect_spec.effect_kind is not SemanticEffectKind.ATTACH: + raise ValueError("A held-object guard must observe an attach effect.") + expectations = tuple( + value + for value in self.effect_spec.state_expectations + if type(value) is HeldObjectStateExpectation + ) + if len(expectations) != 1 or expectations[0].relation is not ( + HeldObjectRelation.ATTACHED + ): + raise ValueError( + "A held-object guard must contain one attached expectation." + ) + if not isinstance(self.effect_monitor, EffectMonitor): + raise TypeError("effect_monitor must be an EffectMonitor.") + invalidation_keys = tuple(self.invalidation_task_state_keys) + if not invalidation_keys or len(set(invalidation_keys)) != len( + invalidation_keys + ): + raise ValueError( + "invalidation_task_state_keys must contain unique non-empty keys." + ) + for key in invalidation_keys: + _validate_identifier(key, field_name="invalidation task-state key") + if type(self.retry_action) is not bool: + raise TypeError("retry_action must be a bool.") + object.__setattr__(self, "active_segments", segments) + object.__setattr__(self, "effect_spec", self.effect_spec.snapshot()) + object.__setattr__( + self, + "invalidation_task_state_keys", + invalidation_keys, + ) + + @property + def task_state_key(self) -> str: + """Return the single held-object relation observed by this guard.""" + expectation = self.effect_spec.state_expectations[0] + assert type(expectation) is HeldObjectStateExpectation + 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.""" @@ -483,6 +615,8 @@ class GroundedSemanticCall: invocation: ActionInvocation 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: @@ -501,6 +635,8 @@ def _create( invocation: ActionInvocation, 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.""" @@ -509,6 +645,8 @@ def _create( object.__setattr__(instance, "invocation", invocation) 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 @@ -534,6 +672,39 @@ def __post_init__(self) -> None: "effect_spec semantic_id must match the analyzed call." ) object.__setattr__(self, "effect_spec", self.effect_spec.snapshot()) + guards = tuple(self.effect_guards) + if not all(type(value) is GroundedHeldObjectGuard for value in guards): + raise TypeError( + "effect_guards must contain exact GroundedHeldObjectGuard values." + ) + guard_ids = [value.guard_id for value in guards] + if len(set(guard_ids)) != len(guard_ids): + raise ValueError("Grounded held-object guard IDs must be unique.") + 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: @@ -1124,11 +1295,29 @@ def ground( (*path, call_index, "effect_monitor"), f"Could not create the grounded effect monitor: {exc}", ) from exc + effect_guards = self._ground_held_object_guards( + analyzed, + effect_spec, + 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, ) @@ -1761,6 +1950,293 @@ 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, + effect_spec: SemanticEffectSpec | None, + context: PlanningContext, + *, + path: tuple[PathPart, ...], + ) -> tuple[GroundedHeldObjectGuard, ...]: + """Create phase-scoped held-object invariants for built-in semantics. + + The guard observes only named action segments whose commanded motion + assumes that a particular endpoint still holds the object. It never + creates or repairs a physical relation. + + Args: + analyzed: Statically linked semantic call. + effect_spec: Grounded terminal effect contract for the call. + context: Latest planning context used to validate verified baselines. + path: Diagnostic path for monitor-construction failures. + + Returns: + Independent guard monitors in deterministic phase order. + """ + monitor_ref = analyzed.effect_monitor_ref + if effect_spec is None or monitor_ref is None: + return () + + call = analyzed.call + definitions: tuple[ + tuple[ + str, + str, + tuple[str, ...], + HeldObjectGuardBaseline, + tuple[str, ...], + bool, + ], + ..., + ] + if type(call) is Pick: + destination = self._held_expectation(effect_spec, "destination") + definitions = ( + ( + "destination_attached", + destination.expectation_id, + ("lift",), + HeldObjectGuardBaseline.PLANNED_EFFECT, + (destination.task_state_key,), + True, + ), + ) + elif type(call) is Place: + source = self._held_expectation(effect_spec, "source") + self._validate_guard_verified_baseline(source, context) + definitions = ( + ( + "source_attached", + source.expectation_id, + ("approach",), + HeldObjectGuardBaseline.VERIFIED_TASK_STATE, + (source.task_state_key,), + False, + ), + ) + elif type(call) is HandOver: + source = self._held_expectation(effect_spec, "source") + destination = self._held_expectation(effect_spec, "destination") + self._validate_guard_verified_baseline(source, context) + definitions = ( + ( + "source_attached", + source.expectation_id, + ("transfer", "approach", "close", "hold"), + HeldObjectGuardBaseline.VERIFIED_TASK_STATE, + (source.task_state_key,), + False, + ), + ( + "destination_attached", + destination.expectation_id, + ("release", "deliver"), + HeldObjectGuardBaseline.PLANNED_EFFECT, + (source.task_state_key, destination.task_state_key), + False, + ), + ) + else: + return () + + guards: list[GroundedHeldObjectGuard] = [] + for ( + guard_id, + expectation_id, + active_segments, + baseline, + invalidation_keys, + retry_action, + ) in definitions: + guard_spec = self._attached_guard_effect_spec( + effect_spec, + expectation_id=expectation_id, + ) + try: + monitor = self._effect_monitor_registry.create( + guard_spec, + monitor_ref, + ) + except (KeyError, TypeError, ValueError) as exc: + raise _diagnostic( + "effect_guard_monitor_creation_failed", + (*path, guard_id), + f"Could not create held-object guard monitor: {exc}", + ) from exc + guards.append( + GroundedHeldObjectGuard( + guard_id=guard_id, + active_segments=active_segments, + baseline=baseline, + effect_spec=guard_spec, + effect_monitor=monitor, + invalidation_task_state_keys=invalidation_keys, + retry_action=retry_action, + ) + ) + return tuple(guards) + + @staticmethod + def _held_expectation( + spec: SemanticEffectSpec, + expectation_id: str, + ) -> HeldObjectStateExpectation: + """Resolve one exact held-object expectation from an effect spec.""" + expectation = spec.state_expectation(expectation_id) + if type(expectation) is not HeldObjectStateExpectation: + raise ValueError( + f"Effect expectation {expectation_id!r} is not held-object state." + ) + return expectation + + @staticmethod + def _validate_guard_verified_baseline( + expectation: HeldObjectStateExpectation, + context: PlanningContext, + ) -> None: + """Require the task state to own the guard's verified relation.""" + held = context.task.get_held_object(expectation.task_state_key) + if held is None or held.semantics.entity_id != expectation.object_id: + raise ValueError( + f"Held-object guard {expectation.expectation_id!r} requires " + f"verified object {expectation.object_id!r} under task-state key " + f"{expectation.task_state_key!r}." + ) + + @staticmethod + def _attached_guard_effect_spec( + terminal_spec: SemanticEffectSpec, + *, + expectation_id: str, + ) -> SemanticEffectSpec: + """Project one terminal expectation into an attached invariant.""" + terminal_expectation = terminal_spec.state_expectation(expectation_id) + if type(terminal_expectation) is not HeldObjectStateExpectation: + raise TypeError("Held-object guards require held-object expectations.") + attached = replace( + terminal_expectation, + relation=HeldObjectRelation.ATTACHED, + ) + clauses: list[EffectClause] = [] + for clause in terminal_spec.clauses: + if clause.expectation_id != expectation_id: + continue + if type(clause) is PoseRelationClause: + clauses.append( + PoseRelationClause( + clause_id=clause.clause_id, + expectation_id=clause.expectation_id, + source=clause.source, + expectation=PoseRelationExpectation.MATCHED, + ) + ) + elif type(clause) is BinaryEffectClause: + clauses.append(replace(clause, expected=True)) + elif type(clause) is ScalarEffectClause: + clauses.append(replace(clause, expectation=ScalarExpectation.PRESENT)) + else: + raise TypeError( + "Held-object guards support pose, binary, and scalar clauses." + ) + if not clauses: + raise ValueError( + f"Held-object expectation {expectation_id!r} has no physical clauses." + ) + return SemanticEffectSpec( + semantic_id=terminal_spec.semantic_id, + effect_kind=SemanticEffectKind.ATTACH, + 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=(attached,), + clauses=tuple(clauses), + ) + @staticmethod def _coordinated_cleanup_expectations( context: PlanningContext, @@ -2180,9 +2656,12 @@ def _broadcast_joint_position( __all__ = [ "AnalyzedSemanticCall", + "GroundedHeldObjectGuard", + "GroundedPhaseEffectGate", "GroundedSemanticCall", "HandOverPoseProvider", "HandOverPoseTargets", + "HeldObjectGuardBaseline", "RelationTargetGrounder", "RegisteredSemanticLowerer", "SemanticEffectDependency", diff --git a/embodichain/lab/sim/skills/effects.py b/embodichain/lab/sim/skills/effects.py index 852cb99a0..7bcbb4636 100644 --- a/embodichain/lab/sim/skills/effects.py +++ b/embodichain/lab/sim/skills/effects.py @@ -1509,12 +1509,90 @@ def _evidence_metadata( ) +@dataclass(frozen=True, slots=True, eq=False) +class EffectExpectationDecision: + """Per-row outcome for one physical state expectation. + + Rows absent from both ``satisfied_mask`` and ``contradicted_mask`` remain + unresolved. ``inverse_satisfied_mask`` is deliberately stronger than + contradiction: it requires every clause in the expectation group to have + reached its explicit inverse band for the configured consecutive-sample + window. This distinction lets failure reconciliation retain a relation + only from complete inverse evidence rather than from one contradictory + clause. + """ + + expectation_id: str + satisfied_mask: torch.Tensor + contradicted_mask: torch.Tensor + inverse_satisfied_mask: torch.Tensor + + def __post_init__(self) -> None: + _validate_identifier( + self.expectation_id, + field_name="EffectExpectationDecision.expectation_id", + ) + for field_name in ( + "satisfied_mask", + "contradicted_mask", + "inverse_satisfied_mask", + ): + value = getattr(self, field_name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{field_name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{field_name} must be a one-dimensional bool tensor.") + masks = ( + self.satisfied_mask, + self.contradicted_mask, + self.inverse_satisfied_mask, + ) + if any(value.shape != masks[0].shape for value in masks[1:]): + raise ValueError("Expectation decision masks must have equal shapes.") + if any(value.device != masks[0].device for value in masks[1:]): + raise ValueError("Expectation decision masks must use the same device.") + if (self.satisfied_mask & self.contradicted_mask).any(): + raise ValueError("satisfied_mask and contradicted_mask must not overlap.") + if (self.inverse_satisfied_mask & ~self.contradicted_mask).any(): + raise ValueError( + "inverse_satisfied_mask must be a subset of contradicted_mask." + ) + object.__setattr__(self, "satisfied_mask", self.satisfied_mask.clone()) + object.__setattr__( + self, + "contradicted_mask", + self.contradicted_mask.clone(), + ) + object.__setattr__( + self, + "inverse_satisfied_mask", + self.inverse_satisfied_mask.clone(), + ) + + def snapshot(self) -> EffectExpectationDecision: + """Return an independently owned expectation outcome.""" + return EffectExpectationDecision( + expectation_id=self.expectation_id, + satisfied_mask=self.satisfied_mask, + contradicted_mask=self.contradicted_mask, + inverse_satisfied_mask=self.inverse_satisfied_mask, + ) + + @dataclass(frozen=True, slots=True, eq=False) class EffectMonitorDecision: - """Uncorrelated per-row decision; runtime adds the verification ID.""" + """Uncorrelated aggregate and per-expectation monitor decision. + + When ``expectation_decisions`` is non-empty, the aggregate masks are + authoritative reductions of that current observation: success is the + conjunction of every satisfied mask and failure is the union of every + contradicted mask. This prevents callers from combining expectation + outcomes observed on different ticks. + """ success_mask: torch.Tensor failure_mask: torch.Tensor + expectation_decisions: tuple[EffectExpectationDecision, ...] = () def __post_init__(self) -> None: for field_name in ("success_mask", "failure_mask"): @@ -1529,8 +1607,51 @@ def __post_init__(self) -> None: raise ValueError("Decision masks must use the same device.") if (self.success_mask & self.failure_mask).any(): raise ValueError("Decision masks must not overlap.") + expectation_decisions = tuple(self.expectation_decisions) + if not all( + type(value) is EffectExpectationDecision for value in expectation_decisions + ): + raise TypeError( + "expectation_decisions must contain exact " + "EffectExpectationDecision values." + ) + expectation_ids = [value.expectation_id for value in expectation_decisions] + if len(set(expectation_ids)) != len(expectation_ids): + raise ValueError("Expectation decision IDs must be unique.") + if expectation_decisions: + for value in expectation_decisions: + if value.satisfied_mask.shape != self.success_mask.shape: + raise ValueError( + "Expectation and aggregate decision masks must have " + "equal shapes." + ) + if value.satisfied_mask.device != self.success_mask.device: + raise ValueError( + "Expectation and aggregate decision masks must use the " + "same device." + ) + expected_success = torch.ones_like(self.success_mask) + expected_failure = torch.zeros_like(self.failure_mask) + for value in expectation_decisions: + expected_success &= value.satisfied_mask + expected_failure |= value.contradicted_mask + if not torch.equal(self.success_mask, expected_success): + raise ValueError( + "success_mask must equal the conjunction of expectation " + "satisfied masks." + ) + if not torch.equal(self.failure_mask, expected_failure): + raise ValueError( + "failure_mask must equal the union of expectation " + "contradicted masks." + ) object.__setattr__(self, "success_mask", self.success_mask.clone()) object.__setattr__(self, "failure_mask", self.failure_mask.clone()) + object.__setattr__( + self, + "expectation_decisions", + tuple(value.snapshot() for value in expectation_decisions), + ) class EffectMonitor(ABC): @@ -1876,8 +1997,9 @@ def __init__( self._cfg = cfg self._attempt_generation: int | None = None self._active_env_ids: frozenset[int] = frozenset() - self._success_counts: dict[int, int] = {} - self._failure_counts: dict[int, int] = {} + self._success_counts: dict[tuple[str, int], int] = {} + self._failure_counts: dict[tuple[str, int], int] = {} + self._inverse_success_counts: dict[tuple[str, int], int] = {} self._last_observations: dict[int, tuple[float, int]] = {} @property @@ -1901,6 +2023,7 @@ def _prepare_request(self, request: EffectVerificationRequest) -> None: self._active_env_ids = active_env_ids self._success_counts.clear() self._failure_counts.clear() + self._inverse_success_counts.clear() self._last_observations.clear() return if not active_env_ids.issubset(self._active_env_ids): @@ -1910,14 +2033,19 @@ def _prepare_request(self, request: EffectVerificationRequest) -> None: ) self._active_env_ids = active_env_ids self._success_counts = { - env_id: count - for env_id, count in self._success_counts.items() - if env_id in active_env_ids + key: count + for key, count in self._success_counts.items() + if key[1] in active_env_ids } self._failure_counts = { - env_id: count - for env_id, count in self._failure_counts.items() - if env_id in active_env_ids + key: count + for key, count in self._failure_counts.items() + if key[1] in active_env_ids + } + self._inverse_success_counts = { + key: count + for key, count in self._inverse_success_counts.items() + if key[1] in active_env_ids } self._last_observations = { env_id: observation @@ -1996,9 +2124,13 @@ def _normalize_evidence( raise ValueError("Evidence contains env_ids outside the effect spec.") missing = self._active_env_ids.difference(observed_env_ids) if missing: + expectation_ids = {clause.expectation_id for clause in self._spec.clauses} for env_id in missing: - self._success_counts[env_id] = 0 - self._failure_counts[env_id] = 0 + for expectation_id in expectation_ids: + key = (expectation_id, env_id) + self._success_counts[key] = 0 + self._failure_counts[key] = 0 + self._inverse_success_counts[key] = 0 raise ValueError( "Evidence must cover every active request env_id exactly once; " f"missing {sorted(missing)}. Acquisition failures must be explicit " @@ -2095,8 +2227,6 @@ def observe( requested_at=request.requested_at, deadline=request.deadline, ) - success_mask = torch.zeros_like(request.env_mask) - failure_mask = torch.zeros_like(request.env_mask) spec_rows = { int(env_id): row for row, env_id in enumerate(self._spec.env_ids.detach().cpu().tolist()) @@ -2128,7 +2258,23 @@ def observe( clauses_by_expectation: dict[str, list[EffectClause]] = {} for clause in self._spec.clauses: clauses_by_expectation.setdefault(clause.expectation_id, []).append(clause) - physical_expectation_ids = set(clauses_by_expectation) + physical_expectation_ids = tuple( + expectation.expectation_id + for expectation in self._spec.state_expectations + if expectation.expectation_id in clauses_by_expectation + ) + satisfied_masks = { + expectation_id: torch.zeros_like(request.env_mask) + for expectation_id in physical_expectation_ids + } + contradicted_masks = { + expectation_id: torch.zeros_like(request.env_mask) + for expectation_id in physical_expectation_ids + } + inverse_satisfied_masks = { + expectation_id: torch.zeros_like(request.env_mask) + for expectation_id in physical_expectation_ids + } for evidence_row, env_id in enumerate(observed_env_ids): request_row = request_rows.get(env_id) @@ -2138,8 +2284,6 @@ def observe( continue self._last_observations[env_id] = observation_token spec_row = spec_rows[env_id] - expected_groups = True - contradicted_group = False for expectation_id in physical_expectation_ids: classifications = [ self._classify_clause( @@ -2153,24 +2297,55 @@ def observe( ] group_expected = all(value == 1 for value in classifications) group_contradicted = any(value == -1 for value in classifications) - expected_groups = expected_groups and group_expected - contradicted_group = contradicted_group or group_contradicted - if expected_groups: - self._success_counts[env_id] = self._success_counts.get(env_id, 0) + 1 - self._failure_counts[env_id] = 0 - elif contradicted_group: - self._failure_counts[env_id] = self._failure_counts.get(env_id, 0) + 1 - self._success_counts[env_id] = 0 - else: - self._success_counts[env_id] = 0 - self._failure_counts[env_id] = 0 - if self._success_counts.get(env_id, 0) >= self._cfg.consecutive_samples: - success_mask[request_row] = True - elif self._failure_counts.get(env_id, 0) >= self._cfg.consecutive_samples: - failure_mask[request_row] = True - success_mask &= request.env_mask - failure_mask &= request.env_mask - return EffectMonitorDecision(success_mask, failure_mask) + group_inverse_satisfied = all(value == -1 for value in classifications) + key = (expectation_id, env_id) + if group_expected: + self._success_counts[key] = self._success_counts.get(key, 0) + 1 + else: + self._success_counts[key] = 0 + if group_contradicted: + self._failure_counts[key] = self._failure_counts.get(key, 0) + 1 + else: + self._failure_counts[key] = 0 + if group_inverse_satisfied: + self._inverse_success_counts[key] = ( + self._inverse_success_counts.get(key, 0) + 1 + ) + else: + self._inverse_success_counts[key] = 0 + if self._success_counts.get(key, 0) >= self._cfg.consecutive_samples: + satisfied_masks[expectation_id][request_row] = True + if self._failure_counts.get(key, 0) >= self._cfg.consecutive_samples: + contradicted_masks[expectation_id][request_row] = True + if ( + self._inverse_success_counts.get(key, 0) + >= self._cfg.consecutive_samples + ): + inverse_satisfied_masks[expectation_id][request_row] = True + + expectation_decisions = tuple( + EffectExpectationDecision( + expectation_id=expectation_id, + satisfied_mask=satisfied_masks[expectation_id] & request.env_mask, + contradicted_mask=( + contradicted_masks[expectation_id] & request.env_mask + ), + inverse_satisfied_mask=( + inverse_satisfied_masks[expectation_id] & request.env_mask + ), + ) + for expectation_id in physical_expectation_ids + ) + success_mask = request.env_mask.clone() + failure_mask = torch.zeros_like(request.env_mask) + for decision in expectation_decisions: + success_mask &= decision.satisfied_mask + failure_mask |= decision.contradicted_mask + return EffectMonitorDecision( + success_mask, + failure_mask, + expectation_decisions, + ) class CompositeEffectMonitorFactory(EffectMonitorFactory): @@ -2222,6 +2397,7 @@ def create( "EffectEvidenceAddress", "EffectEvidenceBatch", "EffectEvidenceSourceRef", + "EffectExpectationDecision", "EffectMonitor", "EffectMonitorDecision", "EffectMonitorFactory", diff --git a/embodichain/lab/sim/skills/integration.py b/embodichain/lab/sim/skills/integration.py index 9acb72234..7453c78c3 100644 --- a/embodichain/lab/sim/skills/integration.py +++ b/embodichain/lab/sim/skills/integration.py @@ -1485,6 +1485,7 @@ def link_call( ), tracking_policy=preset.tracking_policy, recovery_policy=preset.recovery_policy, + workflow_recovery_policy=preset.workflow_recovery_policy, runner_cfg=preset.runner_cfg, effect_monitors=preset.effect_monitors, action_option_templates=preset.action_option_templates, diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index 8e6fd3656..06bcd7f3b 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -915,6 +915,35 @@ def __post_init__(self) -> None: object.__setattr__(self, "resources", MappingProxyType(normalized)) +@dataclass(frozen=True, slots=True) +class WorkflowRecoveryPolicy: + """Bound workflow-level recovery for curated semantic effect failures. + + The atomic action remains the owner of replans and whole-action retries. + This policy applies only after that action emits ``RECOVERY_REQUIRED`` and + returns control to :class:`~embodichain.lab.sim.skills.SkillRuntime`. + + Args: + max_recovery_attempts: Maximum recovery cycles for each environment row + at one semantic-call boundary. Zero disables workflow recovery. + """ + + max_recovery_attempts: int = 0 + + def __post_init__(self) -> None: + """Validate a finite, non-negative per-row recovery budget.""" + if type(self.max_recovery_attempts) is not int: + raise TypeError("max_recovery_attempts must be an integer.") + if not 0 <= self.max_recovery_attempts <= 100: + raise ValueError("max_recovery_attempts must be in [0, 100].") + + def snapshot(self) -> WorkflowRecoveryPolicy: + """Return an independently owned immutable policy.""" + return WorkflowRecoveryPolicy( + max_recovery_attempts=self.max_recovery_attempts, + ) + + @dataclass(frozen=True, slots=True, init=False) class SkillPolicyPreset: """Versioned policies and typed semantic-call option templates.""" @@ -924,6 +953,7 @@ class SkillPolicyPreset: _motion_policy: MotionPolicy _tracking_policy: TrackingPolicy _recovery_policy: RecoveryPolicy + _workflow_recovery_policy: WorkflowRecoveryPolicy _runner_cfg: ExecutionRunnerCfg _effect_monitors: Mapping[str, EffectMonitorRef] _action_option_templates: Mapping[str, ActionOptions] @@ -933,10 +963,11 @@ def __init__( preset_id: str, *, action_option_templates: Mapping[str, ActionOptions], - schema_version: int = 2, + schema_version: int = 3, motion_policy: MotionPolicy | None = None, tracking_policy: TrackingPolicy | None = None, recovery_policy: RecoveryPolicy | None = None, + workflow_recovery_policy: WorkflowRecoveryPolicy | None = None, runner_cfg: ExecutionRunnerCfg | None = None, effect_monitors: Mapping[str, EffectMonitorRef] | None = None, ) -> None: @@ -944,10 +975,10 @@ def __init__( _validate_identifier(preset_id, field_name="SkillPolicyPreset.preset_id") if not isinstance(schema_version, int) or isinstance(schema_version, bool): raise TypeError("SkillPolicyPreset.schema_version must be an integer.") - if schema_version != 2: + if schema_version != 3: raise ValueError( "Unsupported SkillPolicyPreset.schema_version " - f"{schema_version}; supported versions are [2]." + f"{schema_version}; supported versions are [3]." ) selected_motion = MotionPolicy() if motion_policy is None else motion_policy selected_tracking = ( @@ -958,6 +989,11 @@ def __init__( selected_recovery = ( RecoveryPolicy() if recovery_policy is None else recovery_policy ) + selected_workflow_recovery = ( + WorkflowRecoveryPolicy() + if workflow_recovery_policy is None + else workflow_recovery_policy + ) selected_runner = ExecutionRunnerCfg() if runner_cfg is None else runner_cfg if not isinstance(selected_motion, MotionPolicy): raise TypeError("motion_policy must be a MotionPolicy.") @@ -965,6 +1001,10 @@ def __init__( raise TypeError("tracking_policy must be a TrackingPolicy.") if not isinstance(selected_recovery, RecoveryPolicy): raise TypeError("recovery_policy must be a RecoveryPolicy.") + if type(selected_workflow_recovery) is not WorkflowRecoveryPolicy: + raise TypeError( + "workflow_recovery_policy must be a WorkflowRecoveryPolicy." + ) if not isinstance(selected_runner, ExecutionRunnerCfg): raise TypeError("runner_cfg must be an ExecutionRunnerCfg.") selected_effect_monitors = ( @@ -1012,6 +1052,11 @@ def __init__( object.__setattr__(self, "_motion_policy", deepcopy(selected_motion)) object.__setattr__(self, "_tracking_policy", deepcopy(selected_tracking)) object.__setattr__(self, "_recovery_policy", deepcopy(selected_recovery)) + object.__setattr__( + self, + "_workflow_recovery_policy", + selected_workflow_recovery.snapshot(), + ) object.__setattr__(self, "_runner_cfg", deepcopy(selected_runner)) object.__setattr__( self, @@ -1034,6 +1079,11 @@ def recovery_policy(self) -> RecoveryPolicy: """Return an independently owned recovery policy.""" return deepcopy(self._recovery_policy) + @property + def workflow_recovery_policy(self) -> WorkflowRecoveryPolicy: + """Return the bounded semantic-workflow recovery policy.""" + return self._workflow_recovery_policy.snapshot() + @property def tracking_policy(self) -> TrackingPolicy: """Return independently owned endpoint-tracking settings.""" @@ -1091,6 +1141,7 @@ def snapshot(self) -> SkillPolicyPreset: motion_policy=self.motion_policy, tracking_policy=self.tracking_policy, recovery_policy=self.recovery_policy, + workflow_recovery_policy=self.workflow_recovery_policy, runner_cfg=self.runner_cfg, effect_monitors=self.effect_monitors, action_option_templates=self.action_option_templates, @@ -2380,4 +2431,5 @@ def _lower_binding( "RobotSkillProfile", "SkillPolicyPreset", "UnsupportedSkillError", + "WorkflowRecoveryPolicy", ] diff --git a/embodichain/lab/sim/skills/runtime.py b/embodichain/lab/sim/skills/runtime.py index 46fe7da6e..2aa602a96 100644 --- a/embodichain/lab/sim/skills/runtime.py +++ b/embodichain/lab/sim/skills/runtime.py @@ -18,6 +18,7 @@ from __future__ import annotations +from collections import deque from collections.abc import Iterable, Mapping from dataclasses import dataclass, fields, is_dataclass, replace from enum import Enum @@ -29,11 +30,18 @@ from ..atomic_actions.bindings import EndpointBinding from ..atomic_actions.engine import AtomicActionEngine +from ..atomic_actions.effects import StateDelta from ..atomic_actions.execution import ( + EffectExpectationResult, EffectVerificationRequest, EffectVerificationResult, ExecutionEvent, + ExecutionEventKind, ExecutionPlanAttempt, + HeldObjectGuardRequest, + HeldObjectGuardResult, + PhaseEffectGateRequest, + PhaseEffectGateResult, ) from ..atomic_actions.plans import TrajectorySegment from ..atomic_actions.policies import MotionPolicy, RecoveryPolicy @@ -47,20 +55,29 @@ RunnerStatus, RunnerStep, ) -from ..atomic_actions.state import PlanningContext, TaskState +from ..atomic_actions.state import HeldObjectState, PlanningContext, TaskState from ..atomic_actions.tracking import ( FeedbackTerminalAcceptance, TimedTrackingSequence, TrackingMetricCfg, TrackingPolicy, ) -from .calls import SemanticCallSpec -from .compiler import SemanticSkillCompiler +from .calls import HandOver, Pick, Place, SemanticCallSpec +from .compiler import ( + GroundedHeldObjectGuard, + GroundedPhaseEffectGate, + HeldObjectGuardBaseline, + SemanticSkillCompiler, +) from .effects import ( BinaryEffectEvidenceBatch, EffectEvidenceBatch, + EffectExpectationDecision, EffectMonitor, + EffectMonitorDecision, EffectMonitorRef, + HeldObjectRelation, + HeldObjectStateExpectation, JointStateEvidenceBatch, PoseRelationEvidenceBatch, ScalarEffectEvidenceBatch, @@ -74,6 +91,7 @@ SceneObjectRef, SceneRegistry, ) +from .profiles import WorkflowRecoveryPolicy def _snapshot_task_state(state: TaskState) -> TaskState: @@ -236,6 +254,14 @@ class SkillStatus(str, Enum): CANCELLED = "cancelled" +class SkillWorkflowRecoveryRole(str, Enum): + """Role of one real semantic call inside workflow recovery.""" + + RETRY_RETAINED = "retry_retained" + REACQUIRE = "reacquire" + RETRY_REACQUIRED = "retry_reacquired" + + @dataclass(frozen=True, slots=True) class SkillEndpointTrackingChannelTrace: """Stable provider and projector route for one endpoint feedback channel.""" @@ -947,12 +973,17 @@ class SkillEffectTrace: timestamp: float success_mask: torch.Tensor failure_mask: torch.Tensor + expectation_decisions: tuple[EffectExpectationDecision, ...] effect_spec: SemanticEffectSpec monitor_id: str monitor_revision: str | None configured_monitor_params: Mapping[str, object] resolved_monitor_params: Mapping[str, object] 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: if type(self.call_index) is not int or self.call_index < 0: @@ -961,6 +992,44 @@ 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", + "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.gate_id is not None + or self.segment_name is not None + ): + raise ValueError( + "Terminal effect traces cannot declare segment-boundary metadata." + ) + 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( + "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.") for name in ("success_mask", "failure_mask"): @@ -977,6 +1046,54 @@ def __post_init__(self) -> None: raise ValueError("Effect trace masks must not overlap.") if not isinstance(self.effect_spec, SemanticEffectSpec): raise TypeError("effect_spec must be a SemanticEffectSpec.") + expectation_decisions = tuple(self.expectation_decisions) + if not all( + type(value) is EffectExpectationDecision for value in expectation_decisions + ): + raise TypeError( + "expectation_decisions must contain exact " + "EffectExpectationDecision values." + ) + for value in expectation_decisions: + if value.satisfied_mask.shape != self.success_mask.shape: + raise ValueError( + "Expectation and aggregate trace masks must have equal shapes." + ) + if value.satisfied_mask.device != self.success_mask.device: + raise ValueError( + "Expectation and aggregate trace masks must share a device." + ) + physical_ids = tuple( + expectation.expectation_id + for expectation in self.effect_spec.state_expectations + if any( + clause.expectation_id == expectation.expectation_id + for clause in self.effect_spec.clauses + ) + ) + outcome_ids = tuple(value.expectation_id for value in expectation_decisions) + if outcome_ids != physical_ids: + raise ValueError( + "Effect trace must contain one ordered outcome for every " + f"physical expectation; expected={physical_ids}, " + f"got={outcome_ids}." + ) + if expectation_decisions: + expected_success = torch.ones_like(self.success_mask) + expected_failure = torch.zeros_like(self.failure_mask) + for value in expectation_decisions: + expected_success &= value.satisfied_mask + expected_failure |= value.contradicted_mask + if not torch.equal(self.success_mask, expected_success): + raise ValueError( + "success_mask must equal the conjunction of expectation " + "trace outcomes." + ) + if not torch.equal(self.failure_mask, expected_failure): + raise ValueError( + "failure_mask must equal the union of expectation trace " + "outcomes." + ) if type(self.monitor_id) is not str or not self.monitor_id: raise ValueError("monitor_id must be a non-empty string.") if self.monitor_revision is not None and ( @@ -1000,6 +1117,11 @@ def __post_init__(self) -> None: evidence[evidence_id] = batch.snapshot() object.__setattr__(self, "success_mask", self.success_mask.clone()) object.__setattr__(self, "failure_mask", self.failure_mask.clone()) + object.__setattr__( + self, + "expectation_decisions", + tuple(value.snapshot() for value in expectation_decisions), + ) object.__setattr__(self, "effect_spec", self.effect_spec.snapshot()) object.__setattr__( self, @@ -1022,17 +1144,22 @@ def snapshot(self) -> SkillEffectTrace: timestamp=self.timestamp, success_mask=self.success_mask, failure_mask=self.failure_mask, + expectation_decisions=self.expectation_decisions, effect_spec=self.effect_spec, monitor_id=self.monitor_id, monitor_revision=self.monitor_revision, configured_monitor_params=self.configured_monitor_params, resolved_monitor_params=self.resolved_monitor_params, evidence=self.evidence, + boundary_kind=self.boundary_kind, + guard_id=self.guard_id, + gate_id=self.gate_id, + segment_name=self.segment_name, ) def to_metadata(self) -> dict[str, object]: """Return monitor contract, evidence, thresholds, and decision metadata.""" - return { + metadata = { "call_index": self.call_index, "verification_id": self.verification_id, "observation_revision": self.observation_revision, @@ -1051,8 +1178,35 @@ def to_metadata(self) -> dict[str, object]: "decision": { "success_mask": _metadata_value(self.success_mask), "failure_mask": _metadata_value(self.failure_mask), + "expectations": [ + { + "expectation_id": value.expectation_id, + "satisfied_mask": _metadata_value(value.satisfied_mask), + "contradicted_mask": _metadata_value(value.contradicted_mask), + "inverse_satisfied_mask": _metadata_value( + value.inverse_satisfied_mask + ), + } + for value in self.expectation_decisions + ], }, } + metadata["boundary"] = {"kind": self.boundary_kind} + if self.boundary_kind == "in_flight_guard": + metadata["boundary"].update( + { + "guard_id": self.guard_id, + "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 @dataclass(frozen=True, slots=True, eq=False) @@ -1264,6 +1418,211 @@ def to_metadata(self) -> dict[str, object]: } +@dataclass(frozen=True, slots=True, eq=False) +class SkillWorkflowRecoveryTrace: + """One auditable real semantic call within bounded workflow recovery. + + Args: + recovery_id: Monotonic runtime-local trace identifier. + trigger_call_index: Original workflow call held at the shared barrier. + trigger_semantic_id: Semantic ID of the original failed call. + attempt_index: One-based per-row recovery-cycle index. + max_recovery_attempts: Configured per-row recovery-cycle budget. + role: Whether this call retries, re-acquires, or retries after pickup. + source_resource_id: Resolved robot resource that owns the source object. + source_task_state_key: Verified held-object state key for that resource. + entered_mask: Rows that entered this real recovery call. + completed_mask: Entered rows that completed this call. + failed_mask: Entered rows that failed this call. + call: Nested semantic-call trace, or ``None`` when preparation failed. + message: Optional terminal or preparation diagnostic. + """ + + recovery_id: int + trigger_call_index: int + trigger_semantic_id: str + attempt_index: int + max_recovery_attempts: int + role: SkillWorkflowRecoveryRole + source_resource_id: str + source_task_state_key: str + entered_mask: torch.Tensor + completed_mask: torch.Tensor + failed_mask: torch.Tensor + call: SkillCallTrace | None + message: str | None = None + + def __post_init__(self) -> None: + if type(self.recovery_id) is not int or self.recovery_id < 0: + raise ValueError("recovery_id must be a non-negative integer.") + if type(self.trigger_call_index) is not int or self.trigger_call_index < 0: + raise ValueError("trigger_call_index must be non-negative.") + for name in ( + "trigger_semantic_id", + "source_resource_id", + "source_task_state_key", + ): + value = getattr(self, name) + if type(value) is not str or not value: + raise ValueError(f"{name} must be a non-empty string.") + if type(self.attempt_index) is not int or self.attempt_index <= 0: + raise ValueError("attempt_index must be a positive integer.") + if ( + type(self.max_recovery_attempts) is not int + or self.max_recovery_attempts <= 0 + or self.attempt_index > self.max_recovery_attempts + ): + raise ValueError( + "max_recovery_attempts must cover the positive attempt_index." + ) + if not isinstance(self.role, SkillWorkflowRecoveryRole): + raise TypeError("role must be a SkillWorkflowRecoveryRole.") + for name in ("entered_mask", "completed_mask", "failed_mask"): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.dim() != 1: + raise ValueError(f"{name} must be a one-dimensional bool tensor.") + if not ( + self.entered_mask.shape + == self.completed_mask.shape + == self.failed_mask.shape + ) or not ( + self.entered_mask.device + == self.completed_mask.device + == self.failed_mask.device + ): + raise ValueError("Workflow-recovery masks must share shape and device.") + if (self.completed_mask & self.failed_mask).any(): + raise ValueError("Recovery completion and failure masks cannot overlap.") + if ((self.completed_mask | self.failed_mask) & ~self.entered_mask).any(): + raise ValueError("Recovery outcomes must be subsets of entered_mask.") + if self.call is not None: + if type(self.call) is not SkillCallTrace: + raise TypeError("call must be a SkillCallTrace or None.") + if not torch.equal(self.call.entered_mask, self.entered_mask): + raise ValueError("Recovery call entered_mask must match the trace.") + if not torch.equal(self.call.completed_mask, self.completed_mask): + raise ValueError("Recovery call completed_mask must match the trace.") + if not torch.equal(self.call.failed_mask, self.failed_mask): + raise ValueError("Recovery call failed_mask must match the trace.") + elif not torch.equal(self.failed_mask, self.entered_mask): + raise ValueError( + "A recovery preparation failure must fail every entered row." + ) + if self.message is not None and ( + type(self.message) is not str or not self.message + ): + raise ValueError("message must be a non-empty string or None.") + for name in ("entered_mask", "completed_mask", "failed_mask"): + object.__setattr__(self, name, getattr(self, name).clone()) + if self.call is not None: + object.__setattr__(self, "call", self.call.snapshot()) + + def snapshot(self) -> SkillWorkflowRecoveryTrace: + """Return an independently owned workflow-recovery trace.""" + return SkillWorkflowRecoveryTrace( + recovery_id=self.recovery_id, + trigger_call_index=self.trigger_call_index, + trigger_semantic_id=self.trigger_semantic_id, + attempt_index=self.attempt_index, + max_recovery_attempts=self.max_recovery_attempts, + role=self.role, + source_resource_id=self.source_resource_id, + source_task_state_key=self.source_task_state_key, + entered_mask=self.entered_mask, + completed_mask=self.completed_mask, + failed_mask=self.failed_mask, + call=self.call, + message=self.message, + ) + + def to_metadata(self) -> dict[str, object]: + """Return deterministic JSON-safe workflow-recovery metadata.""" + return { + "recovery_id": self.recovery_id, + "trigger_call_index": self.trigger_call_index, + "trigger_semantic_id": self.trigger_semantic_id, + "attempt_index": self.attempt_index, + "max_recovery_attempts": self.max_recovery_attempts, + "role": self.role.value, + "source_resource_id": self.source_resource_id, + "source_task_state_key": self.source_task_state_key, + "masks": { + "entered": _metadata_value(self.entered_mask), + "completed": _metadata_value(self.completed_mask), + "failed": _metadata_value(self.failed_mask), + }, + "call": None if self.call is None else self.call.to_metadata(), + "message": self.message, + } + + +@dataclass(frozen=True, slots=True, eq=False) +class _WorkflowRecoveryWorkItem: + """One cohort scheduled at a shared semantic-call recovery barrier.""" + + role: SkillWorkflowRecoveryRole + call: SemanticCallSpec + env_mask: torch.Tensor + attempt_index: int + + def __post_init__(self) -> None: + if not isinstance(self.role, SkillWorkflowRecoveryRole): + raise TypeError("role must be a SkillWorkflowRecoveryRole.") + if not isinstance(self.call, SemanticCallSpec): + raise TypeError("call must be a SemanticCallSpec.") + if ( + not isinstance(self.env_mask, torch.Tensor) + or self.env_mask.dtype != torch.bool + or self.env_mask.dim() != 1 + or not self.env_mask.any() + ): + raise ValueError( + "env_mask must be a non-empty one-dimensional bool tensor." + ) + if type(self.attempt_index) is not int or self.attempt_index <= 0: + raise ValueError("attempt_index must be a positive integer.") + object.__setattr__(self, "env_mask", self.env_mask.clone()) + + +@dataclass(slots=True) +class _WorkflowRecoveryBarrier: + """Mutable per-call barrier while failed rows recover and rejoin.""" + + trigger_call_index: int + trigger_call: SemanticCallSpec + policy: WorkflowRecoveryPolicy + source_resource_id: str + source_task_state_key: str + entered_mask: torch.Tensor + success_mask: torch.Tensor + final_failure_mask: torch.Tensor + attempt_counts: torch.Tensor + work_items: deque[_WorkflowRecoveryWorkItem] + failure_messages: list[str] + + +@dataclass(frozen=True, slots=True) +class _FinishedCallAttempt: + """Internal terminal projection of one execution session.""" + + trace: SkillCallTrace + completed_mask: torch.Tensor + failed_mask: torch.Tensor + status: RunnerStatus + message: str | None + + +@dataclass(frozen=True, slots=True) +class _WorkflowRecoveryTrigger: + """Resolved workflow policy and source identity for one original call.""" + + policy: WorkflowRecoveryPolicy + source_resource_id: str + source_task_state_key: str + + @dataclass(frozen=True, slots=True, eq=False) class SkillResult: """Immutable workflow snapshot returned by sync and step-wise execution.""" @@ -1280,6 +1639,7 @@ class SkillResult: events: tuple[ExecutionEvent, ...] = () calls: tuple[SkillCallTrace, ...] = () effects: tuple[SkillEffectTrace, ...] = () + workflow_recoveries: tuple[SkillWorkflowRecoveryTrace, ...] = () failures: tuple[SkillFailure, ...] = () wait_duration: float = 0.0 message: str | None = None @@ -1335,6 +1695,13 @@ def __post_init__(self) -> None: raise ValueError("wait_duration must be finite and non-negative.") if self.message is not None and type(self.message) is not str: raise TypeError("message must be a string or None.") + if not all( + type(recovery) is SkillWorkflowRecoveryTrace + for recovery in self.workflow_recoveries + ): + raise TypeError( + "workflow_recoveries must contain SkillWorkflowRecoveryTrace values." + ) object.__setattr__(self, "env_ids", self.env_ids.clone()) for name in ( "success_mask", @@ -1359,6 +1726,11 @@ def __post_init__(self) -> None: "effects", tuple(effect.snapshot() for effect in self.effects), ) + object.__setattr__( + self, + "workflow_recoveries", + tuple(recovery.snapshot() for recovery in self.workflow_recoveries), + ) object.__setattr__( self, "failures", @@ -1377,13 +1749,15 @@ def terminal(self) -> bool: def to_metadata(self) -> dict[str, object]: """Return a fresh deterministic JSON-safe workflow result. - Recovery remains represented by the ordered :class:`ExecutionEvent` - stream and by each call's complete plan-attempt history. The returned - object owns only Python scalars, lists, and dictionaries and can be - serialized with ``json.dumps(..., allow_nan=False)``. + Core recovery remains represented by the ordered + :class:`ExecutionEvent` stream and each call's plan-attempt history. + Workflow re-acquisition additionally appears in + ``workflow_recoveries``. The returned object owns only Python scalars, + lists, and dictionaries and can be serialized with + ``json.dumps(..., allow_nan=False)``. """ return { - "schema_version": 1, + "schema_version": 2, "kind": "skill_result", "status": self.status.value, "workflow_id": self.workflow_id, @@ -1399,6 +1773,9 @@ def to_metadata(self) -> dict[str, object]: "events": [_event_to_metadata(event) for event in self.events], "calls": [call.to_metadata() for call in self.calls], "effects": [effect.to_metadata() for effect in self.effects], + "workflow_recoveries": [ + recovery.to_metadata() for recovery in self.workflow_recoveries + ], "failures": [failure.to_metadata() for failure in self.failures], "wait_duration": self.wait_duration, "message": self.message, @@ -1523,6 +1900,9 @@ def __init__( self._current_call_index: int | None = None self._runner: ExecutionRunner | None = None self._grounded: object | None = None + self._active_call: SemanticCallSpec | None = None + self._active_recovery_item: _WorkflowRecoveryWorkItem | None = None + self._recovery_barrier: _WorkflowRecoveryBarrier | None = None self._call_entered_mask = torch.zeros( self._task_state.batch_size, dtype=torch.bool, @@ -1535,10 +1915,14 @@ def __init__( self._events: list[ExecutionEvent] = [] self._call_traces: list[SkillCallTrace] = [] self._effect_traces: list[SkillEffectTrace] = [] + self._workflow_recovery_traces: list[SkillWorkflowRecoveryTrace] = [] self._failures: list[SkillFailure] = [] self._call_event_offset = 0 self._call_effect_offset = 0 self._observation_revision = 0 + self._next_guard_verification_id = 0 + self._next_gate_verification_id = 0 + self._next_workflow_recovery_id = 0 self._wait_duration = 0.0 self._message: str | None = None @@ -1647,6 +2031,7 @@ def result(self) -> SkillResult: events=tuple(self._events), calls=tuple(self._call_traces), effects=tuple(self._effect_traces), + workflow_recoveries=tuple(self._workflow_recovery_traces), failures=tuple(self._failures), wait_duration=self._wait_duration, message=self._message, @@ -1704,7 +2089,15 @@ def step(self) -> SkillResult: grounded = self._require_grounded() monitor = getattr(grounded, "effect_monitor", None) verifier = self._effect_verifier if monitor is not None else None - runner_step = runner.step(effect_verifier=verifier) + 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) if ( runner_step.status is RunnerStatus.RUNNING @@ -1717,38 +2110,31 @@ 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) - if runner_step.status is RunnerStatus.COMPLETED: - if self._eligible.any() and self._has_next_call: - assert self._current_call_index is not None - next_index = self._current_call_index + 1 - try: - self._prepare_call(next_index) - except Exception as exc: # noqa: BLE001 - preserve workflow trace - self._fail_preparation(next_index, exc) - elif self._eligible.any(): - self._success = self._eligible.clone() - self._status = SkillStatus.COMPLETED - self._current_call_index = None - self._wait_duration = 0.0 - else: - self._status = ( - SkillStatus.CANCELLED - if self._cancelled.any() and not self._failed.any() - else SkillStatus.FAILED - ) - self._current_call_index = None - self._wait_duration = 0.0 - elif runner_step.status is RunnerStatus.CANCELLED: - self._status = SkillStatus.CANCELLED - self._current_call_index = None - self._wait_duration = 0.0 + recovery_item = self._active_recovery_item + trigger = ( + self._workflow_recovery_trigger() + if recovery_item is None and self._active_call_requires_workflow_recovery() + else None + ) + finished = self._finish_active_call(runner_step) + if recovery_item is None: + self._call_traces.append(finished.trace) + self._handle_original_call_finished(finished, trigger=trigger) else: - self._status = SkillStatus.FAILED - self._current_call_index = None - self._wait_duration = 0.0 + self._handle_recovery_call_finished(recovery_item, finished) return self.result def run( @@ -1785,21 +2171,34 @@ def cancel( raise ValueError("reason must be a non-empty string.") if self._status is not SkillStatus.RUNNING: return self.result + pending = self._eligible.clone() + recovery_item = self._active_recovery_item runner_step = self._require_runner().cancel(reason) self._consume_runner_step(runner_step) - active = self._eligible & ~self._failed self._message = runner_step.message or reason - self._finish_current_call(runner_step) - self._cancelled |= active - self._eligible &= ~active + finished = self._finish_active_call(runner_step) + if recovery_item is None: + self._call_traces.append(finished.trace) + else: + self._append_workflow_recovery_trace( + recovery_item, + call=finished.trace, + completed_mask=finished.completed_mask, + failed_mask=finished.failed_mask, + message=finished.message, + ) self._status = ( SkillStatus.CANCELLED if runner_step.status is RunnerStatus.CANCELLED else SkillStatus.FAILED ) if self._status is SkillStatus.FAILED: - self._failed |= active - self._cancelled &= ~active + self._failed |= pending + self._cancelled &= ~pending + else: + self._cancelled |= pending + self._eligible &= ~pending + self._recovery_barrier = None self._current_call_index = None self._wait_duration = 0.0 return self.result @@ -1837,16 +2236,44 @@ def deactivate_rows( ) if type(reason) is not str or not reason: raise ValueError("reason must be a non-empty string.") - changed = self._require_runner().deactivate_rows( - env_mask & self._eligible, + changed = env_mask & self._eligible + self._require_runner().deactivate_rows( + changed, reason=reason, ) self._cancelled |= changed self._eligible &= ~changed + barrier = self._recovery_barrier + if barrier is not None and changed.any(): + barrier.success_mask &= ~changed + retained_items: deque[_WorkflowRecoveryWorkItem] = deque() + for item in barrier.work_items: + retained = item.env_mask & ~changed + if retained.any(): + retained_items.append( + _WorkflowRecoveryWorkItem( + role=item.role, + call=item.call, + env_mask=retained, + attempt_index=item.attempt_index, + ) + ) + barrier.work_items = retained_items if not self._eligible.any(): + recovery_item = self._active_recovery_item runner_step = self._require_runner().cancel(reason) self._consume_runner_step(runner_step) - self._finish_current_call(runner_step) + finished = self._finish_active_call(runner_step) + if recovery_item is None: + self._call_traces.append(finished.trace) + else: + self._append_workflow_recovery_trace( + recovery_item, + call=finished.trace, + completed_mask=finished.completed_mask, + failed_mask=finished.failed_mask, + message=finished.message, + ) self._status = ( SkillStatus.CANCELLED if runner_step.status is RunnerStatus.CANCELLED @@ -1855,6 +2282,7 @@ def deactivate_rows( if self._status is SkillStatus.FAILED: failed = self._call_entered_mask & ~self._cancelled self._failed |= failed + self._recovery_barrier = None self._current_call_index = None self._wait_duration = 0.0 return self.result @@ -1959,6 +2387,9 @@ def _reset_workflow( self._current_call_index = 0 self._runner = None self._grounded = None + self._active_call = None + self._active_recovery_item = None + self._recovery_barrier = None self._eligible = eligible self._success = torch.zeros_like(eligible) self._failed = torch.zeros_like(eligible) @@ -1966,10 +2397,14 @@ def _reset_workflow( self._events = [] self._call_traces = [] self._effect_traces = [] + self._workflow_recovery_traces = [] self._failures = [] self._call_event_offset = 0 self._call_effect_offset = 0 self._observation_revision = 0 + self._next_guard_verification_id = 0 + self._next_gate_verification_id = 0 + self._next_workflow_recovery_id = 0 self._wait_duration = 0.0 self._message = None self._status = SkillStatus.RUNNING @@ -2009,22 +2444,72 @@ def _observe_for_grounding(self) -> PlanningContext: def _prepare_call(self, call_index: int) -> None: """Freshly ground and create exactly one session and runner.""" assert self._workflow is not None + self._prepare_grounded_call( + self._workflow, + analysis_call_index=call_index, + workflow_call_index=call_index, + call=self._calls[call_index], + active_mask=self._eligible, + recovery_item=None, + ) + + def _prepare_recovery_work_item( + self, + item: _WorkflowRecoveryWorkItem, + ) -> None: + """Analyze and ground one real recovery call with fresh observation.""" + barrier = self._require_recovery_barrier() + suffix = self._calls[barrier.trigger_call_index :] + analysis_calls = ( + (item.call, *suffix) + if item.role is SkillWorkflowRecoveryRole.REACQUIRE + else suffix + ) + workflow = self._compiler.analyze( + analysis_calls, + workflow_id=( + f"{self._workflow_id}:workflow_recovery:" + f"{self._next_workflow_recovery_id}" + ), + ) + self._prepare_grounded_call( + workflow, + analysis_call_index=0, + workflow_call_index=barrier.trigger_call_index, + call=item.call, + active_mask=item.env_mask, + recovery_item=item, + ) + + def _prepare_grounded_call( + self, + workflow: object, + *, + analysis_call_index: int, + workflow_call_index: int, + call: SemanticCallSpec, + active_mask: torch.Tensor, + recovery_item: _WorkflowRecoveryWorkItem | None, + ) -> None: + """Install one original or recovery semantic call in a fresh session.""" context = self._observe_for_grounding() grounded = self._compiler.ground( - self._workflow, - call_index, + workflow, + analysis_call_index, context, - eligible_mask=self._eligible, + eligible_mask=active_mask, ) invocation = getattr(grounded, "invocation", None) grounded_eligible = getattr(grounded, "eligible_mask", 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( grounded_eligible, - self._eligible, + active_mask, ): raise ValueError("Grounded call must preserve runtime eligibility.") if (effect_spec is None) != (effect_monitor is None): @@ -2041,12 +2526,26 @@ def _prepare_call(self, call_index: int) -> None: context.env_ids, ): raise ValueError("Grounded effect env_ids must match the call context.") + if not all(type(value) is GroundedHeldObjectGuard for value in effect_guards): + raise TypeError( + "Grounded effect_guards must contain exact " + "GroundedHeldObjectGuard values." + ) + 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( (invocation,), context, - eligible_mask=self._eligible, + eligible_mask=active_mask, ) primed = _PrimedObservationProvider(context, self._observation_provider) runner = ExecutionRunner( @@ -2056,9 +2555,11 @@ def _prepare_call(self, call_index: int) -> None: clock=self._clock, cfg=self._runner_cfg, ) - self._current_call_index = call_index + self._current_call_index = workflow_call_index self._runner = runner - self._call_entered_mask = self._eligible.clone() + self._active_call = call + self._active_recovery_item = recovery_item + self._call_entered_mask = active_mask.clone() self._call_event_offset = len(self._events) self._call_effect_offset = len(self._effect_traces) self._wait_duration = 0.0 @@ -2088,6 +2589,342 @@ def _effect_verifier( ) if request.invocation_revision != spec.invocation_revision: raise ValueError("Effect request revision does not match the effect spec.") + decision = self._observe_effect_monitor( + context, + request, + spec=spec, + monitor=monitor, + ) + expectation_decisions = self._validated_expectation_decisions( + spec, + decision, + ) + invalidation_mask, retry_mask = self._terminal_failure_policy( + grounded, + decision.failure_mask, + expectation_decisions, + ) + return EffectVerificationResult( + verification_id=request.verification_id, + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + invalidation_mask=invalidation_mask, + retry_mask=retry_mask, + expectation_results=tuple( + EffectExpectationResult( + expectation_id=value.expectation_id, + satisfied_mask=value.satisfied_mask, + contradicted_mask=value.contradicted_mask, + inverse_satisfied_mask=value.inverse_satisfied_mask, + ) + for value in expectation_decisions + ), + ) + + @staticmethod + def _validated_expectation_decisions( + spec: SemanticEffectSpec, + decision: EffectMonitorDecision, + ) -> tuple[EffectExpectationDecision, ...]: + """Require one current-observation outcome per physical expectation.""" + physical_ids = tuple( + expectation.expectation_id + for expectation in spec.state_expectations + if any( + clause.expectation_id == expectation.expectation_id + for clause in spec.clauses + ) + ) + outcomes = tuple(decision.expectation_decisions) + if not outcomes and len(physical_ids) == 1: + outcomes = ( + EffectExpectationDecision( + expectation_id=physical_ids[0], + satisfied_mask=decision.success_mask, + contradicted_mask=decision.failure_mask, + inverse_satisfied_mask=torch.zeros_like(decision.failure_mask), + ), + ) + outcome_ids = tuple(value.expectation_id for value in outcomes) + if outcome_ids != physical_ids: + raise ValueError( + "Effect monitor must return one ordered outcome for every " + f"physical expectation; expected={physical_ids}, got={outcome_ids}." + ) + return outcomes + + @staticmethod + def _terminal_failure_policy( + grounded: object, + failure_mask: torch.Tensor, + expectation_decisions: tuple[EffectExpectationDecision, ...], + ) -> tuple[torch.Tensor, torch.Tensor]: + """Select fail-closed invalidation and safe local retry rows.""" + call = getattr(getattr(grounded, "analyzed", None), "call", None) + invalidation = failure_mask.clone() + retry = failure_mask.clone() + if type(call) is Pick: + return invalidation, retry + if type(call) is Place: + source = next( + value + for value in expectation_decisions + if value.expectation_id == "source" + ) + retained = failure_mask & source.inverse_satisfied_mask + return failure_mask & ~retained, retained + if type(call) is HandOver: + source = next( + value + for value in expectation_decisions + if value.expectation_id == "source" + ) + retained = failure_mask & source.inverse_satisfied_mask + 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, + request: HeldObjectGuardRequest, + ) -> HeldObjectGuardResult | None: + """Observe a phase-scoped held-object invariant before dispatch. + + Args: + context: Fresh due-cycle physical observation. + request: Core-owned phase and correlation identity. + + Returns: + Correlated row-local loss decision, or ``None`` when this named + action segment has no held-object invariant. + """ + if context.robot.timestamp > request.deadline: + return None + grounded = self._require_grounded() + guards = tuple(getattr(grounded, "effect_guards", ())) + active = tuple( + guard for guard in guards if request.segment_name in guard.active_segments + ) + if not active: + return None + if len(active) != 1: + raise RuntimeError( + "At most one held-object guard may own an action segment; " + f"segment={request.segment_name!r}, guards=" + f"{[guard.guard_id for guard in active]}." + ) + guard = active[0] + session = self._require_runner().session + if guard.baseline is HeldObjectGuardBaseline.VERIFIED_TASK_STATE: + candidate = session.task_state.get_held_object(guard.task_state_key) + else: + candidate = session.active_plan.expected_effects.held_object_updates.get( + guard.task_state_key + ) + covered = torch.zeros_like(request.env_mask) + if isinstance(candidate, HeldObjectState): + covered = ( + torch.ones_like(request.env_mask) + if candidate.env_mask is None + else candidate.env_mask.to(request.env_mask.device) + ) + if candidate.semantics.entity_id != self._guard_object_id( + guard.effect_spec + ): + covered.zero_() + observed_mask = request.env_mask & covered + failure_mask = request.env_mask & ~covered + if observed_mask.any(): + assert isinstance(candidate, HeldObjectState) + verification_id = self._next_guard_verification_id + self._next_guard_verification_id += 1 + monitor_request = EffectVerificationRequest( + verification_id=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=context.robot.timestamp, + deadline=request.deadline, + env_mask=observed_mask, + expected_effects=StateDelta( + held_object_updates={guard.task_state_key: candidate} + ), + ) + decision = self._observe_effect_monitor( + context, + monitor_request, + spec=guard.effect_spec, + monitor=guard.effect_monitor, + boundary_kind="in_flight_guard", + guard_id=guard.guard_id, + segment_name=request.segment_name, + ) + failure_mask |= decision.failure_mask + invalidation = self._held_object_invalidation( + guard.invalidation_task_state_keys, + failure_mask, + session.task_state, + ) + retry_mask = ( + failure_mask.clone() + if guard.retry_action + else torch.zeros_like(failure_mask) + ) + return HeldObjectGuardResult( + verification_id=request.verification_id, + object_id=self._guard_object_id(guard.effect_spec), + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + failure_mask=failure_mask, + state_invalidation=invalidation, + retry_mask=retry_mask, + message=( + f"Held-object invariant {guard.guard_id!r} failed during " + f"segment {request.segment_name!r}." + if failure_mask.any() + else "" + ), + ) + + @staticmethod + def _guard_object_id(spec: SemanticEffectSpec) -> str: + """Return the canonical object ID from a single guard expectation.""" + expectation = spec.state_expectations[0] + object_id = getattr(expectation, "object_id", None) + if type(object_id) is not str or not object_id: + raise TypeError("Held-object guard expectation must own an object_id.") + return object_id + + @staticmethod + def _held_object_invalidation( + task_state_keys: tuple[str, ...], + failure_mask: torch.Tensor, + task_state: TaskState, + ) -> StateDelta: + """Build conservative removal-only reconciliation for failed rows.""" + if not failure_mask.any(): + return StateDelta() + related = set(task_state_keys) + return StateDelta( + held_object_updates={key: None for key in task_state_keys}, + coordinated_held_object_updates={ + resources: None + for resources in task_state.coordinated_held_objects + if not set(resources).isdisjoint(related) + }, + ) + + def _observe_effect_monitor( + self, + context: PlanningContext, + request: EffectVerificationRequest, + *, + spec: SemanticEffectSpec, + 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.""" + grounded = self._require_grounded() observation_revision = self._observation_revision self._observation_revision += 1 selected_env_ids = spec.env_ids[request.env_mask.to(spec.env_ids.device)] @@ -2097,7 +2934,16 @@ def _effect_verifier( observation_revision=observation_revision, env_ids=selected_env_ids, ) - decision = monitor.observe(request, evidence) + observed = monitor.observe(request, evidence) + expectation_decisions = self._validated_expectation_decisions( + spec, + observed, + ) + decision = EffectMonitorDecision( + success_mask=observed.success_mask, + failure_mask=observed.failure_mask, + expectation_decisions=expectation_decisions, + ) analyzed = getattr(grounded, "analyzed", None) monitor_ref = getattr(analyzed, "effect_monitor_ref", None) if monitor_ref is not None and not isinstance(monitor_ref, EffectMonitorRef): @@ -2120,19 +2966,20 @@ def _effect_verifier( timestamp=context.robot.timestamp, success_mask=decision.success_mask, failure_mask=decision.failure_mask, + expectation_decisions=decision.expectation_decisions, effect_spec=spec, monitor_id=monitor_id, monitor_revision=monitor_revision, configured_monitor_params=configured_monitor_params, resolved_monitor_params=resolved_monitor_params, evidence=evidence, + boundary_kind=boundary_kind, + guard_id=guard_id, + gate_id=gate_id, + segment_name=segment_name, ) self._effect_traces.append(trace) - return EffectVerificationResult( - verification_id=request.verification_id, - success_mask=decision.success_mask, - failure_mask=decision.failure_mask, - ) + return decision def _consume_runner_step(self, runner_step: RunnerStep) -> None: """Merge one runner update into workflow-level traces.""" @@ -2145,11 +2992,14 @@ def _consume_runner_step(self, runner_step: RunnerStep) -> None: if runner_step.message: self._message = runner_step.message - def _finish_current_call(self, runner_step: RunnerStep) -> None: - """Commit terminal row masks and append exactly one call trace.""" + def _finish_active_call(self, runner_step: RunnerStep) -> _FinishedCallAttempt: + """Project one terminal session without deciding workflow eligibility.""" runner = self._require_runner() grounded = self._require_grounded() call_index = self._require_call_index() + call = self._active_call + if not isinstance(call, SemanticCallSpec): + raise RuntimeError("No semantic call is associated with the active runner.") self._task_state = _snapshot_task_state(runner.session.task_state) after = runner.session.eligible_mask invocation = getattr(grounded, "invocation") @@ -2162,20 +3012,6 @@ def _finish_current_call(self, runner_step: RunnerStep) -> None: else: completed = torch.zeros_like(self._call_entered_mask) failed = self._call_entered_mask & ~self._cancelled - after = self._eligible & ~failed - - self._eligible = after.clone() - self._failed |= failed - if failed.any(): - message = runner_step.message or "Semantic call failed for these rows." - self._failures.append( - SkillFailure( - call_index=call_index, - semantic_id=self._calls[call_index].semantic_id, - env_mask=failed, - message=message, - ) - ) plan_attempts = tuple( SkillPlanAttemptTrace.from_execution_attempt( attempt, @@ -2185,27 +3021,419 @@ def _finish_current_call(self, runner_step: RunnerStep) -> None: ) for attempt in runner.session.plan_attempts ) - self._call_traces.append( - SkillCallTrace( - call_index=call_index, - semantic_id=self._calls[call_index].semantic_id, - call_metadata=self._calls[call_index].to_metadata(), - skill_id=invocation.skill_id, - invocation_id=invocation.invocation_id, - invocation_revision=invocation.revision, - status=runner_step.status, - entered_mask=self._call_entered_mask, - completed_mask=completed, - failed_mask=failed, - command_count=runner_step.command_count, - resolved_core_policy=plan_attempts[-1].resolved_core_policy, - plan_attempts=plan_attempts, - events=tuple(self._events[self._call_event_offset :]), - effects=tuple(self._effect_traces[self._call_effect_offset :]), - ) + trace = SkillCallTrace( + call_index=call_index, + semantic_id=call.semantic_id, + call_metadata=call.to_metadata(), + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + status=runner_step.status, + entered_mask=self._call_entered_mask, + completed_mask=completed, + failed_mask=failed, + command_count=runner_step.command_count, + resolved_core_policy=plan_attempts[-1].resolved_core_policy, + plan_attempts=plan_attempts, + events=tuple(self._events[self._call_event_offset :]), + effects=tuple(self._effect_traces[self._call_effect_offset :]), ) self._runner = None self._grounded = None + self._active_call = None + self._active_recovery_item = None + return _FinishedCallAttempt( + trace=trace, + completed_mask=completed, + failed_mask=failed, + status=runner_step.status, + message=runner_step.message, + ) + + def _workflow_recovery_trigger(self) -> _WorkflowRecoveryTrigger | None: + """Resolve preset policy and the failed call's physical source endpoint.""" + call = self._active_call + if type(call) is Place: + source_slot = "primary" + elif type(call) is HandOver: + source_slot = "source" + else: + return None + grounded = self._require_grounded() + preset = grounded.analyzed.bound.preset + policy = preset.workflow_recovery_policy + if type(policy) is not WorkflowRecoveryPolicy: + raise TypeError("Grounded preset workflow_recovery_policy must be exact.") + if policy.max_recovery_attempts == 0: + return None + endpoints = tuple( + endpoint + for endpoint in grounded.invocation.binding.endpoints + if endpoint.slot_id == source_slot + ) + resource_ids = {endpoint.resource_id for endpoint in endpoints} + task_state_keys = {endpoint.task_state_key for endpoint in endpoints} + if not endpoints or len(resource_ids) != 1 or len(task_state_keys) != 1: + raise RuntimeError( + f"Workflow recovery requires one physical source resource and " + f"task-state key for slot {source_slot!r}." + ) + return _WorkflowRecoveryTrigger( + policy=policy, + source_resource_id=next(iter(resource_ids)), + source_task_state_key=next(iter(task_state_keys)), + ) + + def _active_call_requires_workflow_recovery(self) -> bool: + """Whether the active call emitted a row-local external-recovery hand-off.""" + entered = self._call_entered_mask + return any( + event.kind is ExecutionEventKind.RECOVERY_REQUIRED + and bool((event.env_mask.to(entered.device) & entered).any().item()) + for event in self._events[self._call_event_offset :] + ) + + @staticmethod + def _recovery_required_mask(trace: SkillCallTrace) -> torch.Tensor: + """Return failed rows explicitly handed to workflow recovery by core.""" + required = torch.zeros_like(trace.failed_mask) + for event in trace.events: + if event.kind is ExecutionEventKind.RECOVERY_REQUIRED: + required |= event.env_mask.to(required.device) + return required & trace.failed_mask + + def _handle_original_call_finished( + self, + finished: _FinishedCallAttempt, + *, + trigger: _WorkflowRecoveryTrigger | None, + ) -> None: + """Either advance one call barrier or start bounded row-local recovery.""" + if finished.status is RunnerStatus.CANCELLED: + cancelled = finished.trace.entered_mask & self._eligible + self._cancelled |= cancelled + self._eligible &= ~cancelled + self._finish_workflow_terminal() + return + recovery_required = self._recovery_required_mask(finished.trace) + recoverable = ( + torch.zeros_like(recovery_required) + if trigger is None + else recovery_required + ) + if not recoverable.any(): + self._complete_original_call_barrier( + success_mask=finished.completed_mask, + failure_mask=finished.failed_mask, + message=finished.message, + ) + return + assert trigger is not None + permanent_failure = finished.failed_mask & ~recoverable + call_index = self._require_call_index() + barrier = _WorkflowRecoveryBarrier( + trigger_call_index=call_index, + trigger_call=self._calls[call_index], + policy=trigger.policy, + source_resource_id=trigger.source_resource_id, + source_task_state_key=trigger.source_task_state_key, + entered_mask=finished.trace.entered_mask.clone(), + success_mask=finished.completed_mask.clone(), + final_failure_mask=permanent_failure.clone(), + attempt_counts=torch.zeros_like( + finished.trace.entered_mask, + dtype=torch.long, + ), + work_items=deque(), + failure_messages=( + [finished.message or "Semantic call failed for some rows."] + if permanent_failure.any() + else [] + ), + ) + self._recovery_barrier = barrier + self._eligible = (barrier.success_mask | recoverable) & ~self._cancelled + self._failed |= permanent_failure + self._schedule_recovery_cycle(recoverable) + self._start_next_recovery_work_item_or_finish() + + def _handle_recovery_call_finished( + self, + item: _WorkflowRecoveryWorkItem, + finished: _FinishedCallAttempt, + ) -> None: + """Update one recovery cohort and retain the shared call barrier.""" + barrier = self._require_recovery_barrier() + self._append_workflow_recovery_trace( + item, + call=finished.trace, + completed_mask=finished.completed_mask, + failed_mask=finished.failed_mask, + message=finished.message, + ) + if finished.status is RunnerStatus.CANCELLED: + cancelled = item.env_mask & self._eligible + self._cancelled |= cancelled + self._eligible &= ~cancelled + elif item.role is SkillWorkflowRecoveryRole.REACQUIRE: + if finished.completed_mask.any(): + barrier.work_items.append( + _WorkflowRecoveryWorkItem( + role=SkillWorkflowRecoveryRole.RETRY_REACQUIRED, + call=barrier.trigger_call, + env_mask=finished.completed_mask, + attempt_index=item.attempt_index, + ) + ) + if finished.failed_mask.any(): + self._schedule_recovery_cycle(finished.failed_mask) + else: + barrier.success_mask |= finished.completed_mask + if finished.failed_mask.any(): + recovery_required = self._recovery_required_mask(finished.trace) + permanent = finished.failed_mask & ~recovery_required + self._record_permanent_recovery_failure( + permanent, + finished.message + or "The retried semantic call failed without a recovery hand-off.", + ) + self._schedule_recovery_cycle(recovery_required) + self._start_next_recovery_work_item_or_finish() + + def _schedule_recovery_cycle(self, requested_mask: torch.Tensor) -> None: + """Consume one per-row budget and enqueue retained/reacquire cohorts.""" + barrier = self._require_recovery_barrier() + requested = requested_mask & self._eligible & ~self._cancelled + allowed = requested & ( + barrier.attempt_counts < barrier.policy.max_recovery_attempts + ) + exhausted = requested & ~allowed + self._record_permanent_recovery_failure( + exhausted, + "Workflow recovery exhausted its per-row attempt budget.", + ) + if not allowed.any(): + return + barrier.attempt_counts[allowed] += 1 + for attempt_index in range(1, barrier.policy.max_recovery_attempts + 1): + cohort = allowed & (barrier.attempt_counts == attempt_index) + if not cohort.any(): + continue + retained = self._retained_source_mask(cohort) + reacquire = cohort & ~retained + if retained.any(): + barrier.work_items.append( + _WorkflowRecoveryWorkItem( + role=SkillWorkflowRecoveryRole.RETRY_RETAINED, + call=barrier.trigger_call, + env_mask=retained, + attempt_index=attempt_index, + ) + ) + if reacquire.any(): + barrier.work_items.append( + _WorkflowRecoveryWorkItem( + role=SkillWorkflowRecoveryRole.REACQUIRE, + call=self._reacquisition_call(barrier), + env_mask=reacquire, + attempt_index=attempt_index, + ) + ) + + def _retained_source_mask(self, env_mask: torch.Tensor) -> torch.Tensor: + """Return rows whose reconciled symbolic state proves source retention.""" + barrier = self._require_recovery_barrier() + held = self._task_state.get_held_object(barrier.source_task_state_key) + if not isinstance(held, HeldObjectState): + return torch.zeros_like(env_mask) + trigger_object = getattr(barrier.trigger_call, "object", None) + object_id = getattr(trigger_object, "entity_id", None) + if held.semantics.entity_id != object_id: + return torch.zeros_like(env_mask) + active = ( + torch.ones_like(env_mask) + if held.env_mask is None + else held.env_mask.to(env_mask.device) + ) + return env_mask & active + + def _reacquisition_call(self, barrier: _WorkflowRecoveryBarrier) -> Pick: + """Derive a real Pick using the failed call's resolved source resource.""" + trigger_object = getattr(barrier.trigger_call, "object", None) + if type(trigger_object) is not SceneObjectRef: + raise TypeError("Curated workflow recovery requires a SceneObjectRef.") + grasp: SceneAffordanceRef | None = None + for candidate in reversed(self._calls[: barrier.trigger_call_index]): + if ( + type(candidate) is Pick + and candidate.object.entity_id == trigger_object.entity_id + ): + grasp = candidate.grasp + break + return Pick( + object=SceneObjectRef(trigger_object.entity_id), + grasp=(None if grasp is None else SceneAffordanceRef(grasp.entity_id)), + resources={"primary": barrier.source_resource_id}, + ) + + def _start_next_recovery_work_item_or_finish(self) -> None: + """Start the next non-empty cohort or close the recovered call barrier.""" + barrier = self._require_recovery_barrier() + while barrier.work_items: + queued = barrier.work_items.popleft() + active = queued.env_mask & self._eligible & ~self._cancelled + if not active.any(): + continue + item = _WorkflowRecoveryWorkItem( + role=queued.role, + call=queued.call, + env_mask=active, + attempt_index=queued.attempt_index, + ) + try: + self._prepare_recovery_work_item(item) + except Exception as exc: # noqa: BLE001 - row-local recovery failure + message = ( + f"Could not prepare workflow recovery call " + f"{item.call.semantic_id!r}: {type(exc).__name__}: {exc}" + ) + self._append_workflow_recovery_trace( + item, + call=None, + completed_mask=torch.zeros_like(item.env_mask), + failed_mask=item.env_mask, + message=message, + ) + self._record_permanent_recovery_failure(item.env_mask, message) + self._runner = None + self._grounded = None + self._active_call = None + self._active_recovery_item = None + continue + return + self._finish_recovery_barrier() + + def _append_workflow_recovery_trace( + self, + item: _WorkflowRecoveryWorkItem, + *, + call: SkillCallTrace | None, + completed_mask: torch.Tensor, + failed_mask: torch.Tensor, + message: str | None, + ) -> None: + """Append one immutable recovery-call trace with stable correlation.""" + barrier = self._require_recovery_barrier() + self._workflow_recovery_traces.append( + SkillWorkflowRecoveryTrace( + recovery_id=self._next_workflow_recovery_id, + trigger_call_index=barrier.trigger_call_index, + trigger_semantic_id=barrier.trigger_call.semantic_id, + attempt_index=item.attempt_index, + max_recovery_attempts=barrier.policy.max_recovery_attempts, + role=item.role, + source_resource_id=barrier.source_resource_id, + source_task_state_key=barrier.source_task_state_key, + entered_mask=item.env_mask, + completed_mask=completed_mask, + failed_mask=failed_mask, + call=call, + message=message, + ) + ) + self._next_workflow_recovery_id += 1 + + def _record_permanent_recovery_failure( + self, + env_mask: torch.Tensor, + message: str, + ) -> None: + """Remove exhausted rows while leaving other recovery cohorts active.""" + if not env_mask.any(): + return + barrier = self._require_recovery_barrier() + barrier.final_failure_mask |= env_mask + barrier.failure_messages.append(message) + self._failed |= env_mask + self._eligible &= ~env_mask + + def _finish_recovery_barrier(self) -> None: + """Rejoin recovered rows and advance the original program counter once.""" + barrier = self._require_recovery_barrier() + unresolved = ( + barrier.entered_mask + & ~barrier.success_mask + & ~barrier.final_failure_mask + & ~self._cancelled + ) + if unresolved.any(): + self._record_permanent_recovery_failure( + unresolved, + "Workflow recovery ended with unresolved rows.", + ) + success = barrier.success_mask & ~self._cancelled + failure = barrier.final_failure_mask & ~self._cancelled + message = ( + None + if not failure.any() + else "; ".join(dict.fromkeys(barrier.failure_messages)) + ) + self._recovery_barrier = None + self._complete_original_call_barrier( + success_mask=success, + failure_mask=failure, + message=message, + ) + + def _complete_original_call_barrier( + self, + *, + success_mask: torch.Tensor, + failure_mask: torch.Tensor, + message: str | None, + ) -> None: + """Commit final row outcomes and advance exactly one original call.""" + call_index = self._require_call_index() + call = self._calls[call_index] + failure = failure_mask & ~self._cancelled + self._failed |= failure + self._eligible = success_mask & ~self._failed & ~self._cancelled + if failure.any(): + failure_message = message or "Semantic call failed for these rows." + self._failures.append( + SkillFailure( + call_index=call_index, + semantic_id=call.semantic_id, + env_mask=failure, + message=failure_message, + ) + ) + self._message = failure_message + elif self._workflow_recovery_traces: + self._message = None + if self._eligible.any() and self._has_next_call: + next_index = call_index + 1 + try: + self._prepare_call(next_index) + except Exception as exc: # noqa: BLE001 - preserve workflow trace + self._fail_preparation(next_index, exc) + elif self._eligible.any(): + self._success = self._eligible.clone() + self._status = SkillStatus.COMPLETED + self._current_call_index = None + self._wait_duration = 0.0 + else: + self._finish_workflow_terminal() + + def _finish_workflow_terminal(self) -> None: + """Choose one terminal status from final row-local outcomes.""" + self._status = ( + SkillStatus.CANCELLED + if self._cancelled.any() and not self._failed.any() + else SkillStatus.FAILED + ) + self._current_call_index = None + self._wait_duration = 0.0 def _fail_preparation(self, call_index: int, exc: Exception) -> None: """Convert a post-barrier grounding failure to a terminal result.""" @@ -2224,6 +3452,9 @@ def _fail_preparation(self, call_index: int, exc: Exception) -> None: self._current_call_index = None self._runner = None self._grounded = None + self._active_call = None + self._active_recovery_item = None + self._recovery_barrier = None self._wait_duration = 0.0 def _append_preparation_failure_trace( @@ -2297,6 +3528,7 @@ def _append_preparation_failure_trace( def _abort(self, reason: str) -> None: """Safe-stop the active runner and mark remaining rows failed.""" if self._runner is not None: + recovery_item = self._active_recovery_item safe_stop_step = self._runner.cancel(reason) runner_step = replace( safe_stop_step, @@ -2304,7 +3536,17 @@ def _abort(self, reason: str) -> None: message=reason, ) self._consume_runner_step(runner_step) - self._finish_current_call(runner_step) + finished = self._finish_active_call(runner_step) + if recovery_item is None: + self._call_traces.append(finished.trace) + elif self._recovery_barrier is not None: + self._append_workflow_recovery_trace( + recovery_item, + call=finished.trace, + completed_mask=finished.completed_mask, + failed_mask=finished.failed_mask, + message=reason, + ) failed = self._eligible.clone() self._failed |= failed self._eligible &= ~failed @@ -2323,6 +3565,7 @@ def _abort(self, reason: str) -> None: ) self._message = reason self._status = SkillStatus.FAILED + self._recovery_barrier = None self._current_call_index = None self._wait_duration = 0.0 @@ -2336,6 +3579,11 @@ def _require_grounded(self) -> object: raise RuntimeError("No grounded semantic call is active.") return self._grounded + def _require_recovery_barrier(self) -> _WorkflowRecoveryBarrier: + if self._recovery_barrier is None: + raise RuntimeError("No workflow-recovery barrier is active.") + return self._recovery_barrier + def _require_call_index(self) -> int: if self._current_call_index is None: raise RuntimeError("No semantic call is active.") @@ -2513,5 +3761,7 @@ def cancel( "SkillRuntimeProvider", "SkillScene", "SkillStatus", + "SkillWorkflowRecoveryRole", + "SkillWorkflowRecoveryTrace", "task_state_to_metadata", ] diff --git a/embodichain/lab/sim/solvers/base_solver.py b/embodichain/lab/sim/solvers/base_solver.py index 47d9e8dcb..a9bb9e1a5 100644 --- a/embodichain/lab/sim/solvers/base_solver.py +++ b/embodichain/lab/sim/solvers/base_solver.py @@ -13,11 +13,14 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- +from __future__ import annotations + +from abc import ABCMeta, abstractmethod +from dataclasses import fields +from typing import TYPE_CHECKING, Any, Dict, List, Tuple, Union -import torch import numpy as np -from typing import List, Dict, Any, Union, TYPE_CHECKING, Tuple -from abc import abstractmethod, ABCMeta +import torch from embodichain.utils import configclass, logger @@ -98,22 +101,39 @@ def _get_tcp_as_numpy(self) -> np.ndarray: @classmethod def from_dict(cls, init_dict: Dict[str, Any]) -> "SolverCfg": - """Initialize the configuration from a dictionary.""" + """Initialize the concrete solver configuration from a dictionary. + + The concrete config receives all recognized dataclass init fields in its + constructor so initialization and ``__post_init__`` observe the final + inputs exactly once. Legacy unannotated config attributes are applied + afterward. Unknown fields preserve the historical behavior: they are + ignored with a warning. + """ from embodichain.utils.utility import get_class_instance if "class_type" not in init_dict: logger.log_error("class type must be specified in the configuration.") - cfg = get_class_instance( + cfg_type = get_class_instance( "embodichain.lab.sim.solvers", init_dict["class_type"] + "Cfg" - )() + ) + concrete_fields = {field.name: field for field in fields(cfg_type)} + kwargs: Dict[str, Any] = {} + deferred: Dict[str, Any] = {} for key, value in init_dict.items(): - if hasattr(cfg, key): - setattr(cfg, key, value) + field = concrete_fields.get(key) + if field is not None and field.init: + kwargs[key] = value + elif field is not None or hasattr(cfg_type, key): + # A few legacy solver configs expose configurable class + # attributes without dataclass annotations. They cannot be + # constructor arguments, but remain valid serialized fields. + deferred[key] = value else: - logger.log_warning( - f"Key '{key}' not found in {cfg.__class__.__name__}." - ) + logger.log_warning(f"Key '{key}' not found in {cfg_type.__name__}.") + cfg = cfg_type(**kwargs) + for key, value in deferred.items(): + setattr(cfg, key, value) return cfg diff --git a/embodichain_tasks/configs/expert_program/tableware/hand_over.yaml b/embodichain_tasks/configs/expert_program/tableware/hand_over.yaml new file mode 100644 index 000000000..e59f18be3 --- /dev/null +++ b/embodichain_tasks/configs/expert_program/tableware/hand_over.yaml @@ -0,0 +1,41 @@ +schema_version: 1 +program_id: dual_ur5_hand_over + +integration: + robot_profile: dual_ur5_handover_v1 + scene_registry: dual_ur5_handover_v1 + runtime_preset: safe + +targets: + delivery_pose: + kind: cyclic_pose + values: + - position: [0.0, -0.2, 0.7] + quaternion_wxyz: [0.7071067812, 0.7071067812, 0.0, 0.0] + +program: + kind: segment + name: hand_over_can + steps: + kind: sequence + items: + - kind: invoke + call: + kind: pick + object: can + - kind: invoke + call: + kind: hand_over + object: can + final_target: + kind: target_ref + target: delivery_pose + post: + - kind: wait_stable + entity: can + preset: rigid_object + validators: + - kind: object_near_target + object: can + target: delivery_pose + position_tolerance: 0.12 diff --git a/embodichain_tasks/configs/gym/hand_over/dual_ur5.json b/embodichain_tasks/configs/gym/hand_over/dual_ur5.json new file mode 100644 index 000000000..7b5a45551 --- /dev/null +++ b/embodichain_tasks/configs/gym/hand_over/dual_ur5.json @@ -0,0 +1,213 @@ +{ + "id": "HandOver-v1", + "expert_program_path": "../../expert_program/tableware/hand_over.yaml", + "max_episodes": 1, + "max_episode_steps": 1200, + "num_envs": 1, + "arena_space": 3.0, + "physics_config": { + "enable_ccd": true + }, + "env": { + "sim_steps_per_control": 4, + "events": { + "settle_can_on_reset": { + "func": "wait_for_dynamic_objects_to_settle", + "mode": "reset", + "params": { + "entity_cfgs": [ + { + "uid": "handover_object" + } + ], + "min_steps": 10, + "max_steps": 120, + "check_interval_steps": 2, + "required_stable_checks": 3, + "timeout_behavior": "raise" + } + } + }, + "extensions": {} + }, + "robot": { + "uid": "DualUR5HandOver", + "urdf_cfg": { + "fname": "dual_ur5_hand_over", + "name_case": { + "joint": "lower", + "link": "lower" + }, + "components": [ + { + "component_type": "left_arm", + "urdf_path": "UniversalRobots/UR5/UR5.urdf", + "transform": [ + [0.0, -1.0, 0.0, -0.3], + [1.0, 0.0, 0.0, -1.45], + [0.0, 0.0, 1.0, 0.4], + [0.0, 0.0, 0.0, 1.0] + ] + }, + { + "component_type": "right_arm", + "urdf_path": "UniversalRobots/UR5/UR5.urdf", + "transform": [ + [0.0, -1.0, 0.0, 0.3], + [1.0, 0.0, 0.0, -1.45], + [0.0, 0.0, 1.0, 0.4], + [0.0, 0.0, 0.0, 1.0] + ] + }, + { + "component_type": "left_hand", + "urdf_path": "DH_PGI_140_80/DH_PGI_140_80.urdf" + }, + { + "component_type": "right_hand", + "urdf_path": "DH_PGI_140_80/DH_PGI_140_80.urdf" + } + ] + }, + "control_parts": { + "left_arm": ["left_joint[0-9]"], + "right_arm": ["right_joint[0-9]"], + "dual_arm": ["left_joint[0-9]", "right_joint[0-9]"], + "left_hand": ["left_gripper_finger1_joint_1"], + "right_hand": ["right_gripper_finger1_joint_1"] + }, + "drive_pros": { + "stiffness": { + "left_joint[0-9]": 10000.0, + "right_joint[0-9]": 10000.0, + "left_gripper_finger1_joint_1": 2000.0, + "right_gripper_finger1_joint_1": 2000.0, + "left_gripper_finger2_joint_1": 0.0, + "right_gripper_finger2_joint_1": 0.0 + }, + "damping": { + "left_joint[0-9]": 1000.0, + "right_joint[0-9]": 1000.0, + "left_gripper_finger1_joint_1": 50.0, + "right_gripper_finger1_joint_1": 50.0, + "left_gripper_finger2_joint_1": 0.0, + "right_gripper_finger2_joint_1": 0.0 + }, + "max_effort": { + "left_joint[0-9]": 100000.0, + "right_joint[0-9]": 100000.0, + "left_gripper_finger1_joint_1": 140.0, + "right_gripper_finger1_joint_1": 140.0, + "left_gripper_finger2_joint_1": 0.0, + "right_gripper_finger2_joint_1": 0.0 + }, + "drive_type": "force" + }, + "solver_cfg": { + "left_arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "root_link_name": "left_base_link", + "end_link_name": "left_ee_link", + "tcp": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.155], + [0.0, 0.0, 0.0, 1.0] + ], + "ik_nearest_weight": [1.0, 4.0, 1.0, 1.0, 1.0, 1.0] + }, + "right_arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "root_link_name": "right_base_link", + "end_link_name": "right_ee_link", + "tcp": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.155], + [0.0, 0.0, 0.0, 1.0] + ], + "ik_nearest_weight": [1.0, 4.0, 1.0, 1.0, 1.0, 1.0] + } + }, + "init_pos": [1.95, 0.0, 0.1], + "init_rot": [0.0, 0.0, -90.0], + "init_qpos": [ + 0.0, + 0.0, + -1.57, + -1.57, + 1.57, + 1.57, + -1.57, + -1.57, + -1.57, + -1.57, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + }, + "sensor": [], + "light": { + "direct": [ + { + "uid": "main_light", + "color": [0.6, 0.6, 0.6], + "intensity": 30.0, + "init_pos": [0.0, -0.4, 3.0] + } + ] + }, + "background": [ + { + "uid": "support_surface", + "shape": { + "shape_type": "Cube", + "size": [0.8, 1.2, 0.02] + }, + "attrs": { + "mass": 10.0, + "dynamic_friction": 0.9, + "static_friction": 0.95, + "restitution": 0.01 + }, + "body_type": "static", + "init_pos": [0.0, 0.0, 0.49], + "init_rot": [0.0, 0.0, 0.0] + } + ], + "rigid_object": [ + { + "uid": "handover_object", + "shape": { + "shape_type": "Mesh", + "fpath": "SodaCan/simple_cola_can.obj", + "compute_uv": false + }, + "attrs": { + "mass": 0.33, + "dynamic_friction": 0.97, + "static_friction": 0.99, + "angular_damping": 1.0, + "linear_damping": 0.5, + "contact_offset": 0.001, + "rest_offset": 0.0, + "restitution": 0.01, + "min_position_iters": 32, + "min_velocity_iters": 8, + "max_depenetration_velocity": 2.0 + }, + "max_convex_hull_num": 1, + "init_pos": [0.0, 0.02, 0.62], + "init_rot": [90.0, 0.0, 0.0], + "body_scale": [0.56, 0.56, 0.56] + } + ], + "rigid_object_group": [], + "articulation": [] +} diff --git a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py index eef86d858..1a5b7d4cf 100644 --- a/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py +++ b/embodichain_tasks/embodichain_tasks/multi_segments/cube_pick_place.py @@ -66,7 +66,10 @@ from embodichain.lab.sim.robots import URRobotCfg from embodichain.lab.sim.shapes import CubeCfg from embodichain.lab.sim.skills import SceneCollisionRole, SceneDynamics -from embodichain.lab.sim.skills.profiles import SkillPolicyPreset +from embodichain.lab.sim.skills.profiles import ( + SkillPolicyPreset, + WorkflowRecoveryPolicy, +) from embodichain.toolkits.graspkit.pg_grasp import ( AntipodalSamplerCfg, GraspGeneratorCfg, @@ -301,6 +304,9 @@ def create_cube_robot_profile_binding() -> SimulationRobotSkillProfileBinding: "place": PlaceOptions(), }, recovery_policy=RecoveryPolicy(), + workflow_recovery_policy=WorkflowRecoveryPolicy( + max_recovery_attempts=2, + ), tracking_policy=TrackingPolicy.joint_position( in_flight_max_abs_error=0.08, terminal_max_abs_error=0.08, diff --git a/embodichain_tasks/embodichain_tasks/tableware/__init__.py b/embodichain_tasks/embodichain_tasks/tableware/__init__.py index 4d3a1bb3c..826083302 100644 --- a/embodichain_tasks/embodichain_tasks/tableware/__init__.py +++ b/embodichain_tasks/embodichain_tasks/tableware/__init__.py @@ -18,6 +18,7 @@ from __future__ import annotations +from .hand_over import HandOverEnv from .open_drawer import OpenDrawerEnv -__all__ = ["OpenDrawerEnv"] +__all__ = ["HandOverEnv", "OpenDrawerEnv"] diff --git a/embodichain_tasks/embodichain_tasks/tableware/hand_over.py b/embodichain_tasks/embodichain_tasks/tableware/hand_over.py new file mode 100644 index 000000000..4f8d3a77a --- /dev/null +++ b/embodichain_tasks/embodichain_tasks/tableware/hand_over.py @@ -0,0 +1,509 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Declarative dual-UR5 can hand-over environment. + +The task owns only the physical scene, robot-resource profile, and configured +object-space hand-over poses. The packaged Expert Program selects ``pick`` and +``hand_over`` semantic calls; shared runtime components generate and execute +all arm and gripper motion. +""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +import torch + +from embodichain.data import get_data_path +from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.gym.envs.expert_program import ( + AntipodalGraspAffordanceBinding, + ConfiguredHandOverPoseProvider, + ControlPartCommandPreset, + ControlPartEndpointBinding, + ControlPartResourceBinding, + ExpertProgramCfg, + ExpertProgramEnvironmentAdapter, + ExpertProgramEnvironmentMixin, + SimulationExpertProgramRegistration, + SimulationRigidObjectBinding, + SimulationRobotSkillProfileBinding, + SimulationSceneBinding, + create_simulation_expert_program_adapter, + load_expert_program, +) +from embodichain.lab.gym.envs.managers import EventCfg, SceneEntityCfg +from embodichain.lab.gym.envs.managers.events import ( + wait_for_dynamic_objects_to_settle, +) +from embodichain.lab.gym.utils.registration import register_env +from embodichain.lab.sim.atomic_actions import ( + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + GRASP_CAPABILITY, + ExecutionRunnerCfg, + HandOverOptions, + MotionPolicy, + PickUpOptions, +) +from embodichain.lab.sim.cfg import ( + LightCfg, + RigidBodyAttributesCfg, + RigidObjectCfg, + RobotCfg, +) +from embodichain.lab.sim.shapes import CubeCfg, MeshCfg +from embodichain.lab.sim.skills import SceneCollisionRole, SceneDynamics +from embodichain.lab.sim.skills.profiles import ( + SkillPolicyPreset, + WorkflowRecoveryPolicy, +) +from embodichain.toolkits.graspkit.pg_grasp import ( + AntipodalSamplerCfg, + GraspGeneratorCfg, + GripperCollisionCfg, +) +from embodichain_tasks.configs import get_config_path + +__all__ = [ + "HandOverEnv", + "HAND_OVER_EXPERT_PROGRAM_REGISTRATION", + "HAND_OVER_POSE_PROVIDER", + "create_hand_over_robot_profile_binding", + "create_hand_over_scene_binding", +] + +CAN_UID = "can" +CAN_SIMULATION_UID = "handover_object" +SUPPORT_SURFACE_UID = "support_surface" +HAND_OVER_SCENE_REGISTRY_ID = "dual_ur5_handover_v1" +HAND_OVER_ROBOT_PROFILE_ID = "dual_ur5_handover_v1" +HAND_OVER_GRASP_AFFORDANCE_ID = "can_antipodal_grasp" +HAND_OVER_EXPERT_PROGRAM_PATH = Path("expert_program/tableware/hand_over.yaml") + +CAN_MESH_PATH = "SodaCan/simple_cola_can.obj" +ARM_URDF_PATH = "UniversalRobots/UR5/UR5.urdf" +GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" +GRIPPER_TCP_Z = 0.155 +GRIPPER_MAX_OPEN_WIDTH = 0.100 +GRIPPER_FINGER_LENGTH = 0.12 +GRIPPER_ROOT_Z_WIDTH = 0.096 +GRIPPER_Y_THICKNESS = 0.040 +GRIPPER_OPEN_QPOS = 0.0 +GRIPPER_GRASP_QPOS = 0.011 +GRIPPER_MASTER_DRIVE_STIFFNESS = 2e3 +GRIPPER_MASTER_DRIVE_DAMPING = 5e1 +GRIPPER_MASTER_DRIVE_MAX_EFFORT = 140.0 +HAND_OVER_SAMPLE_COUNT = 200 + +SUPPORT_SURFACE_Z = 0.50 +SUPPORT_SURFACE_SIZE = (0.8, 1.2, 0.02) +SUPPORT_SURFACE_CENTER = (0.0, 0.0, 0.49) +CAN_INITIAL_POSITION = (0.0, 0.02, 0.62) +CAN_INITIAL_ROTATION_DEG = (90.0, 0.0, 0.0) +CAN_SCALE = (0.56, 0.56, 0.56) +CAN_MASS = 0.33 + +_GRIPPER_TCP = ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, GRIPPER_TCP_Z), + (0.0, 0.0, 0.0, 1.0), +) +_UR_IK_NEAREST_WEIGHT = (1.0, 4.0, 1.0, 1.0, 1.0, 1.0) +_LEFT_ARM_HOME = (0.0, 0.0, -1.57, -1.57, 1.57, 1.57) +_RIGHT_ARM_HOME = (-1.57, -1.57, -1.57, -1.57, 0.0, 0.0) +_DUAL_UR5_INIT_QPOS = (*_LEFT_ARM_HOME, *_RIGHT_ARM_HOME, 0.0, 0.0, 0.0, 0.0) + + +HAND_OVER_POSE_PROVIDER = ConfiguredHandOverPoseProvider( + middle_position=(0.0, 0.0, 0.7), + middle_quaternion_wxyz=(0.7071067812, 0.7071067812, 0.0, 0.0), + final_position=(0.0, -0.2, 0.7), + final_quaternion_wxyz=(0.7071067812, 0.7071067812, 0.0, 0.0), +) + + +def _dual_ur5_robot_dict() -> dict[str, object]: + """Return the shared serialized dual-UR5 embodiment declaration.""" + return { + "uid": "DualUR5HandOver", + "urdf_cfg": { + "fname": "dual_ur5_hand_over", + "name_case": {"joint": "lower", "link": "lower"}, + "components": [ + { + "component_type": "left_arm", + "urdf_path": ARM_URDF_PATH, + "transform": [ + [0.0, -1.0, 0.0, -0.3], + [1.0, 0.0, 0.0, -1.45], + [0.0, 0.0, 1.0, 0.4], + [0.0, 0.0, 0.0, 1.0], + ], + }, + { + "component_type": "right_arm", + "urdf_path": ARM_URDF_PATH, + "transform": [ + [0.0, -1.0, 0.0, 0.3], + [1.0, 0.0, 0.0, -1.45], + [0.0, 0.0, 1.0, 0.4], + [0.0, 0.0, 0.0, 1.0], + ], + }, + { + "component_type": "left_hand", + "urdf_path": GRIPPER_URDF_PATH, + }, + { + "component_type": "right_hand", + "urdf_path": GRIPPER_URDF_PATH, + }, + ], + }, + "control_parts": { + "left_arm": ["left_joint[0-9]"], + "right_arm": ["right_joint[0-9]"], + "dual_arm": ["left_joint[0-9]", "right_joint[0-9]"], + "left_hand": ["left_gripper_finger1_joint_1"], + "right_hand": ["right_gripper_finger1_joint_1"], + }, + "drive_pros": { + "stiffness": { + "left_joint[0-9]": 1e4, + "right_joint[0-9]": 1e4, + "left_gripper_finger1_joint_1": GRIPPER_MASTER_DRIVE_STIFFNESS, + "right_gripper_finger1_joint_1": GRIPPER_MASTER_DRIVE_STIFFNESS, + "left_gripper_finger2_joint_1": 0.0, + "right_gripper_finger2_joint_1": 0.0, + }, + "damping": { + "left_joint[0-9]": 1e3, + "right_joint[0-9]": 1e3, + "left_gripper_finger1_joint_1": GRIPPER_MASTER_DRIVE_DAMPING, + "right_gripper_finger1_joint_1": GRIPPER_MASTER_DRIVE_DAMPING, + "left_gripper_finger2_joint_1": 0.0, + "right_gripper_finger2_joint_1": 0.0, + }, + "max_effort": { + "left_joint[0-9]": 1e5, + "right_joint[0-9]": 1e5, + "left_gripper_finger1_joint_1": GRIPPER_MASTER_DRIVE_MAX_EFFORT, + "right_gripper_finger1_joint_1": GRIPPER_MASTER_DRIVE_MAX_EFFORT, + "left_gripper_finger2_joint_1": 0.0, + "right_gripper_finger2_joint_1": 0.0, + }, + "drive_type": "force", + }, + "solver_cfg": { + "left_arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "root_link_name": "left_base_link", + "end_link_name": "left_ee_link", + "tcp": _GRIPPER_TCP, + "ik_nearest_weight": _UR_IK_NEAREST_WEIGHT, + }, + "right_arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "root_link_name": "right_base_link", + "end_link_name": "right_ee_link", + "tcp": _GRIPPER_TCP, + "ik_nearest_weight": _UR_IK_NEAREST_WEIGHT, + }, + }, + "init_pos": [1.95, 0.0, 0.1], + "init_rot": [0.0, 0.0, -90.0], + "init_qpos": _DUAL_UR5_INIT_QPOS, + } + + +def _create_default_robot_cfg() -> RobotCfg: + """Create the dual-UR5 and dual-PGI task embodiment.""" + return RobotCfg.from_dict(_dual_ur5_robot_dict()) + + +def _load_default_expert_program() -> ExpertProgramCfg: + """Decode and preflight the packaged semantic hand-over program.""" + program = load_expert_program( + get_config_path(HAND_OVER_EXPERT_PROGRAM_PATH), + validation_context=HAND_OVER_EXPERT_PROGRAM_REGISTRATION.catalog, + ) + HAND_OVER_EXPERT_PROGRAM_REGISTRATION.catalog.preflight(program) + return program + + +def _create_default_env_cfg() -> EmbodiedEnvCfg: + """Create a directly-instantiable physical and semantic task config.""" + cfg = EmbodiedEnvCfg() + cfg.max_episode_steps = 1200 + cfg.robot = _create_default_robot_cfg() + cfg.sensor = [] + cfg.light = EmbodiedEnvCfg.EnvLightCfg( + direct=[ + LightCfg( + uid="main_light", + color=(0.6, 0.6, 0.6), + intensity=30.0, + init_pos=(0.0, -0.4, 3.0), + ) + ] + ) + cfg.background = [ + RigidObjectCfg( + uid=SUPPORT_SURFACE_UID, + shape=CubeCfg(size=list(SUPPORT_SURFACE_SIZE)), + attrs=RigidBodyAttributesCfg( + mass=10.0, + dynamic_friction=0.9, + static_friction=0.95, + restitution=0.01, + ), + body_type="static", + init_pos=list(SUPPORT_SURFACE_CENTER), + init_rot=[0.0, 0.0, 0.0], + ) + ] + cfg.rigid_object = [ + RigidObjectCfg( + uid=CAN_SIMULATION_UID, + shape=MeshCfg(fpath=get_data_path(CAN_MESH_PATH), compute_uv=False), + attrs=RigidBodyAttributesCfg( + mass=CAN_MASS, + dynamic_friction=0.97, + static_friction=0.99, + angular_damping=1.0, + linear_damping=0.5, + contact_offset=0.001, + rest_offset=0.0, + restitution=0.01, + min_position_iters=32, + min_velocity_iters=8, + max_depenetration_velocity=2.0, + ), + max_convex_hull_num=1, + init_pos=list(CAN_INITIAL_POSITION), + init_rot=list(CAN_INITIAL_ROTATION_DEG), + body_scale=CAN_SCALE, + ) + ] + cfg.extensions = {} + cfg.events = { + "settle_can_on_reset": EventCfg( + func=wait_for_dynamic_objects_to_settle, + mode="reset", + params={ + "entity_cfgs": [SceneEntityCfg(uid=CAN_SIMULATION_UID)], + "min_steps": 10, + "max_steps": 120, + "check_interval_steps": 2, + "required_stable_checks": 3, + "timeout_behavior": "raise", + }, + ) + } + cfg.expert_program = _load_default_expert_program() + return cfg + + +def create_hand_over_scene_binding( + *, + grasp_samples: int = 10000, + force_reannotate: bool = False, +) -> SimulationSceneBinding: + """Declare the can, support slab, and antipodal grasp affordance.""" + if isinstance(grasp_samples, bool) or not isinstance(grasp_samples, int): + raise TypeError("grasp_samples must be an integer.") + if grasp_samples < 1: + raise ValueError("grasp_samples must be positive.") + if not isinstance(force_reannotate, bool): + raise TypeError("force_reannotate must be a bool.") + return SimulationSceneBinding( + registry_id=HAND_OVER_SCENE_REGISTRY_ID, + rigid_objects=( + SimulationRigidObjectBinding( + entity_id=CAN_UID, + simulation_uid=CAN_SIMULATION_UID, + dynamics=SceneDynamics.DYNAMIC, + collision_role=SceneCollisionRole.NONE, + semantic_type="soda_can", + default_grasp_affordance=HAND_OVER_GRASP_AFFORDANCE_ID, + ), + SimulationRigidObjectBinding( + entity_id=SUPPORT_SURFACE_UID, + simulation_uid=SUPPORT_SURFACE_UID, + dynamics=SceneDynamics.STATIC, + collision_role=SceneCollisionRole.NONE, + semantic_type="support_surface", + ), + ), + antipodal_grasps=( + AntipodalGraspAffordanceBinding( + entity_id=HAND_OVER_GRASP_AFFORDANCE_ID, + object_id=CAN_UID, + native_name="can_mesh_antipodal", + revision="can-antipodal-v1", + generator_cfg=GraspGeneratorCfg( + viser_port=11801, + antipodal_sampler_cfg=AntipodalSamplerCfg( + n_sample=grasp_samples, + max_length=GRIPPER_MAX_OPEN_WIDTH, + min_length=0.005, + ), + is_partial_annotate=False, + is_filter_ground_collision=False, + ), + gripper_collision_cfg=GripperCollisionCfg( + max_open_length=GRIPPER_MAX_OPEN_WIDTH, + finger_length=GRIPPER_FINGER_LENGTH, + y_thickness=GRIPPER_Y_THICKNESS, + root_z_width=GRIPPER_ROOT_Z_WIDTH, + open_check_margin=0.002, + point_sample_dense=0.012, + ), + force_reannotate=force_reannotate, + ), + ), + ) + + +def create_hand_over_robot_profile_binding() -> SimulationRobotSkillProfileBinding: + """Declare left/right arm-and-gripper semantic resources.""" + motion_capabilities = frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } + ) + return SimulationRobotSkillProfileBinding( + profile_id=HAND_OVER_ROBOT_PROFILE_ID, + resources=tuple( + ControlPartResourceBinding( + resource_id=side, + endpoints=( + ControlPartEndpointBinding( + endpoint_id="motion", + control_part=f"{side}_arm", + capabilities=motion_capabilities, + ), + ControlPartEndpointBinding( + endpoint_id="grasp", + control_part=f"{side}_hand", + capabilities=frozenset({GRASP_CAPABILITY}), + command_preset=f"{side}_parallel_gripper", + ), + ), + ) + for side in ("left", "right") + ), + command_presets=tuple( + ControlPartCommandPreset( + preset_id=f"{side}_parallel_gripper", + control_part=f"{side}_hand", + commands={ + "open": (GRIPPER_OPEN_QPOS,), + "grasp": (GRIPPER_GRASP_QPOS,), + }, + ) + for side in ("left", "right") + ), + defaults={ + "pick_up": {"primary": "left"}, + "hand_over": {"source": "left", "destination": "right"}, + }, + presets=( + SkillPolicyPreset( + "safe", + action_option_templates={ + "pick": PickUpOptions( + pick_object_part="top", + pre_grasp_distance=0.08, + lift_height=0.10, + hand_interp_steps=5, + approach_direction=torch.tensor( + [0.0, -0.7071067812, -0.7071067812], + dtype=torch.float32, + ), + ), + "hand_over": HandOverOptions( + receive_pick_object_part="bottom", + pre_grasp_distance=0.08, + lift_height=0.08, + hand_interp_steps=10, + hold_steps=4, + retreat_steps=28, + receive_approach_direction=torch.tensor( + [0.0, 0.7071067812, -0.7071067812], + dtype=torch.float32, + ), + ), + }, + motion_policy=MotionPolicy(sample_count=HAND_OVER_SAMPLE_COUNT), + workflow_recovery_policy=WorkflowRecoveryPolicy( + max_recovery_attempts=2, + ), + runner_cfg=ExecutionRunnerCfg( + hold_during_effect_verification=False, + hold_on_completion=False, + ), + ), + ), + default_preset="safe", + grounding_providers={ + "hand_over": ConfiguredHandOverPoseProvider.provider_id, + }, + ) + + +HAND_OVER_EXPERT_PROGRAM_REGISTRATION = SimulationExpertProgramRegistration( + scene_binding=create_hand_over_scene_binding(), + robot_profile_binding=create_hand_over_robot_profile_binding(), + handover_pose_providers=(HAND_OVER_POSE_PROVIDER,), +) + + +@register_env( + "HandOver-v1", + max_episode_steps=1200, + expert_program_registration=HAND_OVER_EXPERT_PROGRAM_REGISTRATION, +) +class HandOverEnv(ExpertProgramEnvironmentMixin, EmbodiedEnv): + """Transfer a can between two UR5 arms through a semantic program.""" + + def __init__( + self, + cfg: EmbodiedEnvCfg | None = None, + **kwargs: Any, + ) -> None: + """Initialize the configured scene without task-level motion code.""" + if cfg is None: + cfg = _create_default_env_cfg() + super().__init__(cfg, **kwargs) + self._expert_program_adapter = create_simulation_expert_program_adapter( + self, + registration=HAND_OVER_EXPERT_PROGRAM_REGISTRATION, + ) + + @property + def expert_program_adapter(self) -> ExpertProgramEnvironmentAdapter: + """Return the shared adapter assembled for this environment.""" + return self._expert_program_adapter diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index 4c76f5968..fb34bb829 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -432,6 +432,8 @@ def verify_pickup_effect( verification_id=request.verification_id, success_mask=verified_success, failure_mask=request.env_mask & ~success, + invalidation_mask=request.env_mask & ~success, + retry_mask=request.env_mask & ~success, ) recording_started = start_auto_play_recording( diff --git a/tests/gym/envs/expert_program/test_catalog.py b/tests/gym/envs/expert_program/test_catalog.py index 380e35c60..3886aa8a3 100644 --- a/tests/gym/envs/expert_program/test_catalog.py +++ b/tests/gym/envs/expert_program/test_catalog.py @@ -54,6 +54,7 @@ RegisteredSemanticCall, SemanticCallDescriptor, SkillPolicyPreset, + WorkflowRecoveryPolicy, builtin_semantic_call_catalog, ) from embodichain.lab.sim.atomic_actions.tracking import ( @@ -792,6 +793,26 @@ def test_fingerprint_is_stable_for_equivalent_declarations() -> None: assert len(left.fingerprint) == 64 +def test_fingerprint_covers_workflow_recovery_policy() -> None: + """A recovery budget is immutable registration-owned runtime behavior.""" + base = create_cube_robot_profile_binding().presets[0] + changed = SkillPolicyPreset( + base.preset_id, + schema_version=base.schema_version, + action_option_templates=base.action_option_templates, + motion_policy=base.motion_policy, + tracking_policy=base.tracking_policy, + recovery_policy=base.recovery_policy, + workflow_recovery_policy=WorkflowRecoveryPolicy( + max_recovery_attempts=1, + ), + runner_cfg=base.runner_cfg, + effect_monitors=base.effect_monitors, + ) + + assert _registration().fingerprint != _registration_with_preset(changed).fingerprint + + def test_fingerprint_is_independent_of_catalog_and_provider_insertion_order() -> None: """Semantically equivalent unordered registration inputs hash identically.""" descriptors = tuple(builtin_semantic_call_catalog().descriptors.values()) diff --git a/tests/gym/envs/expert_program/test_simulation_environment.py b/tests/gym/envs/expert_program/test_simulation_environment.py index 5dfcb35b0..0a2eea1ed 100644 --- a/tests/gym/envs/expert_program/test_simulation_environment.py +++ b/tests/gym/envs/expert_program/test_simulation_environment.py @@ -117,6 +117,7 @@ SemanticPose, SemanticRelationTarget, SkillPolicyPreset, + WorkflowRecoveryPolicy, ) from embodichain.lab.sim.skills.effects import ( CONSTRAINT_EFFECT_CHANNEL, @@ -148,6 +149,7 @@ _BATCH_SIZE = 3 _ROBOT_DOF = 2 _STEP_DT = 0.04 +_UNALIGNED_PROFILE_DT = 0.01 _TRACKER_ENV_IDS = torch.tensor((7, 3, 11), dtype=torch.long) _HAND_OPEN_POSITION = 0.0 _HAND_GRASP_POSITION = 0.8 @@ -1007,8 +1009,9 @@ class _MobileRobot: def __init__(self) -> None: self.qpos = torch.zeros(_BATCH_SIZE, _ROBOT_DOF) - def get_qpos(self) -> torch.Tensor: + def get_qpos(self, *, target: bool = False) -> torch.Tensor: """Return the full controller hold state.""" + del target return self.qpos def get_qvel(self) -> torch.Tensor: @@ -1117,7 +1120,7 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: "pick": PickUpOptions(), "place": PlaceOptions(), }, - motion_policy=MotionPolicy(control_dt=0.01), + motion_policy=MotionPolicy(control_dt=_UNALIGNED_PROFILE_DT), tracking_policy=TrackingPolicy.joint_position( in_flight_max_abs_error=0.037, terminal_max_abs_error=0.019, @@ -1125,7 +1128,7 @@ def _profile_binding() -> SimulationRobotSkillProfileBinding: runner_cfg=ExecutionRunnerCfg( command_timeout=0.37, safe_stop_timeout=0.61, - minimum_cycle_time=0.04, + minimum_cycle_time=_UNALIGNED_PROFILE_DT, hold_on_completion=False, ), ), @@ -1274,17 +1277,22 @@ def _motion_generator(robot: _Robot) -> MotionGenerator: return generator -def _factory() -> tuple[SimulationExpertProgramFactory, _Robot]: +def _factory( + robot_profile_binding: SimulationRobotSkillProfileBinding | None = None, +) -> tuple[SimulationExpertProgramFactory, _Robot]: """Create one production factory around CPU-only test doubles.""" robot = _Robot() simulation = _Simulation(robot) + selected_profile_binding = ( + _profile_binding() if robot_profile_binding is None else robot_profile_binding + ) return ( SimulationExpertProgramFactory( simulation, # type: ignore[arg-type] robot, # type: ignore[arg-type] SimulationExpertProgramRegistration( scene_binding=SimulationSceneBinding(registry_id="scene"), - robot_profile_binding=_profile_binding(), + robot_profile_binding=selected_profile_binding, ), step_dt=_STEP_DT, motion_generator_factory=lambda: _motion_generator(robot), @@ -1381,8 +1389,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, @@ -1398,14 +1406,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, @@ -1415,6 +1424,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}, ) @@ -1522,11 +1532,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: @@ -1780,6 +1796,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 @@ -1865,17 +1889,51 @@ def _assert_invocation_equivalent( ) -def test_simulation_factory_aligns_every_motion_policy_to_gym_step() -> None: - """Cadence alignment preserves the exact registered tracking contract.""" - factory, _ = _factory() +def test_simulation_factory_aligns_every_runtime_policy_to_gym_step() -> None: + """Cadence lowering preserves source declarations and unrelated policy.""" + binding = _profile_binding() + base_preset = binding.presets[0] + source_preset = SkillPolicyPreset( + base_preset.preset_id, + schema_version=base_preset.schema_version, + action_option_templates=base_preset.action_option_templates, + motion_policy=base_preset.motion_policy, + tracking_policy=base_preset.tracking_policy, + recovery_policy=base_preset.recovery_policy, + workflow_recovery_policy=WorkflowRecoveryPolicy( + max_recovery_attempts=2, + ), + runner_cfg=base_preset.runner_cfg, + effect_monitors=base_preset.effect_monitors, + ) + binding = replace(binding, presets=(source_preset,)) + source_runner_cfg = source_preset.runner_cfg + factory, _ = _factory(binding) profile = factory.create_robot_skill_profile() - assert profile.presets["safe"].motion_policy.control_dt == pytest.approx(_STEP_DT) - assert profile.presets["safe"].tracking_policy == TrackingPolicy.joint_position( + aligned_preset = profile.presets["safe"] + aligned_runner_cfg = aligned_preset.runner_cfg + assert aligned_preset.motion_policy.control_dt == pytest.approx(_STEP_DT) + assert aligned_runner_cfg.minimum_cycle_time == pytest.approx(_STEP_DT) + assert aligned_runner_cfg.command_timeout == source_runner_cfg.command_timeout + assert aligned_runner_cfg.safe_stop_timeout == source_runner_cfg.safe_stop_timeout + assert aligned_runner_cfg.hold_on_completion is source_runner_cfg.hold_on_completion + assert ( + aligned_runner_cfg.hold_during_effect_verification + is source_runner_cfg.hold_during_effect_verification + ) + assert aligned_preset.tracking_policy == TrackingPolicy.joint_position( in_flight_max_abs_error=0.037, terminal_max_abs_error=0.019, ) + assert aligned_preset.workflow_recovery_policy.max_recovery_attempts == 2 + assert binding.presets[0].motion_policy.control_dt == pytest.approx( + _UNALIGNED_PROFILE_DT + ) + assert binding.presets[0].runner_cfg.minimum_cycle_time == pytest.approx( + _UNALIGNED_PROFILE_DT + ) def test_mllm_config_and_atomic_skills_share_invocations_and_verified_results( @@ -2618,8 +2676,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/gym/envs/expert_program/test_simulation_handover.py b/tests/gym/envs/expert_program/test_simulation_handover.py new file mode 100644 index 000000000..4615b99d1 --- /dev/null +++ b/tests/gym/envs/expert_program/test_simulation_handover.py @@ -0,0 +1,85 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for configured semantic hand-over pose integration.""" + +from __future__ import annotations + +import pytest +import torch + +from embodichain.lab.gym.envs.expert_program import ConfiguredHandOverPoseProvider + + +def _provider() -> ConfiguredHandOverPoseProvider: + """Return one deterministic dual-arm transfer declaration.""" + return ConfiguredHandOverPoseProvider( + middle_position=(0.0, 0.0, 0.7), + middle_quaternion_wxyz=(1.0, 1.0, 0.0, 0.0), + final_position=(0.0, -0.2, 0.7), + final_quaternion_wxyz=(1.0, 1.0, 0.0, 0.0), + ) + + +def test_configured_handover_provider_normalizes_and_owns_targets() -> None: + """Configured poses are normalized and returned as independent values.""" + provider = _provider() + + first = provider.resolve(object(), context=object(), bound=object()) + second = provider.resolve(object(), context=object(), bound=object()) + + expected_rotation = torch.tensor( + [ + [1.0, 0.0, 0.0], + [0.0, 0.0, -1.0], + [0.0, 1.0, 0.0], + ] + ) + assert first.middle.pose is not second.middle.pose + assert first.final.pose is not second.final.pose + assert torch.allclose( + first.middle.pose.to_matrix()[:3, :3], expected_rotation, atol=1e-6 + ) + assert torch.allclose( + first.middle.pose.to_matrix()[:3, 3], torch.tensor([0.0, 0.0, 0.7]) + ) + assert torch.allclose( + first.final.pose.to_matrix()[:3, 3], torch.tensor([0.0, -0.2, 0.7]) + ) + + +@pytest.mark.parametrize( + ("overrides", "error_type"), + [ + ({"middle_position": (0.0, 0.0)}, TypeError), + ({"middle_quaternion_wxyz": (0.0, 0.0, 0.0, 0.0)}, ValueError), + ], +) +def test_configured_handover_provider_rejects_invalid_declarations( + overrides: dict[str, object], + error_type: type[Exception], +) -> None: + """Malformed provider declarations fail before simulation construction.""" + values: dict[str, object] = { + "middle_position": (0.0, 0.0, 0.7), + "middle_quaternion_wxyz": (1.0, 0.0, 0.0, 0.0), + "final_position": (0.0, -0.2, 0.7), + "final_quaternion_wxyz": (1.0, 0.0, 0.0, 0.0), + } + values.update(overrides) + + with pytest.raises(error_type): + ConfiguredHandOverPoseProvider(**values) # type: ignore[arg-type] diff --git a/tests/gym/envs/expert_program/test_simulation_policies.py b/tests/gym/envs/expert_program/test_simulation_policies.py index 51b3bea13..5ad6a2e1c 100644 --- a/tests/gym/envs/expert_program/test_simulation_policies.py +++ b/tests/gym/envs/expert_program/test_simulation_policies.py @@ -85,15 +85,22 @@ def get_local_pose(self, *, to_matrix: bool) -> torch.Tensor: class _Robot: - """Full-qpos source used by post-policy hold actions.""" + """Distinct current- and target-qpos source for post-policy holds.""" - def __init__(self, qpos: torch.Tensor) -> None: - self.qpos = qpos - self.qpos_reads = 0 + def __init__( + self, + current_qpos: torch.Tensor, + target_qpos: torch.Tensor | None = None, + ) -> None: + self.current_qpos = current_qpos.clone() + self.target_qpos = ( + current_qpos.clone() if target_qpos is None else target_qpos.clone() + ) + self.qpos_reads: list[bool] = [] - def get_qpos(self) -> torch.Tensor: - self.qpos_reads += 1 - return self.qpos.clone() + def get_qpos(self, target: bool = False) -> torch.Tensor: + self.qpos_reads.append(target) + return (self.target_qpos if target else self.current_qpos).clone() class _Simulation: @@ -177,10 +184,14 @@ def _port( positions: torch.Tensor, *, preset: DynamicSettleMonitorCfg | None = None, + target_qpos: torch.Tensor | None = None, ) -> tuple[SimulationSegmentPolicyPort, _RigidObject, _Robot]: """Build one policy port and expose its mutable test doubles.""" entity = _RigidObject(positions) - robot = _Robot(torch.tensor([[1.0, 2.0], [3.0, 4.0]])) + robot = _Robot( + torch.tensor([[1.0, 2.0], [3.0, 4.0]]), + target_qpos=target_qpos, + ) port = SimulationSegmentPolicyPort( _Simulation(entity), robot, @@ -226,7 +237,7 @@ def test_pure_preflight_validates_hooks_without_reading_live_state() -> None: port.validate_policy(segment.post_policies[0], segment=segment) port.validate_validator(segment.validators[0], segment=segment) - assert robot.qpos_reads == 1 + assert robot.qpos_reads == [False] assert entity.pose_reads == 0 @@ -238,14 +249,24 @@ def test_pure_preflight_rejects_unknown_settle_preset_without_observation() -> N with pytest.raises(KeyError, match="Unknown settle preset 'missing'"): port.validate_policy(segment.post_policies[0], segment=segment) - assert robot.qpos_reads == 1 + assert robot.qpos_reads == [False] assert entity.pose_reads == 0 -def test_wait_stable_yields_fresh_full_qpos_holds_through_gym() -> None: - """Settling observes only after each yielded hold has been consumed.""" +def test_wait_stable_yields_fresh_target_qpos_holds_through_gym() -> None: + """Settling preserves loaded drive targets with independently owned holds.""" segment = _compiled_segment() - port, _, robot = _port(torch.zeros(2, 3)) + target_qpos = torch.tensor([[5.0, 6.0], [7.0, 8.0]]) + port, _, robot = _port( + torch.zeros(2, 3), + target_qpos=target_qpos, + preset=DynamicSettleMonitorCfg( + min_steps=0, + max_steps=4, + check_interval_steps=1, + required_stable_checks=3, + ), + ) actions = port.actions( segment.post_policies[0], segment=segment, @@ -253,11 +274,21 @@ def test_wait_stable_yields_fresh_full_qpos_holds_through_gym() -> None: ) first = next(actions) - assert torch.equal(first, robot.qpos) + assert torch.equal(first, target_qpos) + assert not torch.equal(first, robot.current_qpos) first.fill_(99.0) + + second = next(actions) + assert torch.equal(second, target_qpos) + assert second.data_ptr() != first.data_ptr() with pytest.raises(StopIteration): next(actions) - assert torch.equal(robot.qpos, torch.tensor([[1.0, 2.0], [3.0, 4.0]])) + assert torch.equal( + robot.current_qpos, + torch.tensor([[1.0, 2.0], [3.0, 4.0]]), + ) + assert torch.equal(robot.target_qpos, target_qpos) + assert robot.qpos_reads == [False, True, True] metadata = port.post_policy_metadata( segment.post_policies[0], @@ -269,11 +300,11 @@ def test_wait_stable_yields_fresh_full_qpos_holds_through_gym() -> None: "linear_velocity": 0.03, "angular_velocity": 0.2, "min_steps": 0, - "max_steps": 3, + "max_steps": 4, "check_interval_steps": 1, - "required_stable_checks": 2, + "required_stable_checks": 3, } - assert metadata["state"]["elapsed_steps"] == 1 + assert metadata["state"]["elapsed_steps"] == 2 assert metadata["state"]["settled_mask"] == [True, True] assert metadata["state"]["timeout_mask"] == [False, False] assert metadata["state"]["max_linear_speed"] == [0.0, 0.0] @@ -283,6 +314,66 @@ def test_wait_stable_yields_fresh_full_qpos_holds_through_gym() -> None: ).tolist() == [True, True] +def test_wait_stable_holds_active_targets_and_inactive_current_qpos() -> None: + """Initial inactive rows use measured holds while active rows keep preload.""" + segment = _compiled_segment() + target_qpos = torch.tensor([[5.0, 6.0], [7.0, 8.0]]) + port, _, robot = _port( + torch.zeros(2, 3), + target_qpos=target_qpos, + preset=DynamicSettleMonitorCfg( + min_steps=0, + max_steps=4, + check_interval_steps=1, + required_stable_checks=3, + ), + ) + active_mask = torch.tensor([True, False]) + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=active_mask, + ) + expected = torch.stack((target_qpos[0], robot.current_qpos[1])) + + first = next(actions) + assert torch.equal(first, expected) + first.fill_(99.0) + + second = next(actions) + assert torch.equal(second, expected) + assert second.data_ptr() != first.data_ptr() + with pytest.raises(StopIteration): + next(actions) + + assert robot.qpos_reads == [False, True, False, True, False] + result = port.post_policy_result( + segment.post_policies[0], + segment=segment, + ) + assert result.tolist() == [True, False] + + +def test_wait_stable_rejects_wrong_target_width_for_all_active_rows() -> None: + """All-active settling fails closed on a malformed full target qpos.""" + segment = _compiled_segment() + port, _, robot = _port(torch.zeros(2, 3)) + robot.target_qpos = torch.zeros(2, 3) + actions = port.actions( + segment.post_policies[0], + segment=segment, + active_mask=torch.ones(2, dtype=torch.bool), + ) + + with pytest.raises( + ValueError, + match="target full qpos must match the construction-time current full qpos", + ): + next(actions) + + assert robot.qpos_reads == [False, True] + + def test_wait_stable_returns_row_local_timeout_result_and_metadata() -> None: """A moving row times out without failing a settled peer or the batch.""" segment = _compiled_segment() diff --git a/tests/gym/envs/tasks/test_hand_over.py b/tests/gym/envs/tasks/test_hand_over.py new file mode 100644 index 000000000..96a1e11bb --- /dev/null +++ b/tests/gym/envs/tasks/test_hand_over.py @@ -0,0 +1,542 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for the declarative dual-UR5 hand-over task.""" + +from __future__ import annotations + +import importlib +import json +from pathlib import Path +from types import SimpleNamespace + +import pytest +import torch + +from embodichain.lab.gym.envs import EmbodiedEnv +from embodichain.lab.gym.envs.demo import execute_demo_episode +from embodichain.lab.gym.envs.expert_program import ( + ConfiguredHandOverPoseProvider, + ExpertProgramEnvironmentMixin, +) +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.lab.gym.utils.registration import ( + REGISTERED_ENVS, + discover_task_packages, +) +from embodichain.lab.sim.atomic_actions import HandOverOptions, PickUpOptions +from embodichain.lab.sim.cfg import RobotCfg +from embodichain.lab.sim.skills import HandOver, Pick + +# Trigger official task auto-registration (idempotent). +discover_task_packages() + +from embodichain_tasks.tableware.hand_over import ( # noqa: E402 + CAN_SIMULATION_UID, + CAN_UID, + CAN_MASS, + GRIPPER_MASTER_DRIVE_DAMPING, + GRIPPER_MASTER_DRIVE_MAX_EFFORT, + GRIPPER_MASTER_DRIVE_STIFFNESS, + GRIPPER_GRASP_QPOS, + GRIPPER_OPEN_QPOS, + HAND_OVER_EXPERT_PROGRAM_REGISTRATION, + HAND_OVER_POSE_PROVIDER, + HAND_OVER_ROBOT_PROFILE_ID, + HAND_OVER_SCENE_REGISTRY_ID, + HAND_OVER_SAMPLE_COUNT, + SUPPORT_SURFACE_UID, + HandOverEnv, + _create_default_env_cfg, + create_hand_over_robot_profile_binding, + create_hand_over_scene_binding, +) + +EXPECTED_GRIPPER_GRASP_QPOS = 0.011 + + +def _gym_config_path() -> Path: + """Return the installed-source dual-UR5 Gym config path.""" + return ( + Path(__file__).parents[4] + / "embodichain_tasks/configs/gym/hand_over/dual_ur5.json" + ) + + +def _gym_payload() -> dict[str, object]: + """Load the runnable HandOver Gym config as inert JSON data.""" + payload = json.loads(_gym_config_path().read_text(encoding="utf-8")) + assert type(payload) is dict + return payload + + +def test_registered_hand_over_task_uses_shared_expert_program_mixin() -> None: + """The task registers one semantic environment without local demo code.""" + from embodichain_tasks.tableware import __all__ + + assert "HandOverEnv" in __all__ + spec = REGISTERED_ENVS["HandOver-v1"] + assert spec.cls is HandOverEnv + assert spec.max_episode_steps == 1200 + assert spec.expert_program_registration is HAND_OVER_EXPERT_PROGRAM_REGISTRATION + assert "expert_program_registration" not in spec.default_kwargs + assert issubclass(HandOverEnv, ExpertProgramEnvironmentMixin) + assert issubclass(HandOverEnv, EmbodiedEnv) + assert "create_demo_action_list" not in HandOverEnv.__dict__ + + +def test_hand_over_gym_config_selects_packaged_program_without_contact_sensor() -> None: + """Normal startup selects the semantic program and needs no contact sensor.""" + payload = _gym_payload() + + assert payload["id"] == "HandOver-v1" + assert payload["expert_program_path"] == ( + "../../expert_program/tableware/hand_over.yaml" + ) + assert payload["sensor"] == [] + assert payload["env"]["extensions"] == {} + settle = payload["env"]["events"]["settle_can_on_reset"] + assert settle["func"] == "wait_for_dynamic_objects_to_settle" + assert settle["params"]["entity_cfgs"] == [{"uid": CAN_SIMULATION_UID}] + + +def test_hand_over_gym_config_builds_dual_ur5_pgi_scene() -> None: + """Config parsing preserves the tutorial robot, can, and support geometry.""" + path = _gym_config_path() + cfg = config_to_cfg(_gym_payload(), source_path=path) + + assert type(cfg.robot) is RobotCfg + assert cfg.robot.uid == "DualUR5HandOver" + assert cfg.robot.control_parts["left_arm"] == ["left_joint[0-9]"] + assert cfg.robot.control_parts["right_arm"] == ["right_joint[0-9]"] + assert cfg.robot.control_parts["left_hand"] == ["left_gripper_finger1_joint_1"] + assert cfg.robot.control_parts["right_hand"] == ["right_gripper_finger1_joint_1"] + assert set(cfg.robot.urdf_cfg.components) == { + "left_arm", + "right_arm", + "left_hand", + "right_hand", + } + assert cfg.robot.solver_cfg["left_arm"].ik_nearest_weight == [ + 1.0, + 4.0, + 1.0, + 1.0, + 1.0, + 1.0, + ] + assert cfg.robot.solver_cfg["left_arm"].root_link_name == "left_base_link" + assert cfg.robot.solver_cfg["left_arm"].end_link_name == "left_ee_link" + assert cfg.robot.solver_cfg["right_arm"].root_link_name == "right_base_link" + assert cfg.robot.solver_cfg["right_arm"].end_link_name == "right_ee_link" + assert cfg.robot.solver_cfg["right_arm"].tcp[2][3] == pytest.approx(0.155) + assert list(cfg.robot.init_qpos) == pytest.approx( + [ + 0.0, + 0.0, + -1.57, + -1.57, + 1.57, + 1.57, + -1.57, + -1.57, + -1.57, + -1.57, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + ] + ) + assert [item.uid for item in cfg.background] == [SUPPORT_SURFACE_UID] + assert [item.uid for item in cfg.rigid_object] == [CAN_SIMULATION_UID] + assert cfg.rigid_object[0].max_convex_hull_num == 1 + assert cfg.expert_program is not None + assert cfg.expert_program.program_id == "dual_ur5_hand_over" + + +def test_hand_over_physics_configs_match_tuned_can_and_pgi_parameters() -> None: + """Python and JSON configs share real can and master-only PGI dynamics.""" + direct_cfg = _create_default_env_cfg() + json_cfg = config_to_cfg(_gym_payload(), source_path=_gym_config_path()) + + expected_values = { + "stiffness": GRIPPER_MASTER_DRIVE_STIFFNESS, + "damping": GRIPPER_MASTER_DRIVE_DAMPING, + "max_effort": GRIPPER_MASTER_DRIVE_MAX_EFFORT, + } + for cfg in (direct_cfg, json_cfg): + assert cfg.rigid_object[0].attrs.mass == pytest.approx(CAN_MASS) + drive = cfg.robot.drive_pros + for property_name, master_value in expected_values.items(): + values = getattr(drive, property_name) + for side in ("left", "right"): + assert values[f"{side}_gripper_finger1_joint_1"] == pytest.approx( + master_value + ) + assert values[f"{side}_gripper_finger2_joint_1"] == pytest.approx(0.0) + + +def test_hand_over_registration_owns_scene_and_pose_provider() -> None: + """Static registration fingerprints the exact grasp and pose declarations.""" + scene = create_hand_over_scene_binding() + grasp = scene.antipodal_grasps[0] + + assert scene.registry_id == HAND_OVER_SCENE_REGISTRY_ID + assert [item.entity_id for item in scene.rigid_objects] == [ + CAN_UID, + SUPPORT_SURFACE_UID, + ] + assert scene.rigid_objects[0].simulation_uid == CAN_SIMULATION_UID + assert grasp.object_id == CAN_UID + assert grasp.generator_cfg.antipodal_sampler_cfg.n_sample == 10000 + assert grasp.force_reannotate is False + assert HAND_OVER_EXPERT_PROGRAM_REGISTRATION.scene_binding == scene + assert HAND_OVER_EXPERT_PROGRAM_REGISTRATION.handover_pose_providers == ( + HAND_OVER_POSE_PROVIDER, + ) + assert ( + ConfiguredHandOverPoseProvider.provider_id + == "simulation.configured_handover_pose" + ) + assert HAND_OVER_POSE_PROVIDER.middle_position == pytest.approx((0.0, 0.0, 0.7)) + assert HAND_OVER_POSE_PROVIDER.final_position == pytest.approx((0.0, -0.2, 0.7)) + + +def test_hand_over_profile_binds_left_pick_and_left_to_right_transfer() -> None: + """The profile selects both participants and its tuned motion policy.""" + binding = create_hand_over_robot_profile_binding() + + assert binding.profile_id == HAND_OVER_ROBOT_PROFILE_ID + assert [resource.resource_id for resource in binding.resources] == [ + "left", + "right", + ] + assert [ + endpoint.control_part + for resource in binding.resources + for endpoint in resource.endpoints + ] == ["left_arm", "left_hand", "right_arm", "right_hand"] + assert dict(binding.defaults["pick_up"]) == {"primary": "left"} + assert dict(binding.defaults["hand_over"]) == { + "source": "left", + "destination": "right", + } + assert binding.presets[0].preset_id == "safe" + assert binding.presets[0].motion_policy.sample_count == HAND_OVER_SAMPLE_COUNT + assert binding.presets[0].workflow_recovery_policy.max_recovery_attempts == 2 + assert binding.presets[0].runner_cfg.hold_during_effect_verification is False + assert binding.presets[0].runner_cfg.hold_on_completion is False + templates = binding.presets[0].action_option_templates + pick_options = templates["pick"] + assert type(pick_options) is PickUpOptions + assert pick_options.pick_object_part == "top" + assert pick_options.pre_grasp_distance == pytest.approx(0.08) + assert pick_options.lift_height == pytest.approx(0.10) + assert pick_options.hand_interp_steps == 5 + torch.testing.assert_close( + pick_options.approach_direction, + torch.tensor([0.0, -0.7071067812, -0.7071067812]), + ) + hand_over_options = templates["hand_over"] + assert type(hand_over_options) is HandOverOptions + assert hand_over_options.receive_pick_object_part == "bottom" + assert hand_over_options.pre_grasp_distance == pytest.approx(0.08) + assert hand_over_options.lift_height == pytest.approx(0.08) + assert hand_over_options.hand_interp_steps == 10 + assert hand_over_options.hold_steps == 4 + assert hand_over_options.retreat_steps == 28 + torch.testing.assert_close( + hand_over_options.receive_approach_direction, + torch.tensor([0.0, 0.7071067812, -0.7071067812]), + ) + assert dict(binding.grounding_providers) == { + "hand_over": ConfiguredHandOverPoseProvider.provider_id, + } + for side, preset in zip(("left", "right"), binding.command_presets): + assert preset.control_part == f"{side}_hand" + assert tuple(preset.commands["open"]) == (GRIPPER_OPEN_QPOS,) + assert tuple(preset.commands["grasp"]) == (GRIPPER_GRASP_QPOS,) + assert tuple(preset.commands["grasp"]) == pytest.approx( + (EXPECTED_GRIPPER_GRASP_QPOS,) + ) + + +def test_direct_default_cfg_loads_the_registered_semantic_program() -> None: + """Direct construction and JSON startup select the same registration IDs.""" + cfg = _create_default_env_cfg() + + assert type(cfg.robot) is RobotCfg + assert cfg.sensor == [] + assert cfg.expert_program is not None + assert cfg.expert_program.integration.scene_registry == HAND_OVER_SCENE_REGISTRY_ID + assert cfg.expert_program.integration.robot_profile == HAND_OVER_ROBOT_PROFILE_ID + assert cfg.expert_program.integration.runtime_preset == "safe" + settle = cfg.events["settle_can_on_reset"] + assert settle.params["entity_cfgs"][0].uid == CAN_SIMULATION_UID + + +def test_task_initialization_passes_only_registration_to_shared_factory( + monkeypatch, +) -> None: + """Task setup has no provider side channel or local motion generator.""" + adapter = object() + captured: dict[str, object] = {} + + def fake_base_init(self, cfg, **kwargs) -> None: + del self, cfg, kwargs + + def fake_create_adapter(environment, **kwargs): + captured["environment"] = environment + captured.update(kwargs) + return adapter + + monkeypatch.setattr(EmbodiedEnv, "__init__", fake_base_init) + task_module = importlib.import_module(HandOverEnv.__module__) + monkeypatch.setattr( + task_module, + "create_simulation_expert_program_adapter", + fake_create_adapter, + ) + + env = HandOverEnv(cfg=object()) + + assert env.expert_program_adapter is adapter + assert captured == { + "environment": env, + "registration": HAND_OVER_EXPERT_PROGRAM_REGISTRATION, + } + + +def test_task_config_compiles_through_real_simulation_factory(monkeypatch) -> None: + """The packaged program reaches the real adapter through explicit mocks.""" + + class FakeRobot: + uid = "DualUR5HandOver" + + @staticmethod + def get_qpos(*, target: bool = False) -> torch.Tensor: + del target + return torch.zeros((1, 16), dtype=torch.float32) + + class FakeRigidObject: + def __init__(self, *, is_non_dynamic: bool) -> None: + self.is_non_dynamic = is_non_dynamic + + robot = FakeRobot() + can = FakeRigidObject(is_non_dynamic=False) + support = FakeRigidObject(is_non_dynamic=True) + + class FakeSimulation: + @staticmethod + def get_robot(uid: str): + return robot if uid == robot.uid else None + + @staticmethod + def get_rigid_object(uid: str): + return { + CAN_SIMULATION_UID: can, + SUPPORT_SURFACE_UID: support, + }.get(uid) + + def fake_base_init(self, cfg, **kwargs) -> None: + del kwargs + self.cfg = cfg + self.sim_cfg = SimpleNamespace(physics_dt=0.01) + self.sim = FakeSimulation() + self.robot = robot + + monkeypatch.setattr(EmbodiedEnv, "__init__", fake_base_init) + cfg = config_to_cfg(_gym_payload(), source_path=_gym_config_path()) + + env = HandOverEnv(cfg=cfg) + segments = tuple(env.compile_expert_program(cfg.expert_program)) + + assert len(segments) == 1 + assert segments[0].name == "hand_over_can" + assert [type(call.call) for call in segments[0].calls] == [Pick, HandOver] + assert len(segments[0].post_policies) == 1 + assert len(segments[0].validators) == 1 + assert env.expert_program_adapter.scene_registry_id == (HAND_OVER_SCENE_REGISTRY_ID) + assert env.expert_program_adapter.robot_profile_id == (HAND_OVER_ROBOT_PROFILE_ID) + + +@pytest.mark.requires_sim +@pytest.mark.slow +def test_real_sim_expert_episode_transfers_can_with_effect_and_validation_trace() -> ( + None +): + """The full semantic episode proves transfer effects, settling, and validation.""" + import gc + + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + + cfg = config_to_cfg(_gym_payload(), source_path=_gym_config_path()) + cfg.num_envs = 1 + cfg.sim_cfg = SimulationManagerCfg( + headless=True, + sim_device="cpu", + num_envs=1, + ) + cfg.sensor = [] + cfg.observations = None + cfg.dataset = None + cfg.init_rollout_buffer = False + cfg.record_trajectory = False + cfg.filter_dataset_saving = True + + env: HandOverEnv | None = None + try: + env = HandOverEnv(cfg=cfg) + env.reset(seed=0) + can = env.sim.get_rigid_object(CAN_SIMULATION_UID) + assert can is not None + initial_can_pose = can.get_local_pose(to_matrix=True).tolist() + initial_left_eef = env.robot.compute_fk( + env.robot.get_qpos(name="left_arm"), + name="left_arm", + to_matrix=True, + ).tolist() + initial_qpos = env.robot.get_qpos().tolist() + + result = execute_demo_episode(env) + + if not result.completed: + runtime = result.segments[0].metadata["runtime"] + failed_call = runtime["calls"][-1] + last_effect = ( + None if not failed_call["effects"] else failed_call["effects"][-1] + ) + pytest.fail( + json.dumps( + { + "terminal_reason": result.terminal_reason, + "initial_can_pose": initial_can_pose, + "initial_left_eef": initial_left_eef, + "initial_qpos": initial_qpos, + "final_can_pose": can.get_local_pose(to_matrix=True).tolist(), + "final_left_eef": env.robot.compute_fk( + env.robot.get_qpos(name="left_arm"), + name="left_arm", + to_matrix=True, + ).tolist(), + "final_left_hand_qpos": env.robot.get_qpos( + name="left_hand" + ).tolist(), + "events": [ + { + "kind": event["kind"], + "timestamp": event["timestamp"], + "message": event["message"], + } + for event in runtime["events"] + ], + "plan_success_masks": [ + attempt["plan_success_mask"] + for attempt in failed_call["plan_attempts"] + ], + "last_effect": last_effect, + "post_policies": result.segments[0].metadata["post_policies"], + "validation": result.segments[0].metadata["validation"], + }, + sort_keys=True, + ), + pytrace=False, + ) + assert result.all_success + assert result.terminal_reason == "success" + assert len(result.segments) == 1 + segment = result.segments[0] + assert segment.name == "hand_over_can" + assert segment.success + + metadata = segment.metadata + runtime = metadata["runtime"] + assert runtime["kind"] == "skill_result" + assert runtime["status"] == "completed" + assert runtime["masks"]["success"] == [True] + assert [call["semantic_id"] for call in runtime["calls"]] == [ + "pick", + "hand_over", + ] + for call in runtime["calls"]: + assert call["status"] == "completed" + assert call["masks"] == { + "entered": [True], + "completed": [True], + "failed": [False], + } + assert call["plan_attempts"] + assert call["plan_attempts"][-1]["plan_success_mask"] == [True] + assert call["effects"] + assert call["effects"][-1]["decision"] == { + "success_mask": [True], + "failure_mask": [False], + } + + pick_effect = runtime["calls"][0]["effects"][-1] + assert pick_effect["effect_spec"]["semantic_id"] == "pick" + assert set(pick_effect["evidence"]) == { + "destination.pose", + "destination.constraint", + } + assert pick_effect["evidence"]["destination.constraint"]["values"] == [True] + + transfer_effect = runtime["calls"][1]["effects"][-1] + assert transfer_effect["effect_spec"]["semantic_id"] == "hand_over" + assert set(transfer_effect["evidence"]) == { + "source.pose", + "source.constraint", + "destination.pose", + "destination.constraint", + } + for evidence in transfer_effect["evidence"].values(): + assert evidence["valid_mask"] == [True] + assert evidence["acquisition_errors"] == [None] + assert evidence["env_ids"] == [0] + assert transfer_effect["evidence"]["source.constraint"]["values"] == [False] + assert transfer_effect["evidence"]["destination.constraint"]["values"] == [True] + + post_policies = metadata["post_policies"] + assert len(post_policies) == 1 + assert post_policies[0]["kind"] == "wait_stable" + assert post_policies[0]["result_mask"] == [True] + assert post_policies[0]["result"]["status"] == "settled" + assert post_policies[0]["result"]["state"]["settled_mask"] == [True] + assert post_policies[0]["result"]["state"]["timeout_mask"] == [False] + + validation = metadata["validation"] + assert validation["runtime_success_mask"] == [True] + assert validation["eligible_mask_before_validation"] == [True] + assert validation["post_policy_success_mask"] == [True] + assert validation["accepted_mask"] == [True] + assert len(validation["validators"]) == 1 + validator = validation["validators"][0] + assert validator["kind"] == "object_near_target" + assert validator["result_mask"] == [True] + assert validator["result"]["accepted_mask"] == [True] + assert validator["result"]["position_tolerance"] == pytest.approx(0.12) + assert validator["result"]["position_error"][0] <= 0.12 + finally: + if env is not None: + env.close() + SimulationManager.flush_cleanup_queue() + gc.collect() + + +__all__: list[str] = [] diff --git a/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py b/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py index 6e6fe9495..fe816f85a 100644 --- a/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py +++ b/tests/gym/envs/tasks/test_multi_segments_cube_pick_place.py @@ -133,6 +133,7 @@ def test_robot_profile_calibrates_physical_tracking_tolerance() -> None: assert tracking.in_flight is not None assert tracking.in_flight.metrics[0].tolerance == 0.08 assert tracking.terminal.metrics[0].tolerance == 0.08 + assert binding.presets[0].workflow_recovery_policy.max_recovery_attempts == 2 def test_task_initialization_delegates_to_shared_simulation_factory( @@ -185,7 +186,8 @@ class FakeRobot: uid = "UR5" @staticmethod - def get_qpos() -> torch.Tensor: + def get_qpos(*, target: bool = False) -> torch.Tensor: + del target return torch.zeros((1, 8), dtype=torch.float32) class FakeCube: diff --git a/tests/gym/envs/tasks/test_open_drawer.py b/tests/gym/envs/tasks/test_open_drawer.py index 725d3c77d..acb4293d1 100644 --- a/tests/gym/envs/tasks/test_open_drawer.py +++ b/tests/gym/envs/tasks/test_open_drawer.py @@ -162,7 +162,8 @@ class FakeRobot: uid = "CobotMagic" @staticmethod - def get_qpos() -> torch.Tensor: + def get_qpos(*, target: bool = False) -> torch.Tensor: + del target return torch.zeros((1, 16), dtype=torch.float32) class FakeDrawer: diff --git a/tests/sim/atomic_actions/test_engine_per_env.py b/tests/sim/atomic_actions/test_engine_per_env.py index b81c96e33..9fad65547 100644 --- a/tests/sim/atomic_actions/test_engine_per_env.py +++ b/tests/sim/atomic_actions/test_engine_per_env.py @@ -42,6 +42,7 @@ EndpointTrackingChannelBinding, EndpointTrackingFeedbackAddress, EntityState, + EffectExpectationResult, ExecutionEventKind, ExecutionSession, ExecutionStatus, @@ -49,6 +50,8 @@ EffectVerificationRequirement, EffectVerificationResult, GraspGoal, + HeldObjectGuardRequest, + HeldObjectGuardResult, HeldObjectState, JointPositionPayload, JointPositionTarget, @@ -56,6 +59,9 @@ ObjectSemantics, PlannerDiagnostics, PlanningContext, + PhaseEffectGateRequest, + PhaseEffectGateRequirement, + PhaseEffectGateResult, RecoveryPolicy, ResolvedActionRequest, RobotObservation, @@ -81,6 +87,30 @@ from embodichain.lab.sim.planners import PlanOptions +def _effect_result( + verification_id: int, + success_mask: torch.Tensor, + failure_mask: torch.Tensor, + *, + invalidation_mask: torch.Tensor | None = None, + retry_mask: torch.Tensor | None = None, + expectation_results: tuple[EffectExpectationResult, ...] = (), +) -> EffectVerificationResult: + """Build an explicit terminal decision with legacy retry semantics.""" + return EffectVerificationResult( + verification_id=verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + invalidation_mask=( + torch.zeros_like(failure_mask) + if invalidation_mask is None + else invalidation_mask + ), + retry_mask=failure_mask if retry_mask is None else retry_mask, + expectation_results=expectation_results, + ) + + class DynamicAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): """Test action whose terminal joint command follows a scene entity x pose.""" @@ -118,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.""" @@ -508,6 +564,74 @@ def _context( ) +def _with_held_object( + context: PlanningContext, + *, + env_mask: torch.Tensor | None = None, +) -> PlanningContext: + """Attach one verified test object to the logical arm resource.""" + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="object", + entity_id="object", + ) + held = HeldObjectState( + semantics=semantics, + object_to_eef=torch.eye(4), + grasp_xpos=torch.eye(4), + env_mask=env_mask, + ) + return replace( + context, + task=TaskState( + batch_size=context.batch_size, + device=context.robot.qpos.device, + held_objects={"arm": held}, + ), + ) + + +def _held_object_loss_result( + request: HeldObjectGuardRequest, + *, + failure_mask: torch.Tensor, + retry_mask: torch.Tensor, +) -> HeldObjectGuardResult: + """Build a loss result exactly correlated with one guard request.""" + return HeldObjectGuardResult( + verification_id=request.verification_id, + object_id="object", + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + failure_mask=failure_mask, + state_invalidation=StateDelta(held_object_updates={"arm": None}), + retry_mask=retry_mask, + message="Observed object-to-endpoint slip.", + ) + + +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, *, @@ -635,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, @@ -642,6 +789,7 @@ def _effect_session( action_timeout: float = 30.0, eligible_mask: torch.Tensor | None = None, action: DynamicAction | None = None, + task_state: TaskState | None = None, ) -> tuple[ExecutionSession, ExecutionTick]: """Advance a test effect action to its verification boundary.""" engine, _ = _engine(batch_size=batch_size) @@ -661,9 +809,12 @@ def _effect_session( ) qpos = tuple(0.0 for _ in range(batch_size)) target = tuple(0.2 for _ in range(batch_size)) + initial_context = _context(0.0, qpos, target, 0) + if task_state is not None: + initial_context = replace(initial_context, task=task_state) session = engine.start( (invocation,), - _context(0.0, qpos, target, 0), + initial_context, eligible_mask=eligible_mask, ) session.tick(_context(0.0, qpos, target, 0)) @@ -697,6 +848,371 @@ 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)) + session = engine.start( + (_invocation(engine, max_action_retries=1),), + initial, + ) + request = session.held_object_guard_request + assert request is not None + assert request.attempt_generation == 0 + assert request.invocation_index == 0 + assert request.next_waypoint_index == 0 + assert request.segment_name == "dynamic" + assert request.env_mask.tolist() == [True, True] + assert request.allowed_held_object_relations == (("arm", "object"),) + assert request.allowed_coordinated_held_object_relations == () + + retried = session.tick( + initial, + held_object_guard_result=_held_object_loss_result( + request, + failure_mask=torch.tensor([True, False]), + retry_mask=torch.tensor([True, False]), + ), + ) + + assert retried.status is ExecutionStatus.RUNNING + assert retried.command is not None + assert retried.command.active_mask.tolist() == [True, True] + held = retried.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [False, True] + lost = next( + event + for event in retried.events + if event.kind is ExecutionEventKind.HELD_OBJECT_LOST + ) + retry = next( + event + for event in retried.events + if event.kind is ExecutionEventKind.ACTION_RETRY + ) + assert lost.env_mask.tolist() == [True, False] + assert retry.env_mask.tolist() == [True, False] + assert session.plan_attempts[-1].action_retry_counts == (1, 0) + + +def test_held_object_loss_result_requires_state_invalidation() -> None: + with pytest.raises(ValueError, match="must contain relation removals"): + HeldObjectGuardResult( + verification_id=0, + object_id="object", + attempt_generation=0, + invocation_index=0, + next_waypoint_index=0, + failure_mask=torch.tensor([True]), + state_invalidation=StateDelta(), + retry_mask=torch.tensor([False]), + ) + + +def test_held_object_guard_rejects_unauthorized_state_invalidation() -> None: + engine, _ = _engine() + initial = _with_held_object(_context(0.0, 0.0, 0.2, 0)) + session = engine.start((_invocation(engine),), initial) + request = session.held_object_guard_request + assert request is not None + + result = HeldObjectGuardResult( + verification_id=request.verification_id, + object_id="object", + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + failure_mask=torch.tensor([True]), + state_invalidation=StateDelta(held_object_updates={"unrelated_resource": None}), + retry_mask=torch.tensor([False]), + ) + with pytest.raises(ValueError, match="authorized relation set"): + session.tick(initial, held_object_guard_result=result) + + +def test_held_object_guard_rejects_wrong_object_identity_on_authorized_key() -> None: + engine, _ = _engine() + initial = _with_held_object(_context(0.0, 0.0, 0.2, 0)) + session = engine.start((_invocation(engine),), initial) + request = session.held_object_guard_request + assert request is not None + + result = HeldObjectGuardResult( + verification_id=request.verification_id, + object_id="another_object", + attempt_generation=request.attempt_generation, + invocation_index=request.invocation_index, + next_waypoint_index=request.next_waypoint_index, + failure_mask=torch.tensor([True]), + state_invalidation=StateDelta(held_object_updates={"arm": None}), + retry_mask=torch.tensor([False]), + ) + with pytest.raises(ValueError, match="key/object identity"): + session.tick(initial, held_object_guard_result=result) + + +def test_stale_held_object_guard_result_is_rejected_within_same_attempt() -> None: + engine, _ = _engine() + initial = _with_held_object(_context(0.0, 0.0, 0.2, 0)) + session = engine.start((_invocation(engine),), initial) + first_request = session.held_object_guard_request + assert first_request is not None + + session.tick(initial) + current_request = session.held_object_guard_request + assert current_request is not None + assert current_request.verification_id == first_request.verification_id + 1 + + stale = HeldObjectGuardResult( + verification_id=first_request.verification_id, + object_id="object", + attempt_generation=current_request.attempt_generation, + invocation_index=current_request.invocation_index, + next_waypoint_index=current_request.next_waypoint_index, + failure_mask=torch.tensor([False]), + state_invalidation=StateDelta(), + retry_mask=torch.tensor([False]), + ) + with pytest.raises(ValueError, match="verification_id"): + session.tick(initial, held_object_guard_result=stale) + + +def test_nonretry_held_object_loss_fails_row_while_peer_continues() -> None: + engine, _ = _engine(batch_size=2) + initial = _with_held_object(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) + session = engine.start((_invocation(engine),), initial) + request = session.held_object_guard_request + assert request is not None + + partial = session.tick( + initial, + held_object_guard_result=_held_object_loss_result( + request, + failure_mask=torch.tensor([True, False]), + retry_mask=torch.tensor([False, False]), + ), + ) + + assert partial.status is ExecutionStatus.RUNNING + assert partial.eligible_mask.tolist() == [False, True] + assert partial.command is not None + assert partial.command.active_mask.tolist() == [False, True] + held = partial.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [False, True] + event_masks = { + event.kind: event.env_mask.tolist() + for event in partial.events + if event.kind + in { + ExecutionEventKind.HELD_OBJECT_LOST, + ExecutionEventKind.RECOVERY_REQUIRED, + } + } + assert event_masks == { + ExecutionEventKind.HELD_OBJECT_LOST: [True, False], + ExecutionEventKind.RECOVERY_REQUIRED: [True, False], + } + assert len(session.plan_attempts) == 1 + assert session.plan_attempts[0].action_retry_counts == (0, 0) + + +def test_missing_or_out_of_phase_held_object_guard_result_preserves_state() -> None: + engine, _ = _engine() + action = EffectAction() + engine.register(action) + initial = _with_held_object(_context(0.0, 0.0, 0.2, 0)) + base = _invocation(engine) + invocation = ActionInvocation( + skill_id=action.skill_id, + goal=base.goal, + binding=base.binding, + motion_policy=base.motion_policy, + recovery_policy=base.recovery_policy, + ) + session = engine.start((invocation,), initial) + + first = session.tick(initial) + assert first.task_state.get_held_object("arm") is not None + session.tick(_with_held_object(_context(0.1, 0.0, 0.2, 0))) + pending = session.tick(_with_held_object(_context(0.2, 0.2, 0.2, 0))) + + assert pending.pending_effect is not None + assert session.held_object_guard_request is None + preserved = session.tick(_with_held_object(_context(0.21, 0.2, 0.2, 0))) + held = preserved.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [True] + + def test_all_rows_planning_failure_skips_inactive_command_frames() -> None: engine, _ = _engine() action = FailedEffectAction() @@ -1741,7 +2257,7 @@ def test_explicit_verification_with_empty_delta_preserves_task_state() -> None: completed = session.tick( _context(0.21, 0.2, 0.2, 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( request.verification_id, success_mask=torch.tensor([True]), failure_mask=torch.tensor([False]), @@ -1766,7 +2282,7 @@ def test_explicit_verification_keeps_partial_and_retry_row_lifecycle() -> None: retry = session.tick( _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( first_request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, True]), @@ -1796,7 +2312,7 @@ def test_explicit_verification_keeps_partial_and_retry_row_lifecycle() -> None: completed = session.tick( _context(0.25, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( second_request.verification_id, success_mask=torch.tensor([False, True]), failure_mask=torch.tensor([False, False]), @@ -1821,7 +2337,7 @@ def test_explicit_verification_partial_success_shrinks_request_without_state_del partial = session.tick( _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( first_request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), @@ -1840,7 +2356,7 @@ def test_explicit_verification_partial_success_shrinks_request_without_state_del completed = session.tick( _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( second_request.verification_id, success_mask=torch.tensor([False, True]), failure_mask=torch.tensor([False, False]), @@ -1896,7 +2412,7 @@ def test_nonempty_effect_is_committed_only_after_external_verification() -> None still_waiting = session.tick(_context(0.25, 0.2, 0.2, 0)) completed = session.tick( _context(0.3, 0.2, 0.2, 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( waiting.pending_effect.verification_id, torch.tensor([True]), torch.tensor([False]), @@ -1939,7 +2455,7 @@ def test_initially_ineligible_rows_never_receive_effects() -> None: completed = session.tick( _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), @@ -1960,7 +2476,7 @@ def test_partial_effect_success_commits_resolved_rows_and_shrinks_request() -> N no_progress = session.tick( _context(0.205, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( first_request.verification_id, success_mask=torch.tensor([False, False]), failure_mask=torch.tensor([False, False]), @@ -1971,7 +2487,7 @@ def test_partial_effect_success_commits_resolved_rows_and_shrinks_request() -> N partial = session.tick( _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( first_request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), @@ -1994,7 +2510,7 @@ def test_partial_effect_success_commits_resolved_rows_and_shrinks_request() -> N with pytest.raises(ValueError, match="verification_id"): session.tick( _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( first_request.verification_id, success_mask=torch.tensor([False, True]), failure_mask=torch.tensor([False, False]), @@ -2004,7 +2520,7 @@ def test_partial_effect_success_commits_resolved_rows_and_shrinks_request() -> N current_request = partial.pending_effect completed = session.tick( _context(0.23, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( current_request.verification_id, success_mask=torch.tensor([False, True]), failure_mask=torch.tensor([False, False]), @@ -2020,17 +2536,45 @@ def test_partial_effect_success_commits_resolved_rows_and_shrinks_request() -> N def test_effect_result_masks_are_owned_disjoint_and_request_scoped() -> None: success = torch.tensor([True, False]) failure = torch.tensor([False, True]) - result = EffectVerificationResult(0, success, failure) + result = _effect_result(0, success, failure) success.fill_(False) failure.fill_(False) assert result.success_mask.tolist() == [True, False] assert result.failure_mask.tolist() == [False, True] with pytest.raises(ValueError, match="must not overlap"): - EffectVerificationResult( + _effect_result( + 0, + torch.tensor([True, False]), + torch.tensor([True, False]), + ) + with pytest.raises(ValueError, match="invalidation_mask must be a subset"): + _effect_result( 0, + torch.tensor([False, False]), torch.tensor([True, False]), + invalidation_mask=torch.tensor([False, True]), + ) + with pytest.raises(ValueError, match="retry_mask must be a subset"): + _effect_result( + 0, + torch.tensor([False, False]), torch.tensor([True, False]), + retry_mask=torch.tensor([False, True]), + ) + with pytest.raises(ValueError, match="conjunction"): + _effect_result( + 0, + torch.tensor([False, False]), + torch.tensor([False, True]), + expectation_results=( + EffectExpectationResult( + expectation_id="destination", + satisfied_mask=torch.tensor([True, False]), + contradicted_mask=torch.tensor([False, True]), + inverse_satisfied_mask=torch.tensor([False, False]), + ), + ), ) session, waiting = _effect_session(batch_size=2) @@ -2053,7 +2597,7 @@ def test_effect_result_masks_are_owned_disjoint_and_request_scoped() -> None: partial = session.tick( _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( preserved.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), @@ -2068,7 +2612,7 @@ def test_effect_result_masks_are_owned_disjoint_and_request_scoped() -> None: with pytest.raises(ValueError, match="subsets"): session.tick( _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( current.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), @@ -2119,7 +2663,7 @@ def test_partial_effect_failure_waits_for_unresolved_rows_then_retries_failure() partial = session.tick( _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( request.verification_id, success_mask=torch.tensor([False, False]), failure_mask=torch.tensor([True, False]), @@ -2135,7 +2679,7 @@ def test_partial_effect_failure_waits_for_unresolved_rows_then_retries_failure() unresolved_request = partial.pending_effect resolved = session.tick( _context(0.22, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( unresolved_request.verification_id, success_mask=torch.tensor([False, True]), failure_mask=torch.tensor([False, False]), @@ -2163,6 +2707,95 @@ def test_partial_effect_failure_waits_for_unresolved_rows_then_retries_failure() assert retry_command.command.active_mask.tolist() == [True, False] +def test_effect_failure_applies_request_owned_invalidation_before_recovery() -> None: + initial = _with_held_object(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)).task + session, waiting = _effect_session( + batch_size=2, + max_action_retries=1, + task_state=initial, + ) + request = waiting.pending_effect + assert request is not None + assert request.failure_invalidation.held_object_updates == {"arm": None} + + terminal = session.tick( + _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), + effect_result=_effect_result( + request.verification_id, + success_mask=torch.tensor([False, True]), + failure_mask=torch.tensor([True, False]), + invalidation_mask=torch.tensor([True, False]), + retry_mask=torch.tensor([False, False]), + ), + ) + + held = terminal.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None + assert held.env_mask.tolist() == [False, True] + assert terminal.eligible_mask.tolist() == [False, True] + assert any( + event.kind is ExecutionEventKind.RECOVERY_REQUIRED + and event.env_mask.tolist() == [True, False] + for event in terminal.events + ) + assert not any( + event.kind is ExecutionEventKind.ACTION_RETRY for event in terminal.events + ) + + +def test_inverse_proof_can_preserve_state_while_failure_requires_recovery() -> None: + initial = _with_held_object(_context(0.0, 0.0, 0.2, 0)).task + session, waiting = _effect_session(task_state=initial) + request = waiting.pending_effect + assert request is not None + failure = torch.tensor([True]) + + terminal = session.tick( + _context(0.21, 0.2, 0.2, 0), + effect_result=_effect_result( + request.verification_id, + success_mask=torch.tensor([False]), + failure_mask=failure, + invalidation_mask=torch.tensor([False]), + retry_mask=torch.tensor([False]), + expectation_results=( + EffectExpectationResult( + expectation_id="source", + satisfied_mask=torch.tensor([False]), + contradicted_mask=failure, + inverse_satisfied_mask=failure, + ), + ), + ), + ) + + held = terminal.task_state.get_held_object("arm") + assert held is not None and held.env_mask is not None and held.env_mask.all() + assert terminal.status is ExecutionStatus.FAILED + assert any( + event.kind is ExecutionEventKind.RECOVERY_REQUIRED for event in terminal.events + ) + + +def test_unresolved_effect_timeout_invalidates_active_state_fail_closed() -> None: + initial = _with_held_object(_context(0.0, 0.0, 0.2, 0)).task + session, waiting = _effect_session( + action_timeout=0.25, + max_action_retries=1, + task_state=initial, + ) + assert waiting.pending_effect is not None + + terminal = session.tick(_context(0.26, 0.2, 0.2, 0)) + + assert terminal.status is ExecutionStatus.FAILED + assert terminal.task_state.get_held_object("arm") is None + kinds = {event.kind for event in terminal.events} + assert ExecutionEventKind.EFFECT_VERIFICATION_TIMEOUT in kinds + assert ExecutionEventKind.RECOVERY_REQUIRED in kinds + assert ExecutionEventKind.ACTION_RETRY not in kinds + + def test_effect_failure_exhaustion_advances_completed_rows_without_empty_request() -> ( None ): @@ -2172,7 +2805,7 @@ def test_effect_failure_exhaustion_advances_completed_rows_without_empty_request terminal = session.tick( _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, True]), @@ -2202,7 +2835,7 @@ def test_deactivating_last_unresolved_effect_row_advances_barrier() -> None: assert request is not None partial = session.tick( _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), @@ -2261,7 +2894,7 @@ def test_effect_request_deadline_is_stable_and_accepts_result_at_boundary() -> N completed = session.tick( _context(0.25, 0.2, 0.2, 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( request.verification_id, success_mask=torch.tensor([True]), failure_mask=torch.tensor([False]), @@ -2293,7 +2926,7 @@ def test_session_revision_cannot_abandon_pending_effect_verification() -> None: assert session.effect_verification_pending is True completed = session.tick( _context(0.3, 0.2, 0.2, 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( waiting.pending_effect.verification_id, torch.tensor([True]), torch.tensor([False]), @@ -2322,7 +2955,7 @@ def test_effect_failure_does_not_commit_and_exhausts_retry_budget() -> None: assert waiting.pending_effect is not None failed = session.tick( _context(0.3, 0.2, 0.2, 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( waiting.pending_effect.verification_id, torch.tensor([False]), torch.tensor([True]), @@ -2359,7 +2992,7 @@ def test_pending_effect_timeout_exhausts_without_committing_late_result() -> Non timed_out = session.tick( _context(0.3, 0.2, 0.2, 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( waiting.pending_effect.verification_id, torch.tensor([True]), torch.tensor([False]), @@ -2384,7 +3017,7 @@ def test_effect_timeout_exhaustion_advances_rows_already_verified() -> None: assert request is not None partial = session.tick( _context(0.21, (0.2, 0.2), (0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), @@ -2455,7 +3088,7 @@ def test_deferred_effect_failure_charges_concurrent_planning_failures() -> None: assert request is not None partial = session.tick( _context(0.21, (0.2, 0.2, 0.2), (0.2, 0.2, 0.2), 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( request.verification_id, success_mask=torch.tensor([False, False, False]), failure_mask=torch.tensor([True, False, False]), @@ -2523,7 +3156,7 @@ def test_effect_retry_invalidates_previous_verification_id() -> None: with pytest.raises(ValueError, match="verification_id"): session.tick( _context(0.55, 0.2, 0.2, 0), - effect_result=EffectVerificationResult( + effect_result=_effect_result( old_id, torch.tensor([True]), torch.tensor([False]), diff --git a/tests/sim/atomic_actions/test_runner.py b/tests/sim/atomic_actions/test_runner.py index 12bec40ac..a17d374cc 100644 --- a/tests/sim/atomic_actions/test_runner.py +++ b/tests/sim/atomic_actions/test_runner.py @@ -42,6 +42,7 @@ ExecutionEventKind, ExecutionRunner, ExecutionRunnerCfg, + HeldObjectGuardRequest, HeldObjectState, JOINT_POSITION_CAPABILITY, JointPositionPayload, @@ -49,6 +50,9 @@ JointPositionTarget, MotionPolicy, ObjectSemantics, + PhaseEffectGateRequest, + PhaseEffectGateRequirement, + PhaseEffectGateResult, PlanningContextTrackingFeedbackProvider, PlanningContext, RecoveryPolicy, @@ -57,6 +61,7 @@ RuntimeCommandFrame, RuntimeEndpointTarget, RunnerStatus, + RunnerStep, SceneSnapshot, SkillBindingContract, SkillEndpointRequirement, @@ -82,6 +87,49 @@ TARGET_POSITION = 1.0 +def _effect_result( + verification_id: int, + success_mask: torch.Tensor, + failure_mask: torch.Tensor, + *, + invalidation_mask: torch.Tensor | None = None, + retry_mask: torch.Tensor | None = None, +) -> EffectVerificationResult: + """Build an explicit terminal decision with legacy retry semantics.""" + return EffectVerificationResult( + verification_id=verification_id, + success_mask=success_mask, + failure_mask=failure_mask, + invalidation_mask=( + torch.zeros_like(failure_mask) + if invalidation_mask is None + else invalidation_mask + ), + retry_mask=failure_mask if retry_mask is None else retry_mask, + ) + + +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.""" @@ -274,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( @@ -321,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 + ), ) @@ -335,11 +392,14 @@ 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, action_timeout: float = 10.0, tracking_runtime: TrackingRuntime | None = None, + hold_on_completion: bool = True, + hold_during_effect_verification: bool = True, ) -> tuple[ ExecutionRunner, FakeClock, @@ -362,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") @@ -379,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( @@ -386,7 +459,11 @@ def _make_runner( provider, sink, clock=clock, - cfg=ExecutionRunnerCfg(minimum_cycle_time=MINIMUM_CYCLE_TIME), + cfg=ExecutionRunnerCfg( + minimum_cycle_time=MINIMUM_CYCLE_TIME, + hold_on_completion=hold_on_completion, + hold_during_effect_verification=hold_during_effect_verification, + ), ) return runner, clock, provider, sink, action @@ -396,7 +473,7 @@ def _successful_effect_result( request: EffectVerificationRequest, ) -> EffectVerificationResult: """Correlate a successful result with the pending effect boundary.""" - return EffectVerificationResult( + return _effect_result( verification_id=request.verification_id, success_mask=torch.ones( context.batch_size, @@ -411,6 +488,26 @@ def _successful_effect_result( ) +def _unresolved_effect_result( + context: PlanningContext, + request: EffectVerificationRequest, +) -> EffectVerificationResult: + """Keep every row pending at the current effect boundary.""" + return _effect_result( + verification_id=request.verification_id, + success_mask=torch.zeros( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + failure_mask=torch.zeros( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + ) + + def test_joint_feedback_ignores_motion_outside_bound_endpoint() -> None: runner, clock, provider, sink, action = _make_runner(control_joint_ids=(0,)) @@ -458,6 +555,147 @@ def test_runner_dispatches_only_when_timed_waypoint_is_due() -> None: assert third.wait_duration == pytest.approx(SECOND_INTERVAL) +def test_runner_calls_held_object_guard_with_fresh_command_phase() -> None: + runner, _, _, sink, _ = _make_runner() + observed: list[tuple[float, HeldObjectGuardRequest]] = [] + + def verifier( + context: PlanningContext, + request: HeldObjectGuardRequest, + ) -> None: + observed.append((context.robot.timestamp, request)) + return None + + first = runner.step(held_object_guard_verifier=verifier) + + assert first.status is RunnerStatus.RUNNING + assert len(sink.sent) == 1 + assert len(observed) == 1 + timestamp, request = observed[0] + assert timestamp == 0.0 + assert request.verification_id == 0 + assert request.segment_name == "timed" + assert request.attempt_generation == 0 + assert request.invocation_index == 0 + assert request.next_waypoint_index == 0 + + +def test_runner_guard_exception_performs_cancel_then_observed_hold() -> None: + runner, clock, _, sink, _ = _make_runner() + runner.step() + clock.advance(FIRST_INTERVAL) + + def verifier( + context: PlanningContext, + request: HeldObjectGuardRequest, + ) -> None: + del context, request + raise RuntimeError("guard evidence unavailable") + + failed = runner.step(held_object_guard_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 [target.target_id for target in sink.cancelled[0]] == ["arm"] + assert failed.message is not None + 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() @@ -858,6 +1096,62 @@ def test_blocking_runner_resumes_a_stored_effect_verification_boundary() -> None assert completed.tick.task_state.get_held_object("arm") is not None +def test_runner_holds_while_effect_verification_is_pending_by_default() -> None: + runner, _, _, sink, _ = _make_runner(with_effect=True) + + blocked = runner.run_until_blocked() + + assert blocked.status is RunnerStatus.RUNNING + assert blocked.tick is not None and blocked.tick.pending_effect is not None + assert [item.operation for item in blocked.dispatches] == [CommandOperation.HOLD] + assert len(sink.held) == 1 + assert [target.target_id for target in sink.held[0][0]] == ["arm"] + + +def test_runner_skips_all_effect_pending_holds_when_disabled() -> None: + runner, clock, _, sink, _ = _make_runner( + with_effect=True, + hold_on_completion=False, + hold_during_effect_verification=False, + ) + + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + assert blocked.dispatches == () + + polls: list[RunnerStep] = [] + for _ in range(2): + clock.advance(MINIMUM_CYCLE_TIME) + polls.append(runner.step(effect_verifier=_unresolved_effect_result)) + + assert all(step.status is RunnerStatus.RUNNING for step in polls) + assert all( + step.tick is not None and step.tick.pending_effect is not None for step in polls + ) + assert all(step.dispatches == () for step in polls) + assert sink.held == [] + + +def test_effect_success_adds_no_hold_when_pending_and_completion_holds_are_disabled() -> ( + None +): + runner, clock, _, sink, _ = _make_runner( + with_effect=True, + hold_on_completion=False, + hold_during_effect_verification=False, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + clock.advance(MINIMUM_CYCLE_TIME) + + completed = runner.step(effect_verifier=_successful_effect_result) + + assert completed.status is RunnerStatus.COMPLETED + assert completed.tick is not None and completed.tick.pending_effect is None + assert completed.dispatches == () + assert sink.held == [] + + def test_resumed_effect_verifier_uses_a_fresh_observation() -> None: runner, clock, _, _, _ = _make_runner(with_effect=True) blocked = runner.run_until_blocked() @@ -959,7 +1253,7 @@ def test_effect_verifier_is_not_called_after_deadline_and_session_retries() -> N def test_effect_result_and_effect_verifier_are_mutually_exclusive() -> None: runner, _, _, sink, action = _make_runner(with_effect=True) - result = EffectVerificationResult( + result = _effect_result( verification_id=0, success_mask=torch.tensor([True]), failure_mask=torch.tensor([False]), @@ -1019,7 +1313,7 @@ def report_no_progress( request: EffectVerificationRequest, ) -> EffectVerificationResult: observed_requests.append((request.verification_id, request.attempt_generation)) - return EffectVerificationResult( + return _effect_result( verification_id=request.verification_id, success_mask=torch.zeros(context.batch_size, dtype=torch.bool), failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), @@ -1055,7 +1349,7 @@ def verify_in_two_updates( None if held is None or held.env_mask is None else held.env_mask.tolist() ) if request.env_mask.tolist() == [True, True]: - return EffectVerificationResult( + return _effect_result( verification_id=request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), @@ -1064,7 +1358,7 @@ def verify_in_two_updates( assert held is not None and held.env_mask is not None assert held.env_mask.tolist() == [True, False] assert torch.equal(context.task.held_objects["arm"].env_mask, held.env_mask) - return EffectVerificationResult( + return _effect_result( verification_id=request.verification_id, success_mask=torch.tensor([False, True]), failure_mask=torch.tensor([False, False]), @@ -1126,6 +1420,34 @@ def test_runner_effect_timeout_exhaustion_cancels_and_holds() -> None: assert sink.cancel_count == 1 +def test_effect_timeout_still_cancels_and_holds_when_pending_holds_are_disabled() -> ( + None +): + runner, clock, _, sink, _ = _make_runner( + with_effect=True, + max_action_retries=0, + action_timeout=2.0, + hold_on_completion=False, + hold_during_effect_verification=False, + ) + blocked = runner.run_until_blocked() + assert blocked.tick is not None and blocked.tick.pending_effect is not None + assert sink.held == [] + request = blocked.tick.pending_effect + clock.advance(request.deadline - clock.now() + MINIMUM_CYCLE_TIME) + + failed = runner.step() + + assert failed.status is RunnerStatus.FAILED + assert [item.operation for item in failed.dispatches] == [ + CommandOperation.CANCEL, + CommandOperation.HOLD, + ] + assert sink.cancel_count == 1 + assert len(sink.held) == 1 + assert [target.target_id for target in sink.held[0][0]] == ["arm"] + + def test_runner_deactivation_refreshes_cached_effect_request() -> None: runner, _, _, _, _ = _make_runner(with_effect=True, batch_size=2) blocked = runner.run_until_blocked() @@ -1149,7 +1471,7 @@ def verify_remaining( context: PlanningContext, request: EffectVerificationRequest, ) -> EffectVerificationResult: - return EffectVerificationResult( + return _effect_result( verification_id=request.verification_id, success_mask=torch.tensor([True, False]), failure_mask=torch.tensor([False, False]), @@ -1169,7 +1491,7 @@ def mismatched_effect_result( context: PlanningContext, request: EffectVerificationRequest, ) -> EffectVerificationResult: - return EffectVerificationResult( + return _effect_result( verification_id=request.verification_id + 1, success_mask=torch.ones(context.batch_size, dtype=torch.bool), failure_mask=torch.zeros(context.batch_size, dtype=torch.bool), diff --git a/tests/sim/skills/test_compiler.py b/tests/sim/skills/test_compiler.py index fdac46a71..6a3dc5a28 100644 --- a/tests/sim/skills/test_compiler.py +++ b/tests/sim/skills/test_compiler.py @@ -70,6 +70,7 @@ GroundedSemanticCall, HandOverPoseProvider, HandOverPoseTargets, + HeldObjectGuardBaseline, RegisteredSemanticLowerer, RelationTargetGrounder, SemanticLowering, @@ -149,7 +150,7 @@ def _preset( registered: bool = False, **kwargs: object, ) -> SkillPolicyPreset: - """Build one complete schema-v2 test preset.""" + """Build one complete schema-v3 test preset.""" kwargs.setdefault( "action_option_templates", _action_option_templates(registered=registered), @@ -751,6 +752,28 @@ def test_pick_effect_spec_binds_destination_and_fresh_monitor_per_grounding() -> assert revised.effect_spec is not None assert revised.effect_spec.invocation_revision == 1 assert revised.effect_monitor.spec.invocation_revision == 1 + assert len(first.effect_guards) == 1 + guard = first.effect_guards[0] + assert guard.guard_id == "destination_attached" + assert guard.active_segments == ("lift",) + assert guard.baseline is HeldObjectGuardBaseline.PLANNED_EFFECT + assert guard.task_state_key == "manipulator" + assert guard.invalidation_task_state_keys == ("manipulator",) + assert guard.retry_action is True + 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: @@ -802,6 +825,31 @@ def test_place_effect_spec_binds_source_and_verified_detach_baseline() -> None: ) assert isinstance(constraint, BinaryEffectClause) assert constraint.expected is False + assert len(grounded.effect_guards) == 1 + guard = grounded.effect_guards[0] + assert guard.guard_id == "source_attached" + assert guard.active_segments == ("approach",) + assert guard.baseline is HeldObjectGuardBaseline.VERIFIED_TASK_STATE + assert guard.task_state_key == "manipulator" + assert guard.invalidation_task_state_keys == ("manipulator",) + assert guard.retry_action is False + guard_pose, guard_constraint = guard.effect_spec.clauses + assert isinstance(guard_pose, PoseRelationClause) + assert guard_pose.expectation is PoseRelationExpectation.MATCHED + 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: @@ -876,6 +924,37 @@ def test_handover_effect_spec_binds_source_and_destination_relations() -> None: assert destination_pose.baseline_object_to_endpoint is None assert isinstance(destination_constraint, BinaryEffectClause) assert destination_constraint.expected is True + assert tuple(guard.guard_id for guard in grounded.effect_guards) == ( + "source_attached", + "destination_attached", + ) + source_guard, destination_guard = grounded.effect_guards + assert source_guard.active_segments == ( + "transfer", + "approach", + "close", + "hold", + ) + assert source_guard.baseline is HeldObjectGuardBaseline.VERIFIED_TASK_STATE + assert source_guard.task_state_key == "left" + assert source_guard.invalidation_task_state_keys == ("left",) + assert source_guard.retry_action is False + assert destination_guard.active_segments == ("release", "deliver") + assert destination_guard.baseline is HeldObjectGuardBaseline.PLANNED_EFFECT + 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: @@ -907,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_effects.py b/tests/sim/skills/test_effects.py index 3794dad03..72e479ea4 100644 --- a/tests/sim/skills/test_effects.py +++ b/tests/sim/skills/test_effects.py @@ -49,6 +49,7 @@ EffectEvidenceAddress, EffectEvidenceBatch, EffectEvidenceSourceRef, + EffectExpectationDecision, EffectMonitor, EffectMonitorDecision, EffectMonitorFactory, @@ -72,8 +73,13 @@ _ENV_IDS = torch.tensor([101, 205, 309], dtype=torch.long) _OBJECT_ID = "scene/cube" _STATE_KEY = "left_actor" +_SOURCE_STATE_KEY = "source_actor" +_DESTINATION_STATE_KEY = "destination_actor" _SKILL_ID = "pick_up" _INVOCATION_ID = "call-7" +_ATTACHED_OFFSET = 0.0 +_DETACHED_OFFSET = 0.1 # Above the built-in 0.06 translation threshold. +_UNRESOLVED_OFFSET = 0.04 # Between the attached and detached thresholds. @dataclass(frozen=True, slots=True) @@ -178,6 +184,120 @@ def _attach_spec() -> SemanticEffectSpec: ) +def _transfer_spec() -> SemanticEffectSpec: + source = _expectation( + HeldObjectRelation.DETACHED, + expectation_id="source", + state_key=_SOURCE_STATE_KEY, + ) + destination = _expectation( + expectation_id="destination", + state_key=_DESTINATION_STATE_KEY, + ) + return SemanticEffectSpec( + semantic_id="hand_over", + effect_kind=SemanticEffectKind.TRANSFER, + skill_id=_SKILL_ID, + invocation_id=_INVOCATION_ID, + invocation_revision=2, + env_ids=_ENV_IDS, + state_expectations=(source, destination), + clauses=( + PoseRelationClause( + "source.pose", + "source", + _source("source_pose_relation"), + PoseRelationExpectation.SEPARATED, + baseline_object_to_endpoint=_poses(0.0, 0.0, 0.0), + ), + BinaryEffectClause( + "source.constraint", + "source", + _source("source_constraint"), + BinaryEvidenceKind.CONSTRAINT, + False, + ), + PoseRelationClause( + "destination.pose", + "destination", + _source("destination_pose_relation"), + PoseRelationExpectation.MATCHED, + ), + BinaryEffectClause( + "destination.constraint", + "destination", + _source("destination_constraint"), + BinaryEvidenceKind.CONSTRAINT, + True, + ), + ), + ) + + +def _transfer_request() -> EffectVerificationRequest: + return _request( + effects=StateDelta( + held_object_updates={ + _SOURCE_STATE_KEY: None, + _DESTINATION_STATE_KEY: _held(), + } + ) + ) + + +def _transfer_evidence( + *, + source_offsets: tuple[float, ...], + source_constraints: tuple[bool, ...], + destination_offsets: tuple[float, ...], + destination_constraints: tuple[bool, ...], + timestamp: float, + revision: int, +) -> Mapping[str, EffectEvidenceBatch]: + valid = torch.ones(len(source_offsets), dtype=torch.bool) + errors = tuple(None for _ in source_offsets) + return { + "source.pose": PoseRelationEvidenceBatch( + "source.pose", + _poses(*source_offsets), + valid, + errors, + timestamp, + _ENV_IDS, + revision, + ), + "source.constraint": BinaryEffectEvidenceBatch( + "source.constraint", + BinaryEvidenceKind.CONSTRAINT, + torch.tensor(source_constraints, dtype=torch.bool), + valid, + errors, + timestamp, + _ENV_IDS, + revision, + ), + "destination.pose": PoseRelationEvidenceBatch( + "destination.pose", + _poses(*destination_offsets), + valid, + errors, + timestamp, + _ENV_IDS, + revision, + ), + "destination.constraint": BinaryEffectEvidenceBatch( + "destination.constraint", + BinaryEvidenceKind.CONSTRAINT, + torch.tensor(destination_constraints, dtype=torch.bool), + valid, + errors, + timestamp, + _ENV_IDS, + revision, + ), + } + + def _request( *, env_mask: torch.Tensor | None = None, @@ -544,6 +664,183 @@ def test_valid_raw_evidence_rejects_nonfinite_payload() -> None: ) +def test_expectation_decision_owns_all_outcome_masks() -> None: + satisfied = torch.tensor([True, False, False]) + contradicted = torch.tensor([False, True, False]) + inverse_satisfied = torch.tensor([False, True, False]) + + decision = EffectExpectationDecision( + "source", + satisfied, + contradicted, + inverse_satisfied, + ) + satisfied.zero_() + contradicted.zero_() + inverse_satisfied.zero_() + + assert decision.satisfied_mask.tolist() == [True, False, False] + assert decision.contradicted_mask.tolist() == [False, True, False] + assert decision.inverse_satisfied_mask.tolist() == [False, True, False] + + aggregate = EffectMonitorDecision( + decision.satisfied_mask, + decision.contradicted_mask, + (decision,), + ) + decision.satisfied_mask.zero_() + assert aggregate.expectation_decisions[0].satisfied_mask.tolist() == [ + True, + False, + False, + ] + + +def test_expectation_decision_requires_complete_inverse_to_be_contradicted() -> None: + with pytest.raises(ValueError, match="subset of contradicted_mask"): + EffectExpectationDecision( + "source", + torch.tensor([False]), + torch.tensor([False]), + torch.tensor([True]), + ) + + +def test_transfer_monitor_reports_each_expectation_and_strong_inverse() -> None: + monitor = CompositeEffectMonitor( + _transfer_spec(), + CompositeEffectMonitorCfg(consecutive_samples=1), + ) + + decision = monitor.observe( + _transfer_request(), + _transfer_evidence( + source_offsets=( + _DETACHED_OFFSET, + _ATTACHED_OFFSET, + _DETACHED_OFFSET, + ), + source_constraints=(False, True, False), + destination_offsets=( + _ATTACHED_OFFSET, + _ATTACHED_OFFSET, + _DETACHED_OFFSET, + ), + destination_constraints=(True, True, False), + timestamp=2.0, + revision=4, + ), + ) + + outcomes = { + outcome.expectation_id: outcome for outcome in decision.expectation_decisions + } + assert tuple(outcomes) == ("source", "destination") + assert outcomes["source"].satisfied_mask.tolist() == [True, False, True] + assert outcomes["source"].contradicted_mask.tolist() == [False, True, False] + assert outcomes["source"].inverse_satisfied_mask.tolist() == [False, True, False] + assert outcomes["destination"].satisfied_mask.tolist() == [True, True, False] + assert outcomes["destination"].contradicted_mask.tolist() == [False, False, True] + assert outcomes["destination"].inverse_satisfied_mask.tolist() == [ + False, + False, + True, + ] + assert decision.success_mask.tolist() == [True, False, False] + assert decision.failure_mask.tolist() == [False, True, True] + + +def test_transfer_contradictions_are_counted_per_expectation() -> None: + monitor = CompositeEffectMonitor( + _transfer_spec(), + CompositeEffectMonitorCfg(consecutive_samples=2), + ) + request = _transfer_request() + monitor.observe( + request, + _transfer_evidence( + source_offsets=(_ATTACHED_OFFSET,) * 3, + source_constraints=(True,) * 3, + destination_offsets=(_ATTACHED_OFFSET,) * 3, + destination_constraints=(True,) * 3, + timestamp=2.0, + revision=4, + ), + ) + + alternating = monitor.observe( + request, + _transfer_evidence( + source_offsets=(_DETACHED_OFFSET,) * 3, + source_constraints=(False,) * 3, + destination_offsets=(_DETACHED_OFFSET,) * 3, + destination_constraints=(False,) * 3, + timestamp=3.0, + revision=5, + ), + ) + persistent = monitor.observe( + request, + _transfer_evidence( + source_offsets=(_DETACHED_OFFSET,) * 3, + source_constraints=(False,) * 3, + destination_offsets=(_DETACHED_OFFSET,) * 3, + destination_constraints=(False,) * 3, + timestamp=4.0, + revision=6, + ), + ) + + assert not alternating.failure_mask.any() + assert persistent.failure_mask.all() + persistent_outcomes = { + outcome.expectation_id: outcome for outcome in persistent.expectation_decisions + } + assert persistent_outcomes["source"].satisfied_mask.all() + assert persistent_outcomes["destination"].contradicted_mask.all() + + +def test_transfer_success_never_stitches_expectations_across_ticks() -> None: + monitor = CompositeEffectMonitor( + _transfer_spec(), + CompositeEffectMonitorCfg(consecutive_samples=1), + ) + request = _transfer_request() + source_only = monitor.observe( + request, + _transfer_evidence( + source_offsets=(_DETACHED_OFFSET,) * 3, + source_constraints=(False,) * 3, + destination_offsets=(_UNRESOLVED_OFFSET,) * 3, + destination_constraints=(True,) * 3, + timestamp=2.0, + revision=4, + ), + ) + destination_only = monitor.observe( + request, + _transfer_evidence( + source_offsets=(_UNRESOLVED_OFFSET,) * 3, + source_constraints=(False,) * 3, + destination_offsets=(_ATTACHED_OFFSET,) * 3, + destination_constraints=(True,) * 3, + timestamp=3.0, + revision=5, + ), + ) + + assert not source_only.success_mask.any() + assert not source_only.failure_mask.any() + assert not destination_only.success_mask.any() + assert not destination_only.failure_mask.any() + destination_outcomes = { + outcome.expectation_id: outcome + for outcome in destination_only.expectation_decisions + } + assert not destination_outcomes["source"].satisfied_mask.any() + assert destination_outcomes["destination"].satisfied_mask.all() + + def test_monitor_requires_pose_and_binary_physical_evidence() -> None: monitor = CompositeEffectMonitor( _attach_spec(), @@ -561,6 +858,9 @@ def test_monitor_requires_pose_and_binary_physical_evidence() -> None: assert not pose_only.success_mask.any() assert pose_only.failure_mask.all() + outcome = pose_only.expectation_decisions[0] + assert outcome.contradicted_mask.all() + assert not outcome.inverse_satisfied_mask.any() def test_monitor_reports_success_only_for_complete_consecutive_evidence() -> None: diff --git a/tests/sim/skills/test_integration.py b/tests/sim/skills/test_integration.py index 1423fb138..350a1d463 100644 --- a/tests/sim/skills/test_integration.py +++ b/tests/sim/skills/test_integration.py @@ -61,6 +61,7 @@ RobotResource, RobotSkillProfile, SkillPolicyPreset, + WorkflowRecoveryPolicy, ) from embodichain.lab.sim.skills.scene import ( AmbiguousSceneAffordanceError, @@ -95,7 +96,7 @@ def _action_option_templates() -> dict[str, object]: def _preset(preset_id: str, **kwargs: object) -> SkillPolicyPreset: - """Build one complete schema-v2 test preset.""" + """Build one complete schema-v3 test preset.""" kwargs.setdefault("action_option_templates", _action_option_templates()) return SkillPolicyPreset(preset_id, **kwargs) @@ -715,6 +716,9 @@ def test_safe_preset_requires_dynamic_collision_for_dynamic_scene( strategy="motion_gen", dynamic_collision_mode=source_mode, ), + workflow_recovery_policy=WorkflowRecoveryPolicy( + max_recovery_attempts=2, + ), ), ) engine = _engine_for_integration( @@ -734,6 +738,7 @@ def test_safe_preset_requires_dynamic_collision_for_dynamic_scene( integration.robot_profile.presets["safe"].motion_policy.dynamic_collision_mode is source_mode ) + assert bound.preset.workflow_recovery_policy.max_recovery_attempts == 2 assert provider.calls == 0 diff --git a/tests/sim/skills/test_profiles.py b/tests/sim/skills/test_profiles.py index 1156786b3..55de8022f 100644 --- a/tests/sim/skills/test_profiles.py +++ b/tests/sim/skills/test_profiles.py @@ -87,6 +87,7 @@ RobotSkillProfile, SkillPolicyPreset, UnsupportedSkillError, + WorkflowRecoveryPolicy, ) _JOINT_IDS = { @@ -1429,7 +1430,7 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: second = bound.preset() assert first is not second - assert first.schema_version == 2 + assert first.schema_version == 3 assert first.motion_policy.sample_count == 80 assert first.tracking_policy is not second.tracking_policy assert first.action_option_templates["pick"] is not ( @@ -1450,8 +1451,8 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: bound.preset(skill_id="typo") with pytest.raises(KeyError, match="not an installed"): bound.preset("safe", skill_id="typo") - with pytest.raises(ValueError, match=r"supported versions are \[2\]"): - SkillPolicyPreset("legacy", action_option_templates={}, schema_version=1) + with pytest.raises(ValueError, match=r"supported versions are \[3\]"): + SkillPolicyPreset("legacy", action_option_templates={}, schema_version=2) incompatible = RobotSkillProfile( "bad_preset", @@ -1469,6 +1470,35 @@ def test_presets_are_versioned_snapshots_and_validate_planner() -> None: incompatible.bind(_engine(control_profiles=_command_profiles())) +def test_workflow_recovery_policy_is_bounded_and_snapshotted() -> None: + source = WorkflowRecoveryPolicy(max_recovery_attempts=2) + preset = SkillPolicyPreset( + "recovering", + action_option_templates={}, + workflow_recovery_policy=source, + ) + + first = preset.workflow_recovery_policy + second = preset.snapshot().workflow_recovery_policy + + assert first.max_recovery_attempts == 2 + assert second == first + assert first is not source + assert second is not first + assert ( + SkillPolicyPreset( + "disabled", action_option_templates={} + ).workflow_recovery_policy.max_recovery_attempts + == 0 + ) + for invalid in (True, 1.5, "2"): + with pytest.raises(TypeError, match="must be an integer"): + WorkflowRecoveryPolicy(invalid) # type: ignore[arg-type] + for invalid in (-1, 101): + with pytest.raises(ValueError, match=r"\[0, 100\]"): + WorkflowRecoveryPolicy(invalid) + + def test_policy_preset_defaults_exact_builtin_effect_monitor_refs() -> None: preset = SkillPolicyPreset("safe", action_option_templates={}) diff --git a/tests/sim/skills/test_runtime.py b/tests/sim/skills/test_runtime.py index 128f23d13..f77a2d764 100644 --- a/tests/sim/skills/test_runtime.py +++ b/tests/sim/skills/test_runtime.py @@ -29,9 +29,11 @@ import embodichain.lab.sim.skills.runtime as runtime_module from embodichain.lab.sim.atomic_actions import ( + ActionBinding, ActionInvocation, ActionOptions, ActionPlan, + Affordance, ArticulationJointState, AtomicAction, AtomicActionEngine, @@ -41,14 +43,21 @@ EndpointBinding, EndpointTrackingChannelBinding, EndpointTrackingFeedbackAddress, + ExecutionEventKind, + HeldObjectGuardRequest, + HeldObjectState, JointPositionTarget, MotionPolicy, + ObjectSemantics, + PhaseEffectGateRequest, PlanningContext, RecoveryPolicy, ResolvedActionRequest, RobotObservation, SceneSnapshot, SkillBindingContract, + SkillEndpointRequirement, + SkillResourceSlot, StateDelta, TaskState, TimedCommandSequence, @@ -56,15 +65,32 @@ TrackingProjectorRef, ) from embodichain.lab.sim.atomic_actions.tracking import TrackingPolicy -from embodichain.lab.sim.skills.calls import RegisteredSemanticCall -from embodichain.lab.sim.skills.compiler import SemanticSkillCompiler +from embodichain.lab.sim.skills.calls import ( + HandOver, + Pick, + Place, + RegisteredSemanticCall, + SemanticCallSpec, +) +from embodichain.lab.sim.skills.compiler import ( + GroundedHeldObjectGuard, + GroundedPhaseEffectGate, + HeldObjectGuardBaseline, + SemanticSkillCompiler, +) from embodichain.lab.sim.skills.effects import ( ArticulationJointStateExpectation, + BinaryEffectClause, + BinaryEvidenceKind, + CONSTRAINT_EFFECT_CHANNEL, ControlPartEvidenceAddress, EffectEvidenceBatch, EffectEvidenceSourceRef, + EffectExpectationDecision, EffectMonitor, EffectMonitorDecision, + HeldObjectRelation, + HeldObjectStateExpectation, JOINT_STATE_EFFECT_CHANNEL, JointStateEffectClause, SemanticEffectKind, @@ -75,11 +101,15 @@ SkillEndpointBindingTrace, SkillRuntime, SkillStatus, + SkillWorkflowRecoveryRole, ) from embodichain.lab.sim.skills.parallel import ParallelTimingPolicy from embodichain.lab.sim.skills.parallel_runtime import ParallelSkillRuntime -from embodichain.lab.sim.skills.profiles import ResourceClaim -from embodichain.lab.sim.skills.scene import SceneRegistry +from embodichain.lab.sim.skills.profiles import ( + ResourceClaim, + WorkflowRecoveryPolicy, +) +from embodichain.lab.sim.skills.scene import SceneObjectRef, SceneRegistry BATCH_SIZE = 2 @@ -201,6 +231,7 @@ def observe( return EffectMonitorDecision( self._decision.success_mask, self._decision.failure_mask, + self._decision.expectation_decisions, ) @@ -261,10 +292,131 @@ def _plan( ) +@dataclass(frozen=True, slots=True) +class _WorkflowEffectGoal: + """Test-only held-object effect for workflow recovery.""" + + goal_kind: ClassVar[str] = "runtime_test_workflow_effect" + + object_id: str + attach: bool + + def __post_init__(self) -> None: + if type(self.object_id) is not str or not self.object_id: + raise ValueError("object_id must be a non-empty string.") + if type(self.attach) is not bool: + raise TypeError("attach must be exactly bool.") + + +class _WorkflowEffectAction(AtomicAction[_WorkflowEffectGoal, ActionOptions]): + """Zero-frame action that commits only a verified held-object effect.""" + + skill_id: ClassVar[str] = "runtime_test_workflow_primary" + GoalType: ClassVar[type] = _WorkflowEffectGoal + source_slot: ClassVar[str] = "primary" + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="primary", + endpoints=(SkillEndpointRequirement(endpoint_id="motion"),), + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[_WorkflowEffectGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + endpoint = request.binding.endpoint(self.source_slot, "motion") + task_state_key = endpoint.task_state_key + assert task_state_key is not None + if goal.attach: + poses = ( + torch.eye( + 4, + dtype=context.robot.qpos.dtype, + device=context.robot.qpos.device, + ) + .unsqueeze(0) + .repeat(context.batch_size, 1, 1) + ) + effect: HeldObjectState | None = HeldObjectState( + semantics=ObjectSemantics( + affordance=Affordance(), + geometry={}, + label=goal.object_id, + entity_id=goal.object_id, + ), + object_to_eef=poses, + grasp_xpos=poses, + env_mask=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + ) + else: + effect = None + return self.build_command_plan( + request, + context, + success=torch.ones( + context.batch_size, + dtype=torch.bool, + device=context.robot.qpos.device, + ), + commands=TimedCommandSequence((), context.env_ids), + expected_effects=StateDelta( + held_object_updates={task_state_key: effect}, + ), + effect_verification=EffectVerificationRequirement("semantic_effect"), + replannable=False, + ) + + +class _WorkflowSourceEffectAction(_WorkflowEffectAction): + """Held-object effect addressed through a hand-over source slot.""" + + skill_id: ClassVar[str] = "runtime_test_workflow_source" + source_slot: ClassVar[str] = "source" + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + SkillResourceSlot( + slot_id="source", + endpoints=(SkillEndpointRequirement(endpoint_id="motion"),), + ), + ) + ) + + +@dataclass(frozen=True, slots=True, eq=False) +class _WorkflowEffectDecision: + """One queued physical-effect result for a grounded recovery call.""" + + success_mask: torch.Tensor + failure_mask: torch.Tensor + inverse_satisfied_mask: torch.Tensor + + def __post_init__(self) -> None: + for name in ( + "success_mask", + "failure_mask", + "inverse_satisfied_mask", + ): + value = getattr(self, name) + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor.") + if value.dtype != torch.bool or value.shape != (BATCH_SIZE,): + raise ValueError(f"{name} must be bool with shape ({BATCH_SIZE},).") + object.__setattr__(self, name, value.clone()) + + @dataclass(frozen=True, slots=True) class _Workflow: workflow_id: str - calls: tuple[RegisteredSemanticCall, ...] + calls: tuple[SemanticCallSpec, ...] @dataclass(frozen=True, slots=True) @@ -307,7 +459,7 @@ def integration(self) -> _Integration: def analyze( self, - calls: tuple[RegisteredSemanticCall, ...], + calls: tuple[SemanticCallSpec, ...], *, workflow_id: str = "semantic_workflow", path: tuple[object, ...] = ("workflow",), @@ -415,6 +567,201 @@ def ground( ) +class _WorkflowRecoveryCompiler(SemanticSkillCompiler): + """Ground queued physical outcomes through real execution sessions.""" + + def __init__( + self, + engine: AtomicActionEngine, + decisions: tuple[_WorkflowEffectDecision, ...], + *, + max_recovery_attempts: int, + ) -> None: + self._test_integration = _Integration(engine, SceneRegistry()) + self._decisions = decisions + self._workflow_policy = WorkflowRecoveryPolicy(max_recovery_attempts) + self.analysis_windows: list[tuple[str, ...]] = [] + self.grounded_calls: list[SemanticCallSpec] = [] + self.grounded_masks: list[torch.Tensor] = [] + self.invocations: list[ActionInvocation] = [] + + @property + def integration(self) -> _Integration: + return self._test_integration + + def analyze( + self, + calls: tuple[SemanticCallSpec, ...], + *, + workflow_id: str = "semantic_workflow", + path: tuple[object, ...] = ("workflow",), + ) -> _Workflow: + del path + self.analysis_windows.append(tuple(call.semantic_id for call in calls)) + return _Workflow(workflow_id, tuple(calls)) + + def ground( + self, + workflow: _Workflow, + call_index: int, + context: PlanningContext, + *, + eligible_mask: torch.Tensor | None = None, + revision: int = 0, + path: tuple[object, ...] = ("workflow",), + ) -> _Grounded: + del path + if eligible_mask is None: + raise ValueError("eligible_mask is required by this compiler.") + decision_index = len(self.grounded_calls) + if decision_index >= len(self._decisions): + raise RuntimeError("No queued workflow-effect decision remains.") + decision = self._decisions[decision_index] + call = workflow.calls[call_index] + if type(call) is Pick: + action_type = _WorkflowEffectAction + source_slot = "primary" + expectation_id = "destination" + relation = HeldObjectRelation.ATTACHED + effect_kind = SemanticEffectKind.ATTACH + attach = True + expected_binary = True + elif type(call) is Place: + action_type = _WorkflowEffectAction + source_slot = "primary" + expectation_id = "source" + relation = HeldObjectRelation.DETACHED + effect_kind = SemanticEffectKind.RELEASE + attach = False + expected_binary = False + elif type(call) is HandOver: + action_type = _WorkflowSourceEffectAction + source_slot = "source" + expectation_id = "source" + relation = HeldObjectRelation.DETACHED + effect_kind = SemanticEffectKind.RELEASE + attach = False + expected_binary = False + else: + raise TypeError( + "Workflow-recovery test compiler accepts Pick, Place, or HandOver." + ) + object_id = call.object.entity_id + binding = ActionBinding( + owner_id=self.integration.engine.binding_owner_id, + endpoints=( + EndpointBinding( + slot_id=source_slot, + endpoint_id="motion", + resource_id="left_actor", + adapter_id="test", + target=JointPositionTarget("virtual", (0,)), + task_state_key="left_gripper", + joint_ids=(0,), + ), + ), + ) + motion_policy = MotionPolicy( + planner="runtime_test", + sample_count=7, + control_dt=0.02, + velocity_limit=0.4, + acceleration_limit=0.8, + ) + tracking_policy = TrackingPolicy.timed() + recovery_policy = RecoveryPolicy( + max_replans=0, + max_action_retries=0, + action_timeout=100.0, + ) + invocation = ActionInvocation( + skill_id=action_type.skill_id, + goal=_WorkflowEffectGoal(object_id=object_id, attach=attach), + binding=binding, + motion_policy=motion_policy, + tracking_policy=tracking_policy, + recovery_policy=recovery_policy, + invocation_id=f"{workflow.workflow_id}:{decision_index}", + revision=revision, + ) + expectation = HeldObjectStateExpectation( + expectation_id=expectation_id, + relation=relation, + object_id=object_id, + slot_id=source_slot, + resource_id="left_actor", + task_state_key="left_gripper", + ) + spec = SemanticEffectSpec( + semantic_id=call.semantic_id, + effect_kind=effect_kind, + skill_id=invocation.skill_id, + invocation_id=invocation.invocation_id, + invocation_revision=invocation.revision, + env_ids=context.env_ids, + state_expectations=(expectation,), + clauses=( + BinaryEffectClause( + clause_id=f"{expectation_id}.constraint", + expectation_id=expectation_id, + source=EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress( + "virtual", + CONSTRAINT_EFFECT_CHANNEL, + ), + ), + evidence_kind=BinaryEvidenceKind.CONSTRAINT, + expected=expected_binary, + ), + ), + ) + monitor = _DecisionMonitor( + spec, + EffectMonitorDecision( + success_mask=decision.success_mask, + failure_mask=decision.failure_mask, + expectation_decisions=( + EffectExpectationDecision( + expectation_id=expectation_id, + satisfied_mask=decision.success_mask, + contradicted_mask=decision.failure_mask, + inverse_satisfied_mask=decision.inverse_satisfied_mask, + ), + ), + ), + ) + analyzed = SimpleNamespace( + call=call, + bound=SimpleNamespace( + robot_profile=SimpleNamespace(profile_id="runtime_test_profile"), + binding=SimpleNamespace(action_binding=binding), + linked=SimpleNamespace( + descriptor=SimpleNamespace(skill_id=invocation.skill_id) + ), + preset=SimpleNamespace( + preset_id="runtime_test_recovery_preset", + schema_version=3, + motion_policy=motion_policy, + tracking_policy=tracking_policy, + recovery_policy=recovery_policy, + workflow_recovery_policy=self._workflow_policy, + ), + ), + ) + self.grounded_calls.append(call) + self.grounded_masks.append(eligible_mask.clone()) + self.invocations.append(invocation) + return _Grounded( + analyzed=analyzed, + invocation=invocation, + effect_spec=spec, + effect_monitor=monitor, + eligible_mask=eligible_mask.clone(), + ) + + @dataclass(slots=True) class _System: runtime: SkillRuntime @@ -427,10 +774,38 @@ class _System: clock: _Clock +@dataclass(slots=True) +class _WorkflowRecoverySystem: + runtime: SkillRuntime + compiler: _WorkflowRecoveryCompiler + engine: AtomicActionEngine + observation: _ObservationProvider + sink: _CommandSink + collector: _Collector + clock: _Clock + + def _mask(*values: bool) -> torch.Tensor: return torch.tensor(values, dtype=torch.bool) +def _workflow_decision( + success_mask: torch.Tensor, + failure_mask: torch.Tensor, + *, + inverse_satisfied_mask: torch.Tensor | None = None, +) -> _WorkflowEffectDecision: + return _WorkflowEffectDecision( + success_mask=success_mask, + failure_mask=failure_mask, + inverse_satisfied_mask=( + torch.zeros_like(failure_mask) + if inverse_satisfied_mask is None + else inverse_satisfied_mask + ), + ) + + def _call(name: str) -> RegisteredSemanticCall: return RegisteredSemanticCall(call_id=f"test.{name}") @@ -479,6 +854,55 @@ def _system( ) +def _workflow_recovery_system( + decisions: tuple[_WorkflowEffectDecision, ...], + *, + max_recovery_attempts: int = 2, + task_state: TaskState | None = None, +) -> _WorkflowRecoverySystem: + robot = Mock() + robot.device = torch.device("cpu") + robot.dof = 1 + robot.control_parts = {} + robot.get_qpos.return_value = torch.zeros(BATCH_SIZE, 1) + robot.get_qvel.return_value = torch.zeros(BATCH_SIZE, 1) + generator = Mock() + generator.robot = robot + generator.device = torch.device("cpu") + generator.planner.cfg.planner_type = "runtime_test" + engine = AtomicActionEngine(generator, load_builtins=False) + engine.register(_WorkflowEffectAction()) + engine.register(_WorkflowSourceEffectAction()) + compiler = _WorkflowRecoveryCompiler( + engine, + decisions, + max_recovery_attempts=max_recovery_attempts, + ) + observation = _ObservationProvider() + sink = _CommandSink() + collector = _Collector() + clock = _Clock() + runtime = SkillRuntime.from_components( + compiler, + observation, + sink, + collector, + task_state=( + TaskState.empty(BATCH_SIZE, "cpu") if task_state is None else task_state + ), + clock=clock, + ) + return _WorkflowRecoverySystem( + runtime=runtime, + compiler=compiler, + engine=engine, + observation=observation, + sink=sink, + collector=collector, + clock=clock, + ) + + def test_runtime_analyzes_once_and_uses_one_fresh_session_per_call( monkeypatch: pytest.MonkeyPatch, ) -> None: @@ -584,6 +1008,345 @@ def test_runtime_keeps_partial_rows_at_the_shared_call_barrier() -> None: assert len(result.failures) == 1 +def test_runtime_reacquires_a_lost_source_with_a_real_pick_then_retries() -> None: + system = _workflow_recovery_system( + ( + _workflow_decision(_mask(True, True), _mask(False, False)), + _workflow_decision(_mask(True, False), _mask(False, True)), + _workflow_decision(_mask(False, True), _mask(False, False)), + _workflow_decision(_mask(False, True), _mask(False, False)), + ) + ) + cube = SceneObjectRef("cube") + + result = system.runtime.run( + Pick(object=cube), + Place(object=cube, inside=SceneObjectRef("bin")), + ) + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.success_mask, _mask(True, True)) + assert torch.equal(result.failure_mask, _mask(False, False)) + assert len(result.calls) == 2 + assert [call.semantic_id for call in system.compiler.grounded_calls] == [ + "pick", + "place", + "pick", + "place", + ] + assert [mask.tolist() for mask in system.compiler.grounded_masks] == [ + [True, True], + [True, True], + [False, True], + [False, True], + ] + assert system.compiler.analysis_windows == [ + ("pick", "place"), + ("pick", "place"), + ("place",), + ] + assert [trace.role for trace in result.workflow_recoveries] == [ + SkillWorkflowRecoveryRole.REACQUIRE, + SkillWorkflowRecoveryRole.RETRY_REACQUIRED, + ] + assert all( + torch.equal(trace.entered_mask, _mask(False, True)) + for trace in result.workflow_recoveries + ) + assert result.workflow_recoveries[0].call is not None + assert result.workflow_recoveries[0].call.semantic_id == "pick" + assert result.workflow_recoveries[1].call is not None + assert result.workflow_recoveries[1].call.semantic_id == "place" + assert any( + event.kind is ExecutionEventKind.RECOVERY_REQUIRED + and torch.equal(event.env_mask, _mask(False, True)) + for event in result.events + ) + metadata = result.to_metadata() + json.dumps(metadata, allow_nan=False, sort_keys=True) + assert [entry["role"] for entry in metadata["workflow_recoveries"]] == [ + "reacquire", + "retry_reacquired", + ] + assert metadata["workflow_recoveries"][0]["source_resource_id"] == "left_actor" + assert metadata["workflow_recoveries"][0]["source_task_state_key"] == ( + "left_gripper" + ) + assert result.task_state.get_held_object("left_gripper") is None + + +def test_runtime_retries_directly_when_verified_source_relation_remains() -> None: + poses = torch.eye(4).unsqueeze(0).repeat(BATCH_SIZE, 1, 1) + initial_state = TaskState( + batch_size=BATCH_SIZE, + device="cpu", + held_objects={ + "left_gripper": HeldObjectState( + semantics=ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="cube", + entity_id="cube", + ), + object_to_eef=poses, + grasp_xpos=poses, + env_mask=_mask(True, True), + ) + }, + ) + system = _workflow_recovery_system( + ( + _workflow_decision( + _mask(True, False), + _mask(False, True), + inverse_satisfied_mask=_mask(False, True), + ), + _workflow_decision(_mask(False, True), _mask(False, False)), + ), + task_state=initial_state, + ) + + result = system.runtime.run( + HandOver( + object=SceneObjectRef("cube"), + receiver="right_actor", + resources={"source": "left_actor"}, + ) + ) + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.success_mask, _mask(True, True)) + assert [call.semantic_id for call in system.compiler.grounded_calls] == [ + "hand_over", + "hand_over", + ] + assert [mask.tolist() for mask in system.compiler.grounded_masks] == [ + [True, True], + [False, True], + ] + assert len(result.workflow_recoveries) == 1 + recovery = result.workflow_recoveries[0] + assert recovery.role is SkillWorkflowRecoveryRole.RETRY_RETAINED + assert recovery.attempt_index == 1 + assert torch.equal(recovery.entered_mask, _mask(False, True)) + assert result.task_state.get_held_object("left_gripper") is None + + +def test_runtime_partitions_retained_and_lost_source_rows_in_one_barrier() -> None: + poses = torch.eye(4).unsqueeze(0).repeat(BATCH_SIZE, 1, 1) + initial_state = TaskState( + batch_size=BATCH_SIZE, + device="cpu", + held_objects={ + "left_gripper": HeldObjectState( + semantics=ObjectSemantics( + affordance=Affordance(), + geometry={}, + label="cube", + entity_id="cube", + ), + object_to_eef=poses, + grasp_xpos=poses, + env_mask=_mask(True, True), + ) + }, + ) + system = _workflow_recovery_system( + ( + _workflow_decision( + _mask(False, False), + _mask(True, True), + inverse_satisfied_mask=_mask(True, False), + ), + _workflow_decision(_mask(True, False), _mask(False, False)), + _workflow_decision(_mask(False, True), _mask(False, False)), + _workflow_decision(_mask(False, True), _mask(False, False)), + ), + task_state=initial_state, + ) + + result = system.runtime.run( + HandOver( + object=SceneObjectRef("cube"), + receiver="right_actor", + resources={"source": "left_actor"}, + ) + ) + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.success_mask, _mask(True, True)) + assert [call.semantic_id for call in system.compiler.grounded_calls] == [ + "hand_over", + "hand_over", + "pick", + "hand_over", + ] + assert [mask.tolist() for mask in system.compiler.grounded_masks] == [ + [True, True], + [True, False], + [False, True], + [False, True], + ] + assert [trace.role for trace in result.workflow_recoveries] == [ + SkillWorkflowRecoveryRole.RETRY_RETAINED, + SkillWorkflowRecoveryRole.REACQUIRE, + SkillWorkflowRecoveryRole.RETRY_REACQUIRED, + ] + assert [trace.attempt_index for trace in result.workflow_recoveries] == [1, 1, 1] + assert result.task_state.get_held_object("left_gripper") is None + + +def test_runtime_bounds_reacquisition_attempts_per_failed_row() -> None: + system = _workflow_recovery_system( + ( + _workflow_decision(_mask(True, True), _mask(False, False)), + _workflow_decision(_mask(True, False), _mask(False, True)), + _workflow_decision(_mask(False, False), _mask(False, True)), + _workflow_decision(_mask(False, False), _mask(False, True)), + ), + max_recovery_attempts=2, + ) + cube = SceneObjectRef("cube") + + result = system.runtime.run( + Pick(object=cube), + Place(object=cube, inside=SceneObjectRef("bin")), + ) + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.success_mask, _mask(True, False)) + assert torch.equal(result.failure_mask, _mask(False, True)) + assert [call.semantic_id for call in system.compiler.grounded_calls] == [ + "pick", + "place", + "pick", + "pick", + ] + assert [trace.role for trace in result.workflow_recoveries] == [ + SkillWorkflowRecoveryRole.REACQUIRE, + SkillWorkflowRecoveryRole.REACQUIRE, + ] + assert [trace.attempt_index for trace in result.workflow_recoveries] == [1, 2] + assert len(result.failures) == 1 + assert "exhausted" in result.failures[0].message + + +def test_runtime_leaves_external_recovery_disabled_at_zero_budget() -> None: + system = _workflow_recovery_system( + ( + _workflow_decision(_mask(True, True), _mask(False, False)), + _workflow_decision(_mask(True, False), _mask(False, True)), + ), + max_recovery_attempts=0, + ) + cube = SceneObjectRef("cube") + + result = system.runtime.run( + Pick(object=cube), + Place(object=cube, inside=SceneObjectRef("bin")), + ) + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.success_mask, _mask(True, False)) + assert torch.equal(result.failure_mask, _mask(False, True)) + assert result.workflow_recoveries == () + assert [call.semantic_id for call in system.compiler.grounded_calls] == [ + "pick", + "place", + ] + + +def test_runtime_resolves_workflow_policy_only_after_typed_core_handoff() -> None: + system = _workflow_recovery_system( + ( + _workflow_decision(_mask(True, True), _mask(False, False)), + _workflow_decision(_mask(True, True), _mask(False, False)), + ) + ) + system.compiler._workflow_policy = object() # type: ignore[assignment] + cube = SceneObjectRef("cube") + + result = system.runtime.run( + Pick(object=cube), + Place(object=cube, inside=SceneObjectRef("bin")), + ) + + assert result.status is SkillStatus.COMPLETED + assert result.workflow_recoveries == () + + +def test_cancel_during_reacquisition_safe_stops_every_barrier_row() -> None: + system = _workflow_recovery_system( + ( + _workflow_decision(_mask(True, True), _mask(False, False)), + _workflow_decision(_mask(True, False), _mask(False, True)), + _workflow_decision(_mask(False, True), _mask(False, False)), + ) + ) + cube = SceneObjectRef("cube") + result = system.runtime.start( + Pick(object=cube), + Place(object=cube, inside=SceneObjectRef("bin")), + ) + while len(system.compiler.grounded_calls) < 3: + if result.wait_duration: + system.clock.sleep(result.wait_duration) + result = system.runtime.step() + + held_before_cancel = system.sink.held + result = system.runtime.cancel("caller stopped recovery") + + assert result.status is SkillStatus.CANCELLED + assert torch.equal(result.cancelled_mask, _mask(True, True)) + assert len(result.workflow_recoveries) == 1 + assert result.workflow_recoveries[0].role is SkillWorkflowRecoveryRole.REACQUIRE + assert result.workflow_recoveries[0].call is not None + assert ( + result.workflow_recoveries[0].call.status + is runtime_module.RunnerStatus.CANCELLED + ) + assert system.sink.cancelled == 1 + assert system.sink.held == held_before_cancel + 1 + + +def test_deactivating_a_waiting_row_does_not_cancel_active_reacquisition() -> None: + system = _workflow_recovery_system( + ( + _workflow_decision(_mask(True, True), _mask(False, False)), + _workflow_decision(_mask(True, False), _mask(False, True)), + _workflow_decision(_mask(False, True), _mask(False, False)), + _workflow_decision(_mask(False, True), _mask(False, False)), + ) + ) + cube = SceneObjectRef("cube") + result = system.runtime.start( + Pick(object=cube), + Place(object=cube, inside=SceneObjectRef("bin")), + ) + while len(system.compiler.grounded_calls) < 3: + if result.wait_duration: + system.clock.sleep(result.wait_duration) + result = system.runtime.step() + + result = system.runtime.deactivate_rows( + _mask(True, False), + reason="parallel peer failed", + ) + while not result.terminal: + if result.wait_duration: + system.clock.sleep(result.wait_duration) + result = system.runtime.step() + + assert result.status is SkillStatus.COMPLETED + assert torch.equal(result.cancelled_mask, _mask(True, False)) + assert torch.equal(result.success_mask, _mask(False, True)) + assert torch.equal(result.failure_mask, _mask(False, False)) + assert [trace.role for trace in result.workflow_recoveries] == [ + SkillWorkflowRecoveryRole.REACQUIRE, + SkillWorkflowRecoveryRole.RETRY_REACQUIRED, + ] + + def test_nonblocking_step_routes_effect_feedback_through_collector() -> None: system = _system((EffectMonitorDecision(_mask(True, True), _mask(False, False)),)) result = system.runtime.start(_call("stepwise")) @@ -602,6 +1365,305 @@ def test_nonblocking_step_routes_effect_feedback_through_collector() -> None: assert system.compiler.monitors[0].requests[0].verification_id == 0 +def test_runtime_preserves_per_expectation_effect_outcomes_in_trace() -> None: + expectation = EffectExpectationDecision( + expectation_id="joint_target", + satisfied_mask=_mask(True, True), + contradicted_mask=_mask(False, False), + inverse_satisfied_mask=_mask(False, False), + ) + system = _system( + ( + EffectMonitorDecision( + _mask(True, True), + _mask(False, False), + (expectation,), + ), + ) + ) + + result = system.runtime.run(_call("expectation_trace")) + + assert result.status is SkillStatus.COMPLETED + assert len(result.effects) == 1 + recorded = result.effects[0].expectation_decisions + assert len(recorded) == 1 + assert recorded[0].expectation_id == "joint_target" + assert result.to_metadata()["effects"][0]["decision"]["expectations"] == [ + { + "expectation_id": "joint_target", + "satisfied_mask": [True, True], + "contradicted_mask": [False, False], + "inverse_satisfied_mask": [False, False], + } + ] + + +@pytest.mark.parametrize( + ("call", "expected_invalidation", "expected_retry"), + ( + ( + Place( + object=SceneObjectRef("cube"), + inside=SceneObjectRef("bin"), + ), + _mask(False, True), + _mask(True, False), + ), + ( + HandOver(object=SceneObjectRef("cube")), + _mask(False, True), + _mask(False, False), + ), + ), +) +def test_terminal_failure_policy_only_retains_strongly_proven_source_attachment( + call: Place | HandOver, + expected_invalidation: torch.Tensor, + expected_retry: torch.Tensor, +) -> None: + failure = _mask(True, True) + source = EffectExpectationDecision( + expectation_id="source", + satisfied_mask=_mask(False, False), + contradicted_mask=failure, + inverse_satisfied_mask=_mask(True, False), + ) + destination = EffectExpectationDecision( + expectation_id="destination", + satisfied_mask=_mask(False, False), + contradicted_mask=failure, + inverse_satisfied_mask=_mask(False, False), + ) + grounded = SimpleNamespace(analyzed=SimpleNamespace(call=call)) + + invalidation, retry = SkillRuntime._terminal_failure_policy( + grounded, + failure, + (source, destination), + ) + + assert torch.equal(invalidation, expected_invalidation) + assert torch.equal(retry, expected_retry) + + +def test_in_flight_guard_collects_live_evidence_and_builds_loss_reconciliation() -> ( + 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), + ) + task_state = TaskState( + batch_size=BATCH_SIZE, + device="cpu", + held_objects={"arm": held}, + ) + expectation = HeldObjectStateExpectation( + expectation_id="source", + relation=HeldObjectRelation.ATTACHED, + object_id="cube", + slot_id="primary", + resource_id="arm", + task_state_key="arm", + ) + spec = SemanticEffectSpec( + semantic_id="carry", + effect_kind=SemanticEffectKind.ATTACH, + skill_id="carry", + invocation_id="workflow:0", + invocation_revision=0, + env_ids=torch.arange(BATCH_SIZE, dtype=torch.long), + state_expectations=(expectation,), + clauses=( + BinaryEffectClause( + clause_id="source.constraint", + expectation_id="source", + source=EffectEvidenceSourceRef( + "test.provider", + "1", + ControlPartEvidenceAddress("hand", "constraint"), + ), + evidence_kind=BinaryEvidenceKind.CONSTRAINT, + expected=True, + ), + ), + ) + monitor = _DecisionMonitor( + spec, + EffectMonitorDecision(_mask(False, True), _mask(True, False)), + ) + guard = GroundedHeldObjectGuard( + guard_id="source_attached", + active_segments=("carry",), + baseline=HeldObjectGuardBaseline.VERIFIED_TASK_STATE, + effect_spec=spec, + effect_monitor=monitor, + invalidation_task_state_keys=("arm",), + retry_action=False, + ) + system.runtime._grounded = SimpleNamespace( + analyzed=SimpleNamespace(effect_monitor_ref=None), + effect_guards=(guard,), + ) + system.runtime._runner = SimpleNamespace( + session=SimpleNamespace(task_state=task_state) + ) + system.runtime._current_call_index = 0 + context = system.observation.observe(task_state) + request = HeldObjectGuardRequest( + verification_id=0, + skill_id="carry", + invocation_id="workflow:0", + invocation_revision=0, + invocation_index=0, + attempt_generation=0, + next_waypoint_index=1, + segment_name="carry", + env_mask=_mask(True, True), + allowed_held_object_relations=(("arm", "cube"),), + allowed_coordinated_held_object_relations=(), + deadline=10.0, + ) + + result = system.runtime._held_object_guard_verifier(context, request) + + assert result is not None + assert torch.equal(result.failure_mask, _mask(True, False)) + assert torch.equal(result.retry_mask, _mask(False, False)) + assert result.state_invalidation.held_object_updates == {"arm": None} + assert len(system.runtime._effect_traces) == 1 + trace = system.runtime._effect_traces[0] + assert trace.boundary_kind == "in_flight_guard" + assert trace.guard_id == "source_attached" + assert trace.segment_name == "carry" + 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)),)) @@ -609,7 +1671,7 @@ def test_result_metadata_is_json_safe_and_contains_typed_runtime_trace() -> None metadata = result.to_metadata() json.dumps(metadata, allow_nan=False, sort_keys=True) - assert metadata["schema_version"] == 1 + assert metadata["schema_version"] == 2 assert metadata["kind"] == "skill_result" call = metadata["calls"][0] assert call["semantic_id"] == "test.metadata" @@ -645,9 +1707,11 @@ def test_result_metadata_is_json_safe_and_contains_typed_runtime_trace() -> None assert "feedback_mode" not in attempt assert result.calls[0].resolved_core_policy.preset_id == "runtime_test_preset" effect = call["effects"][0] + assert effect["boundary"] == {"kind": "terminal"} assert effect["effect_spec"]["semantic_id"] == "test.metadata" assert effect["monitor"]["monitor_id"].endswith("._DecisionMonitor") assert effect["evidence"] == {} + assert metadata["workflow_recoveries"] == [] metadata["masks"]["success"][0] = False assert system.runtime.result.to_metadata()["masks"]["success"] == [True, True] diff --git a/tests/sim/solvers/test_base_solver.py b/tests/sim/solvers/test_base_solver.py new file mode 100644 index 000000000..f2192b27f --- /dev/null +++ b/tests/sim/solvers/test_base_solver.py @@ -0,0 +1,155 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +from __future__ import annotations + +import numpy as np +import pytest + +from embodichain.lab.sim.cfg import RobotCfg +from embodichain.lab.sim.solvers import DifferentialSolverCfg, OPWSolverCfg, URSolverCfg + +UR5_DH_PARAMETERS = { + "d1": 0.089159, + "a2": -0.425, + "a3": -0.39225, + "d4": 0.10915, + "d5": 0.09465, + "d6": 0.0823, +} + + +def assert_ur5_dh_parameters(cfg: URSolverCfg) -> None: + """Assert that a UR solver config contains the UR5 DH parameters.""" + for field_name, expected_value in UR5_DH_PARAMETERS.items(): + assert getattr(cfg, field_name) == pytest.approx(expected_value) + + +def make_ur5_robot_dict() -> dict: + """Return a minimal robot dictionary with a nested UR5 solver config.""" + return { + "control_parts": {"arm": [f"joint_{index}" for index in range(6)]}, + "solver_cfg": { + "arm": { + "class_type": "URSolver", + "ur_type": "ur5", + "root_link_name": "base", + "end_link_name": "tool0", + } + }, + } + + +def test_solver_cfg_from_dict_constructs_ur5_with_derived_dh_parameters(): + cfg = URSolverCfg.from_dict( + { + "class_type": "URSolver", + "ur_type": "ur5", + } + ) + + assert isinstance(cfg, URSolverCfg) + assert cfg.ur_type == "ur5" + assert_ur5_dh_parameters(cfg) + + +def test_solver_cfg_from_dict_runs_concrete_post_init_once(monkeypatch): + post_init_calls = 0 + original_post_init = URSolverCfg.__post_init__ + + def counted_post_init(cfg: URSolverCfg) -> None: + nonlocal post_init_calls + post_init_calls += 1 + original_post_init(cfg) + + monkeypatch.setattr(URSolverCfg, "__post_init__", counted_post_init) + + cfg = URSolverCfg.from_dict( + { + "class_type": "URSolver", + "ur_type": "ur5", + } + ) + + assert post_init_calls == 1 + assert_ur5_dh_parameters(cfg) + + +def test_robot_cfg_from_dict_constructs_nested_ur5_solver(): + cfg = RobotCfg.from_dict(make_ur5_robot_dict()) + + solver_cfg = cfg.solver_cfg["arm"] + assert isinstance(solver_cfg, URSolverCfg) + assert solver_cfg.root_link_name == "base" + assert solver_cfg.end_link_name == "tool0" + assert_ur5_dh_parameters(solver_cfg) + + +def test_robot_cfg_solver_to_dict_from_dict_roundtrip_preserves_derived_values(): + cfg = RobotCfg.from_dict(make_ur5_robot_dict()) + + restored_cfg = RobotCfg.from_dict(cfg.to_dict()) + + restored_solver_cfg = restored_cfg.solver_cfg["arm"] + assert isinstance(restored_solver_cfg, URSolverCfg) + assert restored_solver_cfg.ur_type == "ur5" + assert restored_solver_cfg.root_link_name == "base" + assert restored_solver_cfg.end_link_name == "tool0" + np.testing.assert_allclose(restored_solver_cfg.tcp, np.eye(4)) + assert_ur5_dh_parameters(restored_solver_cfg) + + +def test_solver_cfg_from_dict_applies_other_derived_config_logic(): + cfg = DifferentialSolverCfg.from_dict( + { + "class_type": "DifferentialSolver", + "ik_method": "dls", + } + ) + + assert isinstance(cfg, DifferentialSolverCfg) + assert cfg.ik_method == "dls" + assert cfg.ik_params == {"lambda_val": 0.01} + + +def test_solver_cfg_from_dict_preserves_unannotated_config_attributes(): + cfg = OPWSolverCfg.from_dict( + { + "class_type": "OPWSolver", + "a1": 1.25, + } + ) + + assert isinstance(cfg, OPWSolverCfg) + assert cfg.a1 == pytest.approx(1.25) + + +def test_solver_cfg_from_dict_ignores_unknown_fields(monkeypatch): + warnings = [] + monkeypatch.setattr( + "embodichain.lab.sim.solvers.base_solver.logger.log_warning", + warnings.append, + ) + + cfg = URSolverCfg.from_dict( + { + "class_type": "URSolver", + "ur_type": "ur5", + "unsupported_field": "ignored", + } + ) + + assert not hasattr(cfg, "unsupported_field") + assert warnings == ["Key 'unsupported_field' not found in URSolverCfg."]